Pages: [1]   Go Down
Author Topic: Inverse Kinematics nightmare  (Read 696 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 19
ART ALWAYS
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

I have been working on an articulating plotting robot arm
video here:

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!


Code:
//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);
 }






Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 19
ART ALWAYS
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
//arduino side


#include <AFMotor.h> //Adafruit Motor Shield
AF_Stepper motor1(400, 1);
AF_Stepper motor2(400, 2);

#include <Servo.h>  //Servo Controller

Servo myservo;  // create servo object to control a servo

boolean motors = true;

//functions for IK
int sx,sy,ex,ey,hx,hy;
int ua,la;
float uad, lad;
int UpperArmLength = 100;
int LowerArmLength = UpperArmLength;
//IK

//Current and Go locations

float cx = 400;
float cy = 400;
float gx = cx;
float gy = cy;
//Current and Go


//stuff for serial parsing
int incomingByte;
int digit;
int xval;
int yval;
int dig = 0;
byte PenOn = '-';
//serial


//starting angles of arms
int a1 = 2000; // starting degrees
int b1 = 4000;

//starting point
int x1 = cx;
int y1 = cy;

void setup() {

   Serial.begin(9600);
 Serial.println("DrawingMachine II says hello.");

sx = 300; // middle point, sholder position.
sy = 300;

  delay(2000);
 motor1.setSpeed(30);    
 motor2.setSpeed(30);                                                                                                                                                          
  delay(2000);        


}

void loop(){
 
if (Serial.available() > 0) {

  dig = dig +1;
// read the incoming byte:
incomingByte = Serial.read();

//This is how the single bytes are recieved seperately and recombined.




if (incomingByte == ']'){
    dig = 0;
UpperArmLength = UpperArmLength+=1;
LowerArmLength = LowerArmLength+=1;
}

if (incomingByte == '['){
    dig = 0;
UpperArmLength = UpperArmLength-=1;
LowerArmLength = LowerArmLength-=1;
}


if (incomingByte == '.'){
    dig = 0;
motor1.step(50, BACKWARD, INTERLEAVE);
}

if (incomingByte == ','){
    dig = 0;
  motor1.step(50, FORWARD, INTERLEAVE);

}


if (incomingByte == '>'){
    dig = 0;
motor2.step(50, BACKWARD, INTERLEAVE);
}

if (incomingByte == '<'){
    dig = 0;
  motor2.step(50, FORWARD, INTERLEAVE);

}


  
if(PenOn != incomingByte){
 


if (incomingByte == '+'){
  
delay(1000);
  myservo.write(50);              // tell servo to go to position in variable 'pos'
 delay(1000);
       dig = 0;
      
       motor1.setSpeed(6); //moove slowly for active points
       motor2.setSpeed(6);
PenOn = incomingByte;      
}

if (incomingByte == '-'){
  
 delay(1000);
   myservo.write(80);              // tell servo to go to position in variable 'pos'
    
     dig = 0;
  delay(500);
    
    motor1.setSpeed(30);  // move quickly for non points
    motor2.setSpeed(30);
PenOn = incomingByte;
}




} // if penStatus changed

// asterisk inbetween coords resets


if (incomingByte == '*'){
   xval = 0;
  yval = 0;
  
  dig = 0;
}

  digit = int(incomingByte) - '0';  // - 0, or ascii value -30 from the incoming digit.  // this is confusing,
  //if you take the ascii value of digits 0-9 it equels 30 - 39, if you take the value of the ascii value and - "0"
  //you get the orignal digits. an easy way of converting ascii serial messages
  if(dig == 1){  
 xval += digit * 10000;
}
if(dig == 2){
 xval += digit * 1000;
 }
if(dig == 3){
  xval +=  digit * 100;}
if(dig == 4){
  xval += digit *10;
}
  if(dig == 5){
   xval += digit;
}

if(dig == 6){
 yval += digit * 10000;
 }

if(dig == 7){
  yval +=  digit * 1000;}
 
  if(dig == 8){
  yval += digit * 100;
  }
  if(dig == 9){
  yval += digit * 10;
  }

  if(dig == 10){
  yval += digit;

Serial.print(xval, DEC);
Serial.print(',');
Serial.println(yval, DEC);
//straightLine (xval, yval);
upperArm(xval,yval);
  }
 }
  
  
}


void print(char* str, float n1, float n2) {


  Serial.print(str);
  Serial.print(n1);
  Serial.print(", ");
  Serial.print(n2);
  Serial.println();
  
}


void upperArm(int gox,int goy){
 Serial.println("------from Machine------");
   print("coords = ", gox, 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);

 print("lengths", 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));
print("C = ", C , 0);



 print("B , C = ", B, C);



float D = atan2(dy,dx);
float E = D + B + PI + C;
print("D E =  ", D, E);

ex = int((cos(E) * a)) + sx;
ey = int((sin(E) * a)) + sy;
float degreesA = degrees(E)-360;

print("UpperArm Angle= ", degreesA, 0);

hx = int((cos(D+B) * b)) + ex;
hy = int((sin(D+B) * b)) + ey;
float degreesB = degrees((D+B));

print("LowerArm Angle= ", degreesB,0);

degreesB = degreesA - degreesB;

print("LowerArm Angle= ", degreesB,0);

DegToStp(degreesA, degreesB);
Serial.println("----------------------------");
  
  
  
}


void DegToStp(int degA, int degB){
 
 
int stepsA = map(abs(degA), 0, 360, 0, 8000);

int stepsB = map(abs(degB), 0, 360, 0, 8000);  
  
  print("steps go to= ", stepsA,stepsB);
  print("current step location = ", a1, b1 );  
     Serial.println("--------- ");
//  moveTo(stepsA, stepsB); //comment this out if you have no motors connects, just check the math
}


void moveTo(int angle2in, int angle1in) {

  
  // a2 and b2 are the final lengths of the left and right strings
  int a2 = angle1in;
  int b2 = angle2in;
  print("steps to take= ", a1-a2,b1-b2);
  int stepA;
  int stepB;
  if (a2>a1) {
    stepA=1;
  }
  if (a1>a2) {
    stepA=-1;
  }
  if (a2==a1) {
    stepA=0;
  }
  if (b2>b1) {
    stepB=1;
  }
  if (b1>b2) {
    stepB=-1;
  }
  if (b2==b1) {
    stepB=0;
  }

  // Change the angle of a1 and b1 until they are equal to the desired angle



  while ((a1!=a2) || (b1!=b2)) {
    if (a1!=a2) {
      a1 += stepA;
     if (stepA == -1){
        if(motors == true){
    motor1.step(1, BACKWARD, INTERLEAVE);
        }
  }
  
    if (stepA == 1){
      if(motors == true){
       motor1.step(1, FORWARD, INTERLEAVE);
      }
    }
    }

      //delay(1);
    
    
    if (b1!=b2) {
      if(motors == true){
      b1 += stepB;
      }
      
    if (stepB == -1){
      if(motors == true){
    motor2.step(1, BACKWARD, INTERLEAVE);
      }
    }
    
     if (stepB == 1){
        if(motors == true){
       motor2.step(1, FORWARD, INTERLEAVE);
     }
     }

    // delay(1);
    
    }
  
  }// location not equel to current
}




« Last Edit: April 23, 2011, 02:50:11 pm by harveymoon » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 19
ART ALWAYS
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Here is some data straight from the processing print window.


You can see, Processing first sends a coordinate at the mouse location of 469,366
this correctly sees that the angles should be 3.55 and 49.77
the arduino spits out 21° and 0 °

moving the point a little bit, to 460, 365 means that two suddenly link up again. Both processing and arduino give the same degree angle.
UpperArm Angle= -8.18, 0.00
LowerArm Angle= -60.58, 0.00



Print window contents below:


Processing Side ____________
mouse= 469, 366
Pros UpperArm Angle= -3.553009
Pros LowerArm Angle= -49.770573
lengths : 100 100
Processing Side ____________
469,366
------from Machine------
coords = 469.00, 366.00
lengths100.00, 100.00
C = 3.14, 0.00
B , C = 0.00, 3.14
D E =  0.37, 6.66
UpperArm Angle= 21.33, 0.00
LowerArm Angle= 0.00, 0.00
steps go to= 466.00, 0.00
current step location = 2000.00, 4000.00
---------
----------------------------

Processing Side ____________
mouse= 460, 365
Pros UpperArm Angle= -8.179199
Pros LowerArm Angle= -60.577255
lengths : 100 100
Processing Side ____________
460,365
------from Machine------
coords = 460.00, 365.00
lengths100.00, 100.00
C = 2.08, 0.00
B , C = 0.53, 2.08
D E =  0.39, 6.14
UpperArm Angle= -8.18, 0.00
LowerArm Angle= -60.58, 0.00
steps go to= 177.00, 1333.00
current step location = 2000.00, 4000.00
---------
Logged

0
Offline Offline
Shannon Member
****
Karma: 206
Posts: 12175
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

int is 16 bit signed on Arduino.   I suspect Processing has 32 bit int?
Logged

[ I won't respond to messages, use the forum please ]

Offline Offline
Newbie
*
Karma: 0
Posts: 19
ART ALWAYS
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

WOOP! I think that did it!

 
changed these from int to long.
  long dx = gox - sx;
long dy = goy - sy;



thanks big time!
Logged

Louisville, CO
Offline Offline
Full Member
***
Karma: 2
Posts: 174
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Here's working IK code for Arduino -> http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino
Logged

/felis

Offline Offline
Newbie
*
Karma: 0
Posts: 19
ART ALWAYS
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Felis:
I was looking at that one too. but because there was a Z azix and more joints. it seemed more complicated.

I got it figured out anyway, thanks.
Logged

Pages: [1]   Go Up
Jump to: