main.cpp

00001 #include "mbed.h"
00002 #include "SI1143.h"
00003 #include "TextLCD.h"
00004 
00005 #define PI 3.14159265
00006 
00007 DigitalOut led1(LED1);
00008 SI1143 sensor(p28, p27);
00009 PwmOut spin(p21);
00010 PwmOut shoulder(p22);
00011 PwmOut elbow(p25);
00012 PwmOut grip(p26);
00013 TextLCD lcd(p15, p16, p17, p23, p19, p24);
00014 Serial pc(USBTX, USBRX);
00015  
00016 int main()
00017 {
00018     //LED sensor, bottom-left, top-left, bottom-right
00019     int s1,s2,s3;
00020     double sense1,sense2,sense3;
00021     
00022     //distance between sensors in cm
00023     double diff = 5;
00024     
00025     //sensed and stored coordinates
00026     double xs,ys,zs;
00027     double x=45,y=5,z=50;
00028     
00029     //arbitrary position for arm
00030     double stx=60,sty=25,stz=100;
00031     
00032     //length of arm segment
00033     double len12=7.5, len23=15;
00034     
00035     //polar distance
00036     double r;
00037     
00038     //check values
00039     double root, tproot, wproot;
00040     int timer=0;
00041     
00042     //motor 1,2,3
00043     double angle_sp,angle_sh,angle_el;
00044     
00045     //Setup
00046     lcd.cls();
00047     led1 = 0;
00048     grip.pulsewidth_us(700);
00049     sensor.bias(4,20);
00050     wait(1);
00051     
00052     while(1)
00053     {
00054         //Read each led sensor
00055         s1 = sensor.get_ps1(4);
00056         s2 = sensor.get_ps2(4);
00057         s3 = sensor.get_ps3(4);
00058         sense1 = /*1000 - */1500/s1;
00059         sense2 = /*1000 - */1500/s2;
00060         sense3 = /*1000 - */1200/s3;
00061         
00062         //Control for gripper
00063         if (((s1>800) || (s2>600) || (s3>600)) && !timer)
00064         {
00065             timer=20;
00066             if (!led1)
00067             {
00068                 led1 = 1;
00069                 grip.pulsewidth_us(1350);
00070             }
00071             else
00072             {
00073                 led1 = 0;
00074                 grip.pulsewidth_us(700);
00075             }
00076         }
00077         timer--;
00078         if (timer < 0) timer=0;
00079         
00080         //Set outer limit
00081         if (sense1>110) sense1 = 110;
00082         if (sense2>110) sense2 = 110;
00083         if (sense3>110) sense3 = 110;
00084         
00085         //Trilateration for x
00086         xs = 40 + ((pow(sense1,2) - pow(sense3,2) + pow(diff,2)) / (2*diff));
00087         if (xs<5) xs = 10;
00088         if (xs>95) xs = 90;
00089         
00090         //Trilateration for y
00091         ys = -sty + ((pow(sense1,2) - pow(sense2,2) + pow(diff,2)) / (2*diff));
00092         if (ys<-50) ys = -40;
00093         if (ys>50) ys = 40;
00094         
00095         //Trilateration for z
00096         root = pow(sense1+60,2) - pow(y,2) - pow(x,2);
00097         zs = sqrt(root);
00098         if (zs>70) zs = 70;
00099         
00100         //Tolerance level to store x,y,z
00101         if (abs(x-xs) <= 7) x = xs;
00102         if (abs(y-ys) <= 10) y = ys;
00103         if (abs(z-zs) <= 6) z = zs;
00104         
00105         //Used for debugging
00106         //x=60;
00107         //z=15;
00108         //for(y=-90;y<=90;y+=10){wait(1);
00109         
00110         //Angle for spin motor, converted into degrees
00111         angle_sp = atan((stz - z)/(x - stx)) * 180/PI;
00112         if (angle_sp < 0)
00113             angle_sp += 180;
00114         angle_sp = 180 - angle_sp;
00115         
00116         //Polar distance and arcosine wrapping
00117         r = sqrt(pow(x - stx,2) + pow(stz - z,2));
00118         wproot = 2*pow(r,2) - pow(len12,2) - pow(len23,2);
00119         while (wproot > 225)
00120             wproot -= 225;
00121         while (wproot < -225)
00122             wproot += 225;
00123         
00124         //Angles for elbow motor first, then shoulder motor
00125         angle_el = acos(wproot/(2*len12*len23));
00126         tproot = len12 + len23*cos(angle_el);
00127         angle_sh = (-r*len23*sin(angle_el) + y*tproot)/(y*len23*sin(angle_el) + r*tproot);
00128         
00129         //Angles for shoulder and elbow motor, converted into degrees
00130         angle_sh = 180 - (angle_sh * 180/PI);
00131         angle_el = 90 + (angle_el * 180/PI);
00132         
00133         //Covert degrees to pulses to appropriate motors
00134         spin.pulsewidth_us(800 + (angle_sp*20)/3);
00135         shoulder.pulsewidth_us(800 + (angle_sh*20)/3);
00136         elbow.pulsewidth_us(2300 - (angle_el*95)/9);
00137         
00138         //Debug info
00139         printf("%.1f %.1f %.1f\r\n",xs,ys,zs);
00140         //printf("%.1f %.1f %.1f\r\n",x,y,z);
00141         //printf("%d %d %d\r\n",s1,s2,s3);
00142         lcd.cls();
00143         lcd.printf("sp:%.0f sh:%.0f\nel:%.0f ",angle_sp,180-angle_sh,angle_el);
00144         lcd.printf("%.0f,%.0f,%.0f",x,y,z);//}
00145         //printf("%.2f %.2f %.2f\r\n\n",angle_sp,angle_sh,angle_el);
00146     }
00147 }