Diff: MeArm.cpp

Revision:

1:d2b7780ca6b3

Parent:

0:49b8e971fa83

--- a/MeArm.cpp	Tue Nov 04 20:54:49 2014 +0000
+++ b/MeArm.cpp	Tue Jul 04 16:13:12 2017 +0000
@@ -1,164 +1,185 @@
 /*
 @file MeArm.cpp
-
-@brief Member functions implementations
-
 */
 #include "mbed.h"
 #include "MeArm.h"
 
-MeArm::MeArm(PinName basePin, PinName shoulderPin, PinName elbowPin, PinName gripperPin)
+MeArm::MeArm(PinName middle_pin, PinName left_pin, PinName right_pin, PinName claw_pin)
 {
-    // set up pins as required
-    base = new PwmOut(basePin);
-    shoulder = new PwmOut(shoulderPin);
-    elbow = new PwmOut(elbowPin);
-    gripper = new PwmOut(gripperPin);
-    // PWM objects have 50 Hz frequency by default so shouldn't need to set servo frequency
-    // can do it anyway to be sure. All channels share same period so just need to set one of them
-    base->period(1.0/50.0);  // servos are typically 50 Hz
-    // setup USB serial for debug/comms
-    serial = new Serial(USBTX,USBRX);
-    // leds used for debug
-    leds = new BusOut(LED4,LED3,LED2,LED1);
+    // PwmOut objects for servos
+    _middle_servo = new PwmOut(middle_pin);
+    _left_servo = new PwmOut(left_pin);
+    _right_servo = new PwmOut(right_pin);
+    _claw_servo = new PwmOut(claw_pin);
+
+    // set link lengths (mm)
+    _a[0] = 15.0;
+    _a[1] = 80.0;
+    _a[2] = 80.0;
+    _a[3] = 65.0;
+    
+    // default calibration values, will need changing for each MeArm   
+    _claw_cal[0] = 2200;
+    _claw_cal[1] = 1400;
+    //middle
+    _theta_1_cal[0] = 1650;
+    _theta_1_cal[1] = 2700;
+    // right
+    _theta_2_cal[0] = 2000;
+    _theta_2_cal[1] = 1000;
+    //left 
+    _theta_3_cal[0] = 1900;
+    _theta_3_cal[1] = 850;
+    
+    // height of shoulder servo off the floor (including gears under gripper)
+    _base_height = 30.0;
+
+    // set PWM period - all channels share so just need to set one
+    _middle_servo->period_ms(20);
+
 }
 
-void MeArm::setTarget(float x,float y,float z)
+MeArm::~MeArm()
 {
-    target.x = x;
-    target.y = y;
-    target.z = z;    
-    
-    // TODO - add checks for valid target
+    delete _middle_servo, _left_servo, _right_servo, _claw_servo;
 }
 
-/// TODO - add checks for valid angles
-void MeArm::moveBaseServo(float angle)
+void MeArm::set_claw_calibration_values(int open_val, int closed_val)
 {
-    // arm facing forward is defined as 90 degrees, angle increases counter-clockwise
-    int pulseWidthMicroSeconds = 575.0 + 985.0*angle/90.0;
-    serial->printf("Base Pulse = %d us\n",pulseWidthMicroSeconds);
-    // equation to convert angle to pulsewidth in us
-    base->pulsewidth_us(pulseWidthMicroSeconds);
-    // move servo
+    _claw_cal[0] = closed_val;
+    _claw_cal[1] = open_val;
 }
 
-void MeArm::moveShoulderServo(float angle)
+void MeArm::set_middle_calibration_values(int zero, int ninety)
+{
+    _theta_1_cal[0] = zero;
+    _theta_1_cal[1] = ninety;
+}
+void MeArm::set_right_calibration_values(int zero, int ninety)
 {
-    // angle relative to horizontal
-    int pulseWidthMicroSeconds = 2705.0 - 1097.0*angle/90.0;
-    serial->printf("Shoulder Pulse = %d us\n",pulseWidthMicroSeconds);
-    // equation to convert angle to pulsewidth in us
-    shoulder->pulsewidth_us(pulseWidthMicroSeconds);
-    // move servo
+    _theta_2_cal[0] = zero;
+    _theta_2_cal[1] = ninety;
+}
+void MeArm::set_left_calibration_values(int zero, int ninety)
+{
+    _theta_3_cal[0] = zero;
+    _theta_3_cal[1] = ninety;
 }
 
-void MeArm::moveElbowServo(float angle)
+void MeArm::set_claw(float value)
 {
-    // angle relative to horizontal
-    int pulseWidthMicroSeconds = 1910.0 - 1095.0*angle/90.0;
-    serial->printf("Elbow Pulse = %d us\n",pulseWidthMicroSeconds);
-    // equation to convert angle to pulsewidth in us
-    elbow->pulsewidth_us(pulseWidthMicroSeconds);
-    // move servo
+    // open - closed value
+    int range = _claw_cal[1]  -_claw_cal[0];
+
+    // closed + val * range
+    int pw = _claw_cal[0] + int(range*value);
+
+    _claw_servo->pulsewidth_us(pw);
 }
 
-void MeArm::moveServos()
+void MeArm::goto_xyz(float x, float y, float z)
 {
-    moveBaseServo(thetaBase);    
-    moveShoulderServo(thetaShoulder);    
-    moveElbowServo(thetaElbow);    
+    // set position
+    _x=x;
+    _y=y;
+    _z=z - _base_height;
+    
+    //printf("x = %f y = %f z = %f\n",x,y,z);
+
+    // do inverse kinematics
+    calc_middle_angle();
+    calc_right_angle();
+    calc_left_angle();
+    
+    // convert to pulse-width
+    calc_pw();
+    
+    // update servo position
+    move_servos();    
 }
 
-// calculate angle in DEGREES
-float MeArm::cosineRule(float adj1, float adj2, float opp)
+// base servo
+void MeArm::calc_middle_angle()
+{
+    _theta[0] = RAD2DEG * atan2( _y , _x);
+    //printf("theta[0] = %f\n",_theta[0]);
+}
+
+// "elbow" servo
+void MeArm::calc_left_angle()
 {
-    // cosine rule
-    float tmp = (adj1*adj1 + adj2*adj2 - opp*opp) / (2*adj1*adj2);
-    //println("tmp = " + tmp);
+// for the elbow, we use the 2-link planar simplification,
+    // so work out the direct distance (radius) from the shoulder to the target in xy
+    // then subtract the shoulder and gripper offsets to get the 'x' value
+    float d = sqrt( _x*_x + _y*_y );
+    float x_bar = d - _a[0] - _a[3];
 
-    // check for valid angle
-    if ( fabs(tmp) <= 1.0 ) {
-        return RAD2DEG*acos(tmp);  
-    } else {
-        error(1);
+    float D = (x_bar*x_bar + _z*_z - _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[2]*_a[1]);
+
+    if ( D*D > 1.0) {   // if this is the case, we will get complex roots and hence invalid solution
+        printf("Error!\n");
+        exit(1);
     }
 
-    return -1.0;  // will never get here due to infinite loop
-}
+    // from IK derivation
+    // we want the -ve root to get the 'elbow' up solution
+    _theta[2] = RAD2DEG*atan2( -sqrt(1-D*D) , D);
+    //printf("theta[2] = %f\n",_theta[2]);
+
 
-void MeArm::error(int code)
-{
-    // flash error code in infinite loop
-    while(1) {
-        leds->write(code);
-        wait_ms(100);
-        leds->write(0);
-        wait_ms(100);
-    }
+    // we don't actually control the elbow angle in the MeArm
+    // we need to convert to the angle that we do control
+    _theta[2] = -(_theta[1] + _theta[2]);
+    //printf("theta[2] = %f\n",_theta[2]);
+
 }
 
-void MeArm::solveInverseKinematics()
+// shoulder servo
+void MeArm::calc_right_angle()
 {
-    serial->printf("#################################\n");
-    serial->printf("\nx = %f y = %f z = %f\n\n",target.x,target.y,target.z);
-    
-    float maxReach = 220.0;  // distance when arm outstretched (mm)
+    // for the shoulder, we use the 2-link planar simplification,
+    // so work out the direct distance (radius) from the shoulder to the target in xy
+    // then subtract the shoulder and gripper offsets to get the 'x' value
 
-    // rudimentary checks if target is within reach - won't cover all possibilities - need to catch more later
-    if (target.z > 140.0 || target.z < 25.0 || target.y < 0 || target.y > maxReach || target.x < -maxReach || target.x > maxReach ) {
-        error(2);  // hang and flash error code
-    }
+    float d = sqrt( _x*_x + _y*_y );
+    float x_bar = d - _a[0] - _a[3];
 
-    // distance to target in x-y plane
-    float rTarget = sqrt(target.x*target.x + target.y*target.y);
-    serial->printf("Distance to target = %f mm\n",rTarget);
+    // from IK derivation
+    float E = (x_bar*x_bar + _z*_z + _a[1]*_a[1] - _a[2]*_a[2])/(2.0*_a[1]);
+    float r = sqrt( x_bar*x_bar + _z*_z );
 
-    // if target out of reach
-    if (rTarget > maxReach ) {
-        error(3); // hang and flash error code
+    if (E > r) {  // if this is the case, we will get complex roots and hence invalid solution
+        printf("Error!\n");
+        exit(1);
     }
 
-    // distance to wrist position in x-y plane
-    float rWrist = rTarget - L3;
-    serial->printf("Distance to wrist = %f mm\n",rWrist);
-
-    // shoulder offset from centre of rotation of base
-    float rShoulder = L0;
-    serial->printf("Distance to shoulder = %f mm\n",rShoulder);
+    // from IK derivation
+    // we want the +ve root to get the 'elbow' up solution
+    _theta[1] = RAD2DEG*( atan2(sqrt( r*r - E*E ),E) + atan2(_z,x_bar) );
+    //printf("theta[1] = %f\n",_theta[1]);
 
-    // direct distance from shoulder to wrist
-    float Dsw = sqrt( (rWrist - rShoulder)*(rWrist - rShoulder) + (target.z-BASE_HEIGHT)*(target.z-BASE_HEIGHT) );
-    serial->printf("Direct distance from shoulder to wrist = %f mm\n",Dsw);
-
-    // angle from shoulder to wrist
-    float thetaSw;
-    // angle from shoulder to wrist - with check
-    if (fabs((rWrist - rShoulder)/Dsw) <= 1.0) {
-        thetaSw = RAD2DEG*acos((rWrist - rShoulder)/Dsw);
-    } else {
-      error(4); 
-    }
+}
 
-    // if target is below base, then reflect the angle
-    if (target.z < BASE_HEIGHT)
-        thetaSw*=-1.0;
-
-    serial->printf("Angle from shoulder to wrist = %f degree\n" , thetaSw);
+void MeArm::calc_pw(){
+    
+    // 90 - 0
+    float range = (_theta_1_cal[1] - _theta_1_cal[0])/90.0;
+    _pw[0] = _theta_1_cal[0] + int(range*_theta[0]);
     
-    // elbow internal angle
-    float beta = cosineRule(L1, L2, Dsw);
-    serial->printf("Elbow internal angle = %f degrees\n",beta);
+    range = (_theta_2_cal[1] - _theta_2_cal[0])/90.0;
+    _pw[1] = _theta_2_cal[0] + int(range*_theta[1]);
+    
+    range = (_theta_3_cal[1] - _theta_3_cal[0])/90.0;
+    _pw[2] = _theta_3_cal[0] + int(range*_theta[2]);
+    
+    //printf("pw[0] = %i pw[1] = %i pw[2] = %i\n",_pw[0],_pw[1],_pw[2]);
+    
+}
 
-    // shoulder angle from horizontal
-    thetaShoulder = thetaSw + cosineRule(Dsw, L1, L2);
-    serial->printf("Shoulder servo angle = %f degrees\n",thetaShoulder);
-
-    // convert to servo angle
-    thetaElbow = 180.0 - thetaShoulder - beta;
-    serial->printf("Elbow servo angle = %f\n",thetaElbow);
+void MeArm::move_servos() {
     
-    // calculate angle from top view
-    thetaBase = RAD2DEG*atan2(target.y, target.x);
-    serial->printf("Base servo angle = %f degrees\n",thetaBase);
-}
+    //printf("Move servos\n");
+    _middle_servo->pulsewidth_us(_pw[0]);
+    _right_servo->pulsewidth_us(_pw[1]); 
+    _left_servo->pulsewidth_us(_pw[2]);    
+}
\ No newline at end of file