Guillermo Torijano
First version, control arm through an imaginary 3D space.
Dependencies: SI1143 TextLCD mbed
Revision 0:ee5445096838, committed 2013-12-10
- Comitter:
- GAT27
- Date:
- Tue Dec 10 19:46:49 2013 +0000
- Commit message:
- v1.00
Changed in this revision
--- /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
--- /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
--- /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);
+ }
+}
--- /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