Giancarlo R. / Mbed 2 deprecated Arm_Serial_Coordinates

Takes in serial coordinates (in mm) and moves servos in Lynxmotion AL5 arm accordingly.

Dependencies:   mbed Servo

robo.cpp@0:4a15e2d20446, 2011-12-14 (annotated)

Committer:
gvalentin3
Date:
Wed Dec 14 17:08:38 2011 +0000
Revision:
0:4a15e2d20446
First published version. Height is still hardcoded to -70mm.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gvalentin3 0:4a15e2d20446 1 //Robo class implementation for controlling robotic arm
gvalentin3 0:4a15e2d20446 2 // Servos: A 1ms pulse is 0 degrees, 1.5ms is 90 degrees and a 2 ms pulse is approximately 180 degrees.
gvalentin3 0:4a15e2d20446 3 // New timing pulses are sent to the servo every 20 ms.
gvalentin3 0:4a15e2d20446 4 // In theory .005556ms = 1 degree
gvalentin3 0:4a15e2d20446 5 // 5.56
gvalentin3 0:4a15e2d20446 6 // Currently .000010 = 1 increment
gvalentin3 0:4a15e2d20446 7
gvalentin3 0:4a15e2d20446 8 #include "robo.h"
gvalentin3 0:4a15e2d20446 9 #include "mbed.h"
gvalentin3 0:4a15e2d20446 10
gvalentin3 0:4a15e2d20446 11 #define PI 3.14159265
gvalentin3 0:4a15e2d20446 12
gvalentin3 0:4a15e2d20446 13 robo::robo(PinName s, PinName b, PinName e, PinName w, PinName c,PinName t){
gvalentin3 0:4a15e2d20446 14 spin = new PwmOut(s);
gvalentin3 0:4a15e2d20446 15 base = new PwmOut(b);
gvalentin3 0:4a15e2d20446 16 elbow = new PwmOut(e);
gvalentin3 0:4a15e2d20446 17 wrist = new PwmOut(w);
gvalentin3 0:4a15e2d20446 18 claw = new PwmOut(c);
gvalentin3 0:4a15e2d20446 19 trash = new PwmOut(t);
gvalentin3 0:4a15e2d20446 20 spin->period_ms(20);
gvalentin3 0:4a15e2d20446 21 base->period_ms(20);
gvalentin3 0:4a15e2d20446 22 elbow->period_ms(20);
gvalentin3 0:4a15e2d20446 23 wrist->period_ms(20);
gvalentin3 0:4a15e2d20446 24 claw->period_ms(20);
gvalentin3 0:4a15e2d20446 25 trash->period_ms(20);
gvalentin3 0:4a15e2d20446 26 }
gvalentin3 0:4a15e2d20446 27
gvalentin3 0:4a15e2d20446 28 void robo::write(float angle, int servo){
gvalentin3 0:4a15e2d20446 29 //float oneDeg = 5.555556;
gvalentin3 0:4a15e2d20446 30 float oneRad = 318.309886;
gvalentin3 0:4a15e2d20446 31 float spinOffset = 1150;
gvalentin3 0:4a15e2d20446 32 float baseOffset = 950;
gvalentin3 0:4a15e2d20446 33 float elbowOffset = 1250;
gvalentin3 0:4a15e2d20446 34 float wristOffset = 950;
gvalentin3 0:4a15e2d20446 35 float clawOffset = 1000;
gvalentin3 0:4a15e2d20446 36
gvalentin3 0:4a15e2d20446 37
gvalentin3 0:4a15e2d20446 38 switch(servo){
gvalentin3 0:4a15e2d20446 39 case(0):
gvalentin3 0:4a15e2d20446 40 spin->pulsewidth_us(angle*oneRad + spinOffset);
gvalentin3 0:4a15e2d20446 41 break;
gvalentin3 0:4a15e2d20446 42 case(1):
gvalentin3 0:4a15e2d20446 43 base->pulsewidth_us(angle*oneRad + baseOffset);
gvalentin3 0:4a15e2d20446 44 break;
gvalentin3 0:4a15e2d20446 45 case(2):
gvalentin3 0:4a15e2d20446 46 elbow->pulsewidth_us(angle*oneRad + elbowOffset);
gvalentin3 0:4a15e2d20446 47 break;
gvalentin3 0:4a15e2d20446 48 case(3):
gvalentin3 0:4a15e2d20446 49 wrist->pulsewidth_us(angle*oneRad + wristOffset);
gvalentin3 0:4a15e2d20446 50 break;
gvalentin3 0:4a15e2d20446 51 case(4):
gvalentin3 0:4a15e2d20446 52 claw->pulsewidth_us(angle*10 + clawOffset);
gvalentin3 0:4a15e2d20446 53 break;
gvalentin3 0:4a15e2d20446 54 case(5):
gvalentin3 0:4a15e2d20446 55 trash->pulsewidth_us(angle*oneRad + 1000);
gvalentin3 0:4a15e2d20446 56 break;
gvalentin3 0:4a15e2d20446 57 }
gvalentin3 0:4a15e2d20446 58
gvalentin3 0:4a15e2d20446 59 }
gvalentin3 0:4a15e2d20446 60
gvalentin3 0:4a15e2d20446 61 void robo::writeSpin(float angle){
gvalentin3 0:4a15e2d20446 62 write(angle,0);
gvalentin3 0:4a15e2d20446 63 }
gvalentin3 0:4a15e2d20446 64 void robo::writeBase(float angle){
gvalentin3 0:4a15e2d20446 65 write(angle,1);
gvalentin3 0:4a15e2d20446 66 }
gvalentin3 0:4a15e2d20446 67 void robo::writeElbow(float angle){
gvalentin3 0:4a15e2d20446 68 write(angle,2);
gvalentin3 0:4a15e2d20446 69 }
gvalentin3 0:4a15e2d20446 70 void robo::writeWrist(float angle){
gvalentin3 0:4a15e2d20446 71 write(angle,3);
gvalentin3 0:4a15e2d20446 72 }
gvalentin3 0:4a15e2d20446 73 void robo::writeClaw(float angle){
gvalentin3 0:4a15e2d20446 74 write(angle,4);
gvalentin3 0:4a15e2d20446 75 }
gvalentin3 0:4a15e2d20446 76 void robo::writeTrash(float angle){
gvalentin3 0:4a15e2d20446 77 write(angle,5);
gvalentin3 0:4a15e2d20446 78 }
gvalentin3 0:4a15e2d20446 79
gvalentin3 0:4a15e2d20446 80
gvalentin3 0:4a15e2d20446 81
gvalentin3 0:4a15e2d20446 82
gvalentin3 0:4a15e2d20446 83 float* robo::calculateangle(float x1e, float y1e, float oe, float angles[]){
gvalentin3 0:4a15e2d20446 84
gvalentin3 0:4a15e2d20446 85 //relative units
gvalentin3 0:4a15e2d20446 86 float a1 = 95;//mm's from length from base to elbow;
gvalentin3 0:4a15e2d20446 87 float a2 = 110;//mm's from length from elbow to wrist;
gvalentin3 0:4a15e2d20446 88 float a3 = 45;//mm's from length from wrist to claw;
gvalentin3 0:4a15e2d20446 89
gvalentin3 0:4a15e2d20446 90 float ce = cos(oe);
gvalentin3 0:4a15e2d20446 91 float se = sin(oe);
gvalentin3 0:4a15e2d20446 92
gvalentin3 0:4a15e2d20446 93 float o2 = -acos( ((x1e-a3*ce)*(x1e-a3*ce) + (y1e-a3*se)*(y1e-a3*se) - a1*a1 - a2*a2) / (2*a1*a2) );
gvalentin3 0:4a15e2d20446 94
gvalentin3 0:4a15e2d20446 95 float s2 = sin(o2);
gvalentin3 0:4a15e2d20446 96 float c2 = cos(o2);
gvalentin3 0:4a15e2d20446 97
gvalentin3 0:4a15e2d20446 98 float delta = a1*a1 + a2*a2 + 2*a1*a2*c2;
gvalentin3 0:4a15e2d20446 99 float det1 = (x1e-a3*ce)*(a1+a2*c2) - (y1e-a3*se)*(-a2*s2);
gvalentin3 0:4a15e2d20446 100
gvalentin3 0:4a15e2d20446 101 float o1 = acos(det1/delta);
gvalentin3 0:4a15e2d20446 102 float o3 = oe-o1-o2;
gvalentin3 0:4a15e2d20446 103
gvalentin3 0:4a15e2d20446 104 angles[0]= o1;
gvalentin3 0:4a15e2d20446 105 angles[1]= -o2; //Had to use negative sign to reconcile with arm configuation.
gvalentin3 0:4a15e2d20446 106 angles[2]= -o3; //Had to use negative sign to reconcile with arm configuation.
gvalentin3 0:4a15e2d20446 107
gvalentin3 0:4a15e2d20446 108 return angles;
gvalentin3 0:4a15e2d20446 109
gvalentin3 0:4a15e2d20446 110
gvalentin3 0:4a15e2d20446 111 }
gvalentin3 0:4a15e2d20446 112
gvalentin3 0:4a15e2d20446 113