First version, control arm through an imaginary 3D space.
Dependencies: SI1143 TextLCD mbed
main.cpp
- Committer:
- GAT27
- Date:
- 2013-12-10
- Revision:
- 0:ee5445096838
File content as of revision 0:ee5445096838:
#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); } }