First version, control arm through an imaginary 3D space.

Dependencies:   SI1143 TextLCD mbed

Files at this revision

API Documentation at this revision

Comitter:
GAT27
Date:
Tue Dec 10 19:46:49 2013 +0000
Commit message:
v1.00

Changed in this revision

SI1143.lib Show annotated file Show diff for this revision Revisions of this file
TextLCD.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r ee5445096838 SI1143.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SI1143.lib	Tue Dec 10 19:46:49 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/GAT27/code/SI1143/#50b60d59d568
diff -r 000000000000 -r ee5445096838 TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Tue Dec 10 19:46:49 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
diff -r 000000000000 -r ee5445096838 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 10 19:46:49 2013 +0000
@@ -0,0 +1,147 @@
+#include "mbed.h"
+#include "SI1143.h"
+#include "TextLCD.h"
+
+#define PI 3.14159265
+
+DigitalOut led1(LED1);
+SI1143 sensor(p28, p27);
+PwmOut spin(p21);
+PwmOut shoulder(p22);
+PwmOut elbow(p25);
+PwmOut grip(p26);
+TextLCD lcd(p15, p16, p17, p23, p19, p24);
+Serial pc(USBTX, USBRX);
+ 
+int main()
+{
+    //LED sensor, bottom-left, top-left, bottom-right
+    int s1,s2,s3;
+    double sense1,sense2,sense3;
+    
+    //distance between sensors in cm
+    double diff = 5;
+    
+    //sensed and stored coordinates
+    double xs,ys,zs;
+    double x=45,y=5,z=50;
+    
+    //arbitrary position for arm
+    double stx=60,sty=25,stz=100;
+    
+    //length of arm segment
+    double len12=7.5, len23=15;
+    
+    //polar distance
+    double r;
+    
+    //check values
+    double root, tproot, wproot;
+    int timer=0;
+    
+    //motor 1,2,3
+    double angle_sp,angle_sh,angle_el;
+    
+    //Setup
+    lcd.cls();
+    led1 = 0;
+    grip.pulsewidth_us(700);
+    sensor.bias(4,20);
+    wait(1);
+    
+    while(1)
+    {
+        //Read each led sensor
+        s1 = sensor.get_ps1(4);
+        s2 = sensor.get_ps2(4);
+        s3 = sensor.get_ps3(4);
+        sense1 = /*1000 - */1500/s1;
+        sense2 = /*1000 - */1500/s2;
+        sense3 = /*1000 - */1200/s3;
+        
+        //Control for gripper
+        if (((s1>800) || (s2>600) || (s3>600)) && !timer)
+        {
+            timer=20;
+            if (!led1)
+            {
+                led1 = 1;
+                grip.pulsewidth_us(1350);
+            }
+            else
+            {
+                led1 = 0;
+                grip.pulsewidth_us(700);
+            }
+        }
+        timer--;
+        if (timer < 0) timer=0;
+        
+        //Set outer limit
+        if (sense1>110) sense1 = 110;
+        if (sense2>110) sense2 = 110;
+        if (sense3>110) sense3 = 110;
+        
+        //Trilateration for x
+        xs = 40 + ((pow(sense1,2) - pow(sense3,2) + pow(diff,2)) / (2*diff));
+        if (xs<5) xs = 10;
+        if (xs>95) xs = 90;
+        
+        //Trilateration for y
+        ys = -sty + ((pow(sense1,2) - pow(sense2,2) + pow(diff,2)) / (2*diff));
+        if (ys<-50) ys = -40;
+        if (ys>50) ys = 40;
+        
+        //Trilateration for z
+        root = pow(sense1+60,2) - pow(y,2) - pow(x,2);
+        zs = sqrt(root);
+        if (zs>70) zs = 70;
+        
+        //Tolerance level to store x,y,z
+        if (abs(x-xs) <= 7) x = xs;
+        if (abs(y-ys) <= 10) y = ys;
+        if (abs(z-zs) <= 6) z = zs;
+        
+        //Used for debugging
+        //x=60;
+        //z=15;
+        //for(y=-90;y<=90;y+=10){wait(1);
+        
+        //Angle for spin motor, converted into degrees
+        angle_sp = atan((stz - z)/(x - stx)) * 180/PI;
+        if (angle_sp < 0)
+            angle_sp += 180;
+        angle_sp = 180 - angle_sp;
+        
+        //Polar distance and arcosine wrapping
+        r = sqrt(pow(x - stx,2) + pow(stz - z,2));
+        wproot = 2*pow(r,2) - pow(len12,2) - pow(len23,2);
+        while (wproot > 225)
+            wproot -= 225;
+        while (wproot < -225)
+            wproot += 225;
+        
+        //Angles for elbow motor first, then shoulder motor
+        angle_el = acos(wproot/(2*len12*len23));
+        tproot = len12 + len23*cos(angle_el);
+        angle_sh = (-r*len23*sin(angle_el) + y*tproot)/(y*len23*sin(angle_el) + r*tproot);
+        
+        //Angles for shoulder and elbow motor, converted into degrees
+        angle_sh = 180 - (angle_sh * 180/PI);
+        angle_el = 90 + (angle_el * 180/PI);
+        
+        //Covert degrees to pulses to appropriate motors
+        spin.pulsewidth_us(800 + (angle_sp*20)/3);
+        shoulder.pulsewidth_us(800 + (angle_sh*20)/3);
+        elbow.pulsewidth_us(2300 - (angle_el*95)/9);
+        
+        //Debug info
+        printf("%.1f %.1f %.1f\r\n",xs,ys,zs);
+        //printf("%.1f %.1f %.1f\r\n",x,y,z);
+        //printf("%d %d %d\r\n",s1,s2,s3);
+        lcd.cls();
+        lcd.printf("sp:%.0f sh:%.0f\nel:%.0f ",angle_sp,180-angle_sh,angle_el);
+        lcd.printf("%.0f,%.0f,%.0f",x,y,z);//}
+        //printf("%.2f %.2f %.2f\r\n\n",angle_sp,angle_sh,angle_el);
+    }
+}
diff -r 000000000000 -r ee5445096838 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Dec 10 19:46:49 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file