Author Topic: Need help testing inverse kinematics code  (Read 1361 times)

0 Members and 1 Guest are viewing this topic.

Offline tomcharleyTopic starter

  • Jr. Member
  • **
  • Posts: 19
  • Helpful? 0
Need help testing inverse kinematics code
« on: March 21, 2012, 10:20:26 AM »
Dear Forum Users,

    I am working on a project that requires the use of a robotic arm to push buttons.  The code that I have written is based on the Robot Arm Designer spreadsheet calculations that I found on this website.  I have written it for use by an Arduino Uno microcontroller, which would actuate two servos (shoulder and elbow joints), mounted on a two linkage arm.  The linkages are each 10 inches in the code, but this could be easily changed for a different arm length.
    For the moment, the code is supposed to actuate based on commands from a computer in a terminal screen (pressing a number from 1 to 7) to have the arm push horizontally towards a button.  The goal here is to make sure that the tip of the arm moves in a straight line.  The robot that the arm will be mounted on will not be able to localize itself precisely enough to know how far away the button is, but the vertical position of the arm is certain because it is dictated by the height of the shoulder mount.  Hence the need for the tip of the arm to move horizontally.
    Now that I have my code written, I would like to see if it works.  However, I have no servos to test it out.  I would like to know if the code works before any servos I order get here so that I can perfect the code in the meantime.  If anyone has the parts and the time to test out my code I would appreciate it.
    Since I am a Mechanical Engineer and don't have a ton of experience coding, this might be a bit messy.  Feel free to correct my code any way you see fit.  I welcome constructive criticism.

Thanks!
Tom

tldr: I need help testing some Arduino code for a two-servo arm.  It needs to move the tip of the arm in a straight line.

Code: [Select]
/* Arm Controller Code
 *
 * Adapted from the "Robot Arm Designer" spreadsheet design
 * Produced by [url=http://www.societyofrobots.com]www.societyofrobots.com[/url]
 *
 * Customized for use at Carnegie Mellon University
 * Tom Charley ... 2012
 */

/** Set up variables for equations **/
double upperarm = 10;            // length of first arm segment
double forearm  = 10;            // length of second arm segment

double x;                        // horizontal position of arm tip
double y;                        // vertical position of arm tip
double c;                        // not sure yet what this stands for
double s;                        // not sure yet what this stands for
double k1;                       // upper arm plus parallel portion of forearm (I think...)
double k2;                       // portion of forearm (I think...)
double theta;                    // angle between vertical and upper arm (rad)
double psi;                      // angle between upper arm and forearm (rad)
double angle1;                   // theta in degrees
double angle2;                   // psi in degrees
double angleshoulder;            // angle of shoulder servo converted to pulse width
double angleelbow;               // angle of elbow servo converted to pulse width
double pi = 3.141592653589;      // pi... yum

/** Set up microcontroller interface **/
int shoulder = 2;                // shoulder servo
int elbow    = 4;                // elbow servo

int refreshTime  =  20;          // time (ms) between pulses (50Hz) 

int moveServo;                   // raw user input 
long lastPulse   = 0;            // recorded time (ms) of the last pulse

void setup()
{
  pinMode(shoulder, OUTPUT);     // set microcontroller pins to output
  pinMode(elbow, OUTPUT);        // set microcontroller pins to output
  angleshoulder = 400;           // give the servo a starting point (or it floats)
  angleelbow = 400;              // give the servo a starting point (or it floats)
  Serial.begin(9600);            // Tell it what to do in the Terminal screen
  Serial.println("          Arduino Serial Servo Control"); 
  Serial.println("             Press 0 to retract arm");
  Serial.println("Press 1 through 7 for different button positions"); 
  Serial.println(); 
 
}
   
  /* This is the calculation that needs to be done for each position calculation:
  *
  * c = (square(x) + square(y) - square(upperarm) - square(forearm))/(2 * upperarm * forearm);
  * s = sqrt(1 - square(c));
  * k1 = upperarm + (forearm * c);
  * k2 = forearm * s;
  * theta = atan2(x,y) - atan2(k1,k2);
  * angle1 = (180 * theta) / pi;
  * angle2 = (180 * psi) / pi;
  */
void calculation()
{
  c = (square(x) + square(y) - square(upperarm) - square(forearm))/(2 * upperarm * forearm);
  s = sqrt(1 - square(c));
  k1 = upperarm + (forearm * c);
  k2 = forearm * s;
  theta = atan2(x,y) - atan2(k1,k2);
  angle1 = (180 * theta) / pi;
  angle2 = (180 * psi) / pi;
  angleshoulder = map(angle1, 0, 180, 400, 2200);
  angleelbow = map(angle2, 0, 180, 400, 2200);
}

void actuate()

  // pulse the servo every refreshTime with current pulseWidth 
  // this will hold the servo's position if unchanged, or move it if changed 
  if (millis() - lastPulse >= refreshTime) { 
    digitalWrite(shoulder, HIGH);       // start the pulse 
    delayMicroseconds(angleshoulder);   // pulse width 
    digitalWrite(shoulder, LOW);        // stop the pulse 
    digitalWrite(elbow, HIGH);          // start the pulse 
    delayMicroseconds(angleelbow);      // pulse width 
    digitalWrite(elbow, LOW);           // stop the pulse
    lastPulse = millis();               // save the time of the last pulse 
  } 
}

void loop()
{
  // wait for serial input
  if (Serial.available() > 0)
  {
    // read the incoming byte: 
    moveServo = Serial.read(); 
    // In ASCII, '0' is 48, and the numbers go up sequentially to '7' at 55
    if (moveServo == 48)
    {
      x = 0;
      y = 10;
      calculation;
      actuate;
      actuate;
      actuate;
    } 
    if (moveServo == 49)
    {
      x = 5;
      y = 0;
      calculation;
      actuate;
      actuate;
      actuate;
      x = 6;
      y = 0;
      calculation;
      actuate;
      actuate;
      x = 7;
      y = 0;
      calculation;
      actuate;
      actuate;
      x = 8;
      y = 0;
      calculation;
      actuate;
      actuate;
    } 
    if (moveServo == 50) {
      x = 5;
      y = 1;
      calculation;
      actuate;
      actuate;
      actuate;
      x = 6;
      y = 1;
      calculation;
      actuate;
      actuate;
      x = 7;
      y = 1;
      calculation;
      actuate;
      actuate;
      x = 8;
      y = 1;
      calculation;
      actuate;
      actuate;
    } 
    if (moveServo == 51) {
      x = 5;
      y = 2;
      calculation;
      actuate;
      actuate;
      actuate;
      x = 6;
      y = 2;
      calculation;
      actuate;
      actuate;
      x = 7;
      y = 2;
      calculation;
      actuate;
      actuate;
      x = 8;
      y = 2;
      calculation;
      actuate;
      actuate;
    } 
    if (moveServo == 52) {
      x = 5;
      y = 3;
      calculation;
      actuate;
      actuate;
      actuate;
      x = 6;
      y = 3;
      calculation;
      actuate;
      actuate;
      x = 7;
      y = 3;
      calculation;
      actuate;
      actuate;
      x = 8;
      y = 3;
      calculation;
      actuate;
      actuate;
    } 
    if (moveServo == 53) {
      x = 5;
      y = 4;
      calculation;
      actuate;
      actuate;
      actuate;
      x = 6;
      y = 4;
      calculation;
      actuate;
      actuate;
      x = 7;
      y = 4;
      calculation;
      actuate;
      actuate;
      x = 8;
      y = 4;
      calculation;
      actuate;
      actuate;
    } 
    if (moveServo == 54) {
      x = 5;
      y = 5;
      calculation;
      actuate;
      actuate;
      actuate;
      x = 6;
      y = 5;
      calculation;
      actuate;
      actuate;
      x = 7;
      y = 5;
      calculation;
      actuate;
      actuate;
      x = 8;
      y = 5;
      calculation;
      actuate;
      actuate;
    }
    if (moveServo == 55) {
      x = 5;
      y = 6;
      calculation;
      actuate;
      actuate;
      actuate;
      x = 6;
      y = 6;
      calculation;
      actuate;
      actuate;
      x = 7;
      y = 6;
      calculation;
      actuate;
      actuate;
      x = 8;
      y = 6;
      calculation;
      actuate;
      actuate;
    }
   
    // print pulseWidth back to the Serial Monitor
     Serial.print("Shoulder Pulse Width: "); 
     Serial.print(angleshoulder); 
     Serial.print("us  ");         // microseconds 
     Serial.print("Shoulder Angle: "); 
     Serial.print(angle1); 
     Serial.println("deg");      // degrees 
     Serial.print("Elbow Pulse Width: "); 
     Serial.print(angleelbow); 
     Serial.print("us  ");         // microseconds 
     Serial.print("Elbow Angle: "); 
     Serial.print(angle2); 
     Serial.println("deg");      // degrees   
  } 
}

 


Get Your Ad Here