Diff: main.cpp

Revision:

0:ee5445096838

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