I have been working on an articulating plotting robot arm
video here: The Drawing Machine II prototype on Vimeo
Essentially I would like to get the Inverse kinematics figured out on the arduino, so that i can just send over a XY coord and it will move the angles of each joint to get to that position.
Things were working fine until I started playing with the arm lengths, then the data would start acting erratic.
I have essentially the same mathematics running on a processing sketch and the arduino. The processing sketch will visualize where it expects it to be, when you click, it sends over a XY coords to the arduino where it should do the same math with the coords and spit back what it thinks the two angles should be.
In the processing print window, you can see the correct values on the processing side, and the sometimes correct values from the machine side.
At some places, it matches completely with the processing sketch, sometimes though it will go haywire and not do what I want at all.
here is some code, Please HELP!
//processing side sketch
int sx,sy,ex,ey,hx,hy;
int ua,la;
int UpperArmLength = 100;
int LowerArmLength = UpperArmLength;
import processing.serial.*; // serial communication
float degreesA;
float degreesB;
boolean serial = true;
Serial myPort; //declare serial port
float uad, lad;
void setup(){
size(600,600);
background(255, 224, 150);
sx = 300;
sy = 300;
println(Serial.list());
String portName = Serial.list()[0]; // Change this Value to fit which Serial Port it is on.
myPort = new Serial(this, portName, 9600);
}
int counter = 0;
void draw(){
fill(255);
rect(0,0,width,height);
upperArm(mouseX,mouseY);
if (counter == 0){
println("start Bot : ");
}
while (myPort.available() > 0) {
char inByte = myPort.readChar();
print(inByte);
counter = 0;
}
if (counter == 2){
println();
// println("----end Bot-----");
}
counter++;
}
void upperArm(int gox, int goy){
int dx = gox - sx;
int dy = goy - sy;
float distance = sqrt(dx*dx+dy*dy);
int a = UpperArmLength;
int b = LowerArmLength;
float c = min(distance, a + b);
float B = acos((b*b-a*a-c*c)/(-2*a*c));
float C = acos((c*c-a*a-b*b)/(-2*a*b));
float D = atan2(dy,dx);
float E = D + B + PI + C;
ex = int((cos(E) * a)) + sx;
ey = int((sin(E) * a)) + sy;
degreesA = degrees(E)-360;
hx = int((cos(D+B) * b)) + ex;
hy = int((sin(D+B) * b)) + ey;
degreesB = degrees((D+B));
degreesB = degreesA - degreesB;
stroke(255,0,0,100);
fill(240,0,0,200);
ellipse(sx,sy,10,10);
ellipse(ex,ey,8,8);
ellipse(hx,hy,6,6);
stroke(0);
line(sx,sy,ex,ey);
line(ex,ey,hx,hy);
//delay(15000);
}
void mousePressed(){
println("Processing Side ____________");
println("mouse= " +mouseX +", " + mouseY);
println("Pros UpperArm Angle= "+ degreesA +" ");
println("Pros LowerArm Angle= "+ degreesB);
println("lengths : " + UpperArmLength + " " + LowerArmLength);
println("Processing Side ____________");
if(serial == true){
myPort.write("*");
int xtoGo = mouseX * 100000;
String xy = nf(xtoGo + mouseY, 10);
delay(250);
myPort.write(xy);
}
}
void keyPressed() {
if((key == ',')||(key == '.')||(key == '<')||(key == '>')||(key == '-')||(key == '+')||(key == '[')||(key == ']')){
DirectCommand(str(key));
if(key == ']'){
UpperArmLength = UpperArmLength+=1;
LowerArmLength = LowerArmLength+=1;
}
if(key == '['){
UpperArmLength = UpperArmLength-=1;
LowerArmLength = LowerArmLength-=1;
}
} // matches a correct command
}
void DirectCommand(String j){
myPort.write(j);
}