Hi everyone, we are working on robot arm kit. This is the 1st version of it. Please post everything you think about it. The BOM list and 3D model are coming soon.
The motion range, XZ Plane
XY Plane
Here is the program (In this case, the MPU6050 is used to correct the angle sensor at the first time, you can ignore that)
#include <Makeblock.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#define A_RATIO 0.14516 // 18:62/2
#define B_RATIO 0.1125 // 18/80/2
#define DOCK_X 762
#define DOCK_Y 253
#define V_A 769
#define V_B 646
MeEncoderMotor a(0x2, 0x0); //Motor at Slot1
MeEncoderMotor b(0x2, 0x1); //Motor at Slot1
MeEncoderMotor c(0x3, 0x0); //Motor at Slot1
MeDCMotor grab(M2);
MePort crane(PORT_3);
// the control
MePort p0(PORT_6);
MePort p1(PORT_7);
MePort btn(PORT_8);
// the poly matrix of murata's potential meter
#define A_0 607.47
#define A_1 3.063
int follow = 0;
int init_c, init_x, init_y;
float arm_x,arm_y,last_x,last_y;
float ctrl_a, ctrl_b, ctrl_c, last_a, last_b, last_c;
float tmp;
void setup() {
a.begin();
b.begin();
c.begin();
Serial.begin(9600);
MPU6050_init();
delay(300);
backToDock();
a.begin();
b.begin();
c.begin();
init_c = p1.Aread2();
// x y use potential value instead
init_x = DOCK_X;//crane.Aread2();
init_y = DOCK_Y;//crane.Aread1();
}
void backToDock()
{
int x = crane.Aread2();
float angx = (DOCK_X-x)/A_1;
moveA(angx);
int y = crane.Aread1();
float angy = (DOCK_Y-y)/A_1;
moveB(angy);
delay(1000);
}
char buff[64];
char buffIndex = 0;
void loop() {
while (Serial.available()) {
char c = Serial.read();
Serial.print(c);
if (c == '\n') {
parseCommand(buff);
memset(buff, 0, 64);
buffIndex = 0;
} else {
buff[buffIndex++] = c;
}
}
arm_x = (crane.Aread2() - init_x) / A_1;
arm_y = (crane.Aread1() - init_y) / A_1;
ctrl_a = -(p0.Aread2()-V_A) / 2.1682f; // different??
ctrl_b = (p0.Aread1()-V_B) / 2.1682f;
ctrl_b = ctrl_b-ctrl_a; // change to absolute angle
ctrl_c = (p1.Aread2() - init_c) / 3.5700f; // poly fit for potential base
//Serial.print(crane.Aread1());Serial.print(",");Serial.println(init_y);
//Serial.print(crane.Aread2());Serial.print(",");Serial.println(init_x);
//Serial.print(p1.Aread2());Serial.print(",");Serial.println(init_c);
delay(100);
int btna = btn.Aread2();
//Serial.println(btna);
if (btna < 50) {
} else if (btna > 450 && btna < 500) {
grab.run(254);
} else if (btna > 640 && btna < 680) { // follow crane
if (abs(ctrl_a - last_a) > 3) {
moveA(ctrl_a);
last_a = ctrl_a;
}
if (abs(ctrl_b - last_b) > 3) {
moveB(ctrl_b);
last_b = ctrl_b;
}
if (abs(ctrl_c - last_c) > 3) {
//moveC(ctrl_c/2); // ratio to 1:2
moveC(ctrl_c);
last_c = ctrl_c;
}
} else if (btna > 710 && btna < 735) {
grab.run(-254);
} else {
follow = 0;
grab.stop();
}
}
#define M_PI 3.14159265359
void parseCommand(char * cmd)
{
if (cmd[0] == 'r') {
float posa = a.getCurrentPosition();
float posb = b.getCurrentPosition();
float posc = c.getCurrentPosition();
Serial.print(posa); Serial.print(","); Serial.print(posb); Serial.print(","); Serial.println(posc);
}else if (cmd[0] == 'a') {
Serial.print(getA());Serial.print(",");Serial.println(ctrl_a);
}else if (cmd[0] == 'b') {
Serial.print(getB());Serial.print(",");Serial.println(ctrl_b);
}else if(cmd[0]=='c'){
Serial.print(ctrl_c);Serial.print(",");Serial.println(init_c);
}else if(cmd[0]=='i'){
Serial.printf("x=%d,y=%d\n",init_x,init_y);
}else {
int posa, posb, posc;
sscanf(cmd, "%d %d %d", &posa, &posb, &posc);
Serial.printf("move %d,%d,%d\n", posa, posb, posc);
if (posa > 90 || posa < -90 || posb > 90 || posb < -90 || posc > 180 || posc< -180) {
Serial.printf("error value\n");
} else {
moveA(posa);
moveB(posb);
moveC(posc);
}
}
}
void moveA(int ang)
{
float targetAng = ((float)ang);
float value = targetAng / A_RATIO;
Serial.print("move a="); Serial.print(targetAng); Serial.print(" value="); Serial.println(value);
a.moveTo(value, 0.3f);
while(abs(a.getCurrentPosition()-value)>10){
Serial.println(a.getCurrentPosition()-value);
}
Serial.println("done a");
}
void moveB(int ang)
{
float targetAng = ((float)ang );
float value = targetAng / A_RATIO;
Serial.print("move b="); Serial.print(targetAng); Serial.print(" value="); Serial.println(value);
b.moveTo(value, 0.3f);
while(abs(b.getCurrentPosition()-value)>10){
Serial.println(b.getCurrentPosition()-value);
}
Serial.println("done b");
}
void moveC(int ang)
{
float targetAng = (float)ang;
float value = targetAng / B_RATIO;
Serial.print("move c="); Serial.print(targetAng); Serial.print(" value="); Serial.println(value);
c.moveTo(value, 0.3f);
while(abs(c.getCurrentPosition()-value)>10){
Serial.println(c.getCurrentPosition()-value);
}
Serial.println("done c");
}
float getA()
{
int ax = acc_x();
int ay = acc_y();
float ang = atan2(ay, ax) / M_PI * 180;
return ang;
}
float getB()
{
int ax = acc_x();
int ay = acc_y();
float ang = atan2(ax, ay) / M_PI * 180;
return ang;
}
void systemReset(){
a.begin();
b.begin();
MPU6050_init();
delay(1000);
buzzerOn();delay(300);buzzerOff();delay(100);
buzzerOn();delay(100);buzzerOff();
buzzerOn();delay(300);buzzerOff();delay(100);
}
- The control panel, Me Baseboard V1.0
- The strong robot gripper
New parts, we have made some improvement after I finished the video. More holes on the plates.
