Diff: robo.cpp

Revision:

0:4a15e2d20446

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robo.cpp	Wed Dec 14 17:08:38 2011 +0000
@@ -0,0 +1,113 @@
+//Robo class implementation for controlling robotic arm
+// Servos: A 1ms pulse is 0 degrees, 1.5ms is 90 degrees and a 2 ms pulse is approximately 180 degrees. 
+// New timing pulses are sent to the servo every 20 ms.
+// In theory .005556ms = 1 degree
+// 5.56
+// Currently .000010 = 1 increment
+
+#include "robo.h"
+#include "mbed.h"
+
+#define PI 3.14159265
+
+robo::robo(PinName s, PinName b, PinName e, PinName w, PinName c,PinName t){
+    spin = new PwmOut(s);
+    base = new PwmOut(b);
+    elbow = new PwmOut(e);
+    wrist = new PwmOut(w);
+    claw = new PwmOut(c);
+    trash = new PwmOut(t);
+    spin->period_ms(20);
+    base->period_ms(20);
+    elbow->period_ms(20);
+    wrist->period_ms(20);
+    claw->period_ms(20);
+    trash->period_ms(20);
+}
+
+void robo::write(float angle, int servo){
+    //float oneDeg = 5.555556;
+    float oneRad = 318.309886;
+    float spinOffset = 1150;
+    float baseOffset = 950;
+    float elbowOffset = 1250;
+    float wristOffset = 950;
+    float clawOffset = 1000;
+    
+
+        switch(servo){
+            case(0):
+                spin->pulsewidth_us(angle*oneRad + spinOffset);
+            break;
+            case(1):
+                base->pulsewidth_us(angle*oneRad + baseOffset);
+            break;
+            case(2):
+                elbow->pulsewidth_us(angle*oneRad + elbowOffset);
+            break;
+            case(3):
+                wrist->pulsewidth_us(angle*oneRad + wristOffset);
+            break;
+            case(4):
+                claw->pulsewidth_us(angle*10 + clawOffset);
+            break;
+            case(5):
+                trash->pulsewidth_us(angle*oneRad + 1000);
+            break;            
+        }
+     
+}
+
+void robo::writeSpin(float angle){
+    write(angle,0);
+}
+void robo::writeBase(float angle){
+    write(angle,1);
+    }
+void robo::writeElbow(float angle){
+    write(angle,2);
+}
+void robo::writeWrist(float angle){
+    write(angle,3);
+}
+void robo::writeClaw(float angle){
+    write(angle,4);
+}
+void robo::writeTrash(float angle){
+    write(angle,5);
+}
+
+
+
+
+float* robo::calculateangle(float x1e, float y1e, float oe, float angles[]){      
+
+//relative units
+float a1 = 95;//mm's from length from base to elbow;
+float a2 = 110;//mm's from length from elbow to wrist;
+float a3 = 45;//mm's from length from wrist to claw;
+
+float ce = cos(oe);
+float se = sin(oe);
+
+float o2 = -acos(  ((x1e-a3*ce)*(x1e-a3*ce) + (y1e-a3*se)*(y1e-a3*se) - a1*a1 - a2*a2) / (2*a1*a2)   );
+
+float s2 = sin(o2);
+float c2 = cos(o2);
+
+float delta = a1*a1 + a2*a2 + 2*a1*a2*c2;
+float det1 = (x1e-a3*ce)*(a1+a2*c2) - (y1e-a3*se)*(-a2*s2);
+                                    
+float o1 = acos(det1/delta);
+float o3 = oe-o1-o2;
+
+angles[0]= o1;
+angles[1]= -o2; //Had to use negative sign to reconcile with arm configuation.
+angles[2]= -o3; //Had to use negative sign to reconcile with arm configuation.
+
+return angles;
+
+
+}
+
+