Revision 0:4a15e2d20446, committed 2011-12-14

Comitter:

gvalentin3

Date:

Wed Dec 14 17:08:38 2011 +0000

Commit message:

First published version. Height is still hardcoded to -70mm.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Wed Dec 14 17:08:38 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Dec 14 17:08:38 2011 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+#include "robo.h"
+
+#define PI 3.14159265
+#include "mbed.h"
+
+robo arm(p21, p22, p23, p24, p25, p26);
+Serial myserial(p28, p27);
+
+float deg = 180/PI;
+float rad = PI/180;
+float openclaw = 0;
+float closeclaw = 100;
+
+char xin;
+char yin;
+char cords[3];
+int x;
+int y;
+int z;
+int zin;
+
+int main() {
+
+//Start Arm in standard position.//
+arm.writeBase(90*rad);   
+wait(0.2);
+arm.writeElbow(90*rad); 
+wait(0.2);
+arm.writeWrist(90*rad);
+wait(0.2);
+arm.writeClaw(openclaw);
+wait(0.2);
+
+
+while (1) {
+
+    myserial.scanf("%s", &cords); //scan a string with the coordinates of the object;
+        
+    xin=cords[0];
+    zin=cords[1];
+        
+    x= (int)xin;   //x will be distance sent by amigobot's camera. (It is y in the image, but x in the algorithm)
+    y= -70;       //height form the base of the arm to the top of the trash in mm. Right now it is hardcoded.
+    z= (int)zin; //z will be the horizontal distance. Right now its not being used.
+
+    float * angles;  //Initialize angles vector where the calculatiosn will be stored.
+    angles = (float *)malloc(3 * sizeof(float));
+    angles[0] = 0;
+    angles[1] = 0;
+    angles[2] = 0;
+
+    float oe = -90*rad;
+    angles = arm.calculateangle(x, y, oe, angles);// convert positions to angles and store in angles variable
+
+    //store the angles into a printable variable (Haven't had luck printing array entries)
+    float baseangle = angles[0];
+    float elbowangle = angles[1];
+    float wristangle = angles[2];
+
+    // Pick up Object
+    arm.writeBase(baseangle);   //move arm based on calculated angles
+    wait(0.05);
+    arm.writeElbow(elbowangle); //move arm based on calculated angles
+    wait(0.05);
+    arm.writeWrist(wristangle); //move arm based on calculated angles
+    wait(1.5);
+    arm.writeClaw(closeclaw);   //close the claw before moving to object
+    wait(1);
+
+    // Deposit Object in bin behind it
+    arm.writeBase(70*rad);
+    wait(0.2);
+    arm.writeElbow(0*rad);
+    wait(0.7);
+    arm.writeElbow(-90*rad);
+    wait(0.7);
+    arm.writeWrist(0*rad);
+    wait(1);
+
+    // Let go of Object
+    arm.writeClaw(openclaw);
+    wait(1.5);
+
+    //Return to initial position
+    arm.writeBase(90*rad);
+    wait(0.2);
+    arm.writeElbow(0*rad);
+    wait(1);
+    arm.writeElbow(90*rad); 
+    wait(0.2);
+    arm.writeWrist(90*rad);
+    wait(0.2);      
+    //free(angles);
+}//end of while     
+}//end main

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Dec 14 17:08:38 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912

--- /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;
+
+
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robo.h	Wed Dec 14 17:08:38 2011 +0000
@@ -0,0 +1,25 @@
+//Robo class header for controlling robotic arm
+//Created 10/12/2011 by MLD 427
+
+#include "mbed.h"
+class robo{
+    private:
+    PwmOut *spin;
+    PwmOut *base;
+    PwmOut *elbow;
+    PwmOut *wrist;
+    PwmOut *claw;
+    PwmOut *trash;
+    public:
+    robo(PinName s, PinName b, PinName e, PinName w, PinName c, PinName t);
+    void write(float percent, int motor);
+    void writeSpin(float percent);
+    void writeBase(float percent);
+    void writeElbow(float percent);
+    void writeWrist(float percent);
+    void writeClaw(float percent);
+    void writeTrash(float percent);
+    float* calculateangle(float x1e, float y1e, float oe, float angles[]);
+    
+    
+};
\ No newline at end of file