Craig Evans / MeArm

Library for controlling #MeArm from phenoptix (http://www.phenoptix.com/collections/mearm). Solves inverse kinematics of the arm to position gripper in 3D space.

Dependents:   wizArm_WIZwiki-W7500ECO wizArm_WIZwiki-W7500ECO MeArm_Lab2_Task1 MeArm_Lab3_Task1 ... more

MeArm.cpp@0:49b8e971fa83, 2014-11-04 (annotated)

Committer:
eencae
Date:
Tue Nov 04 20:54:49 2014 +0000
Revision:
0:49b8e971fa83
Child:
1:d2b7780ca6b3
Initial commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eencae 0:49b8e971fa83 1 /*
eencae 0:49b8e971fa83 2 @file MeArm.cpp
eencae 0:49b8e971fa83 3
eencae 0:49b8e971fa83 4 @brief Member functions implementations
eencae 0:49b8e971fa83 5
eencae 0:49b8e971fa83 6 */
eencae 0:49b8e971fa83 7 #include "mbed.h"
eencae 0:49b8e971fa83 8 #include "MeArm.h"
eencae 0:49b8e971fa83 9
eencae 0:49b8e971fa83 10 MeArm::MeArm(PinName basePin, PinName shoulderPin, PinName elbowPin, PinName gripperPin)
eencae 0:49b8e971fa83 11 {
eencae 0:49b8e971fa83 12 // set up pins as required
eencae 0:49b8e971fa83 13 base = new PwmOut(basePin);
eencae 0:49b8e971fa83 14 shoulder = new PwmOut(shoulderPin);
eencae 0:49b8e971fa83 15 elbow = new PwmOut(elbowPin);
eencae 0:49b8e971fa83 16 gripper = new PwmOut(gripperPin);
eencae 0:49b8e971fa83 17 // PWM objects have 50 Hz frequency by default so shouldn't need to set servo frequency
eencae 0:49b8e971fa83 18 // can do it anyway to be sure. All channels share same period so just need to set one of them
eencae 0:49b8e971fa83 19 base->period(1.0/50.0); // servos are typically 50 Hz
eencae 0:49b8e971fa83 20 // setup USB serial for debug/comms
eencae 0:49b8e971fa83 21 serial = new Serial(USBTX,USBRX);
eencae 0:49b8e971fa83 22 // leds used for debug
eencae 0:49b8e971fa83 23 leds = new BusOut(LED4,LED3,LED2,LED1);
eencae 0:49b8e971fa83 24 }
eencae 0:49b8e971fa83 25
eencae 0:49b8e971fa83 26 void MeArm::setTarget(float x,float y,float z)
eencae 0:49b8e971fa83 27 {
eencae 0:49b8e971fa83 28 target.x = x;
eencae 0:49b8e971fa83 29 target.y = y;
eencae 0:49b8e971fa83 30 target.z = z;
eencae 0:49b8e971fa83 31
eencae 0:49b8e971fa83 32 // TODO - add checks for valid target
eencae 0:49b8e971fa83 33 }
eencae 0:49b8e971fa83 34
eencae 0:49b8e971fa83 35 /// TODO - add checks for valid angles
eencae 0:49b8e971fa83 36 void MeArm::moveBaseServo(float angle)
eencae 0:49b8e971fa83 37 {
eencae 0:49b8e971fa83 38 // arm facing forward is defined as 90 degrees, angle increases counter-clockwise
eencae 0:49b8e971fa83 39 int pulseWidthMicroSeconds = 575.0 + 985.0*angle/90.0;
eencae 0:49b8e971fa83 40 serial->printf("Base Pulse = %d us\n",pulseWidthMicroSeconds);
eencae 0:49b8e971fa83 41 // equation to convert angle to pulsewidth in us
eencae 0:49b8e971fa83 42 base->pulsewidth_us(pulseWidthMicroSeconds);
eencae 0:49b8e971fa83 43 // move servo
eencae 0:49b8e971fa83 44 }
eencae 0:49b8e971fa83 45
eencae 0:49b8e971fa83 46 void MeArm::moveShoulderServo(float angle)
eencae 0:49b8e971fa83 47 {
eencae 0:49b8e971fa83 48 // angle relative to horizontal
eencae 0:49b8e971fa83 49 int pulseWidthMicroSeconds = 2705.0 - 1097.0*angle/90.0;
eencae 0:49b8e971fa83 50 serial->printf("Shoulder Pulse = %d us\n",pulseWidthMicroSeconds);
eencae 0:49b8e971fa83 51 // equation to convert angle to pulsewidth in us
eencae 0:49b8e971fa83 52 shoulder->pulsewidth_us(pulseWidthMicroSeconds);
eencae 0:49b8e971fa83 53 // move servo
eencae 0:49b8e971fa83 54 }
eencae 0:49b8e971fa83 55
eencae 0:49b8e971fa83 56 void MeArm::moveElbowServo(float angle)
eencae 0:49b8e971fa83 57 {
eencae 0:49b8e971fa83 58 // angle relative to horizontal
eencae 0:49b8e971fa83 59 int pulseWidthMicroSeconds = 1910.0 - 1095.0*angle/90.0;
eencae 0:49b8e971fa83 60 serial->printf("Elbow Pulse = %d us\n",pulseWidthMicroSeconds);
eencae 0:49b8e971fa83 61 // equation to convert angle to pulsewidth in us
eencae 0:49b8e971fa83 62 elbow->pulsewidth_us(pulseWidthMicroSeconds);
eencae 0:49b8e971fa83 63 // move servo
eencae 0:49b8e971fa83 64 }
eencae 0:49b8e971fa83 65
eencae 0:49b8e971fa83 66 void MeArm::moveServos()
eencae 0:49b8e971fa83 67 {
eencae 0:49b8e971fa83 68 moveBaseServo(thetaBase);
eencae 0:49b8e971fa83 69 moveShoulderServo(thetaShoulder);
eencae 0:49b8e971fa83 70 moveElbowServo(thetaElbow);
eencae 0:49b8e971fa83 71 }
eencae 0:49b8e971fa83 72
eencae 0:49b8e971fa83 73 // calculate angle in DEGREES
eencae 0:49b8e971fa83 74 float MeArm::cosineRule(float adj1, float adj2, float opp)
eencae 0:49b8e971fa83 75 {
eencae 0:49b8e971fa83 76 // cosine rule
eencae 0:49b8e971fa83 77 float tmp = (adj1*adj1 + adj2*adj2 - opp*opp) / (2*adj1*adj2);
eencae 0:49b8e971fa83 78 //println("tmp = " + tmp);
eencae 0:49b8e971fa83 79
eencae 0:49b8e971fa83 80 // check for valid angle
eencae 0:49b8e971fa83 81 if ( fabs(tmp) <= 1.0 ) {
eencae 0:49b8e971fa83 82 return RAD2DEG*acos(tmp);
eencae 0:49b8e971fa83 83 } else {
eencae 0:49b8e971fa83 84 error(1);
eencae 0:49b8e971fa83 85 }
eencae 0:49b8e971fa83 86
eencae 0:49b8e971fa83 87 return -1.0; // will never get here due to infinite loop
eencae 0:49b8e971fa83 88 }
eencae 0:49b8e971fa83 89
eencae 0:49b8e971fa83 90 void MeArm::error(int code)
eencae 0:49b8e971fa83 91 {
eencae 0:49b8e971fa83 92 // flash error code in infinite loop
eencae 0:49b8e971fa83 93 while(1) {
eencae 0:49b8e971fa83 94 leds->write(code);
eencae 0:49b8e971fa83 95 wait_ms(100);
eencae 0:49b8e971fa83 96 leds->write(0);
eencae 0:49b8e971fa83 97 wait_ms(100);
eencae 0:49b8e971fa83 98 }
eencae 0:49b8e971fa83 99 }
eencae 0:49b8e971fa83 100
eencae 0:49b8e971fa83 101 void MeArm::solveInverseKinematics()
eencae 0:49b8e971fa83 102 {
eencae 0:49b8e971fa83 103 serial->printf("#################################\n");
eencae 0:49b8e971fa83 104 serial->printf("\nx = %f y = %f z = %f\n\n",target.x,target.y,target.z);
eencae 0:49b8e971fa83 105
eencae 0:49b8e971fa83 106 float maxReach = 220.0; // distance when arm outstretched (mm)
eencae 0:49b8e971fa83 107
eencae 0:49b8e971fa83 108 // rudimentary checks if target is within reach - won't cover all possibilities - need to catch more later
eencae 0:49b8e971fa83 109 if (target.z > 140.0 || target.z < 25.0 || target.y < 0 || target.y > maxReach || target.x < -maxReach || target.x > maxReach ) {
eencae 0:49b8e971fa83 110 error(2); // hang and flash error code
eencae 0:49b8e971fa83 111 }
eencae 0:49b8e971fa83 112
eencae 0:49b8e971fa83 113 // distance to target in x-y plane
eencae 0:49b8e971fa83 114 float rTarget = sqrt(target.x*target.x + target.y*target.y);
eencae 0:49b8e971fa83 115 serial->printf("Distance to target = %f mm\n",rTarget);
eencae 0:49b8e971fa83 116
eencae 0:49b8e971fa83 117 // if target out of reach
eencae 0:49b8e971fa83 118 if (rTarget > maxReach ) {
eencae 0:49b8e971fa83 119 error(3); // hang and flash error code
eencae 0:49b8e971fa83 120 }
eencae 0:49b8e971fa83 121
eencae 0:49b8e971fa83 122 // distance to wrist position in x-y plane
eencae 0:49b8e971fa83 123 float rWrist = rTarget - L3;
eencae 0:49b8e971fa83 124 serial->printf("Distance to wrist = %f mm\n",rWrist);
eencae 0:49b8e971fa83 125
eencae 0:49b8e971fa83 126 // shoulder offset from centre of rotation of base
eencae 0:49b8e971fa83 127 float rShoulder = L0;
eencae 0:49b8e971fa83 128 serial->printf("Distance to shoulder = %f mm\n",rShoulder);
eencae 0:49b8e971fa83 129
eencae 0:49b8e971fa83 130 // direct distance from shoulder to wrist
eencae 0:49b8e971fa83 131 float Dsw = sqrt( (rWrist - rShoulder)*(rWrist - rShoulder) + (target.z-BASE_HEIGHT)*(target.z-BASE_HEIGHT) );
eencae 0:49b8e971fa83 132 serial->printf("Direct distance from shoulder to wrist = %f mm\n",Dsw);
eencae 0:49b8e971fa83 133
eencae 0:49b8e971fa83 134 // angle from shoulder to wrist
eencae 0:49b8e971fa83 135 float thetaSw;
eencae 0:49b8e971fa83 136 // angle from shoulder to wrist - with check
eencae 0:49b8e971fa83 137 if (fabs((rWrist - rShoulder)/Dsw) <= 1.0) {
eencae 0:49b8e971fa83 138 thetaSw = RAD2DEG*acos((rWrist - rShoulder)/Dsw);
eencae 0:49b8e971fa83 139 } else {
eencae 0:49b8e971fa83 140 error(4);
eencae 0:49b8e971fa83 141 }
eencae 0:49b8e971fa83 142
eencae 0:49b8e971fa83 143 // if target is below base, then reflect the angle
eencae 0:49b8e971fa83 144 if (target.z < BASE_HEIGHT)
eencae 0:49b8e971fa83 145 thetaSw*=-1.0;
eencae 0:49b8e971fa83 146
eencae 0:49b8e971fa83 147 serial->printf("Angle from shoulder to wrist = %f degree\n" , thetaSw);
eencae 0:49b8e971fa83 148
eencae 0:49b8e971fa83 149 // elbow internal angle
eencae 0:49b8e971fa83 150 float beta = cosineRule(L1, L2, Dsw);
eencae 0:49b8e971fa83 151 serial->printf("Elbow internal angle = %f degrees\n",beta);
eencae 0:49b8e971fa83 152
eencae 0:49b8e971fa83 153 // shoulder angle from horizontal
eencae 0:49b8e971fa83 154 thetaShoulder = thetaSw + cosineRule(Dsw, L1, L2);
eencae 0:49b8e971fa83 155 serial->printf("Shoulder servo angle = %f degrees\n",thetaShoulder);
eencae 0:49b8e971fa83 156
eencae 0:49b8e971fa83 157 // convert to servo angle
eencae 0:49b8e971fa83 158 thetaElbow = 180.0 - thetaShoulder - beta;
eencae 0:49b8e971fa83 159 serial->printf("Elbow servo angle = %f\n",thetaElbow);
eencae 0:49b8e971fa83 160
eencae 0:49b8e971fa83 161 // calculate angle from top view
eencae 0:49b8e971fa83 162 thetaBase = RAD2DEG*atan2(target.y, target.x);
eencae 0:49b8e971fa83 163 serial->printf("Base servo angle = %f degrees\n",thetaBase);
eencae 0:49b8e971fa83 164 }