![](/media/cache/profiles/5a211ba771eef85828a0a9299467ea19.jpg.50x50_q85.png)
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