Diff: MeArm.cpp

Revision:

0:49b8e971fa83

Child:

1:d2b7780ca6b3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MeArm.cpp	Tue Nov 04 20:54:49 2014 +0000
@@ -0,0 +1,164 @@
+/*
+@file MeArm.cpp
+
+@brief Member functions implementations
+
+*/
+#include "mbed.h"
+#include "MeArm.h"
+
+MeArm::MeArm(PinName basePin, PinName shoulderPin, PinName elbowPin, PinName gripperPin)
+{
+    // 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);
+}
+
+void MeArm::setTarget(float x,float y,float z)
+{
+    target.x = x;
+    target.y = y;
+    target.z = z;    
+    
+    // TODO - add checks for valid target
+}
+
+/// TODO - add checks for valid angles
+void MeArm::moveBaseServo(float angle)
+{
+    // 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
+}
+
+void MeArm::moveShoulderServo(float angle)
+{
+    // 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
+}
+
+void MeArm::moveElbowServo(float angle)
+{
+    // 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
+}
+
+void MeArm::moveServos()
+{
+    moveBaseServo(thetaBase);    
+    moveShoulderServo(thetaShoulder);    
+    moveElbowServo(thetaElbow);    
+}
+
+// calculate angle in DEGREES
+float MeArm::cosineRule(float adj1, float adj2, float opp)
+{
+    // cosine rule
+    float tmp = (adj1*adj1 + adj2*adj2 - opp*opp) / (2*adj1*adj2);
+    //println("tmp = " + tmp);
+
+    // check for valid angle
+    if ( fabs(tmp) <= 1.0 ) {
+        return RAD2DEG*acos(tmp);  
+    } else {
+        error(1);
+    }
+
+    return -1.0;  // will never get here due to infinite loop
+}
+
+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);
+    }
+}
+
+void MeArm::solveInverseKinematics()
+{
+    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)
+
+    // 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
+    }
+
+    // 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);
+
+    // if target out of reach
+    if (rTarget > maxReach ) {
+        error(3); // hang and flash error code
+    }
+
+    // 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);
+
+    // 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);
+    
+    // elbow internal angle
+    float beta = cosineRule(L1, L2, Dsw);
+    serial->printf("Elbow internal angle = %f degrees\n",beta);
+
+    // 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);
+    
+    // calculate angle from top view
+    thetaBase = RAD2DEG*atan2(target.y, target.x);
+    serial->printf("Base servo angle = %f degrees\n",thetaBase);
+}