First version, control arm through an imaginary 3D space.
Dependencies: SI1143 TextLCD mbed
main.cpp@0:ee5445096838, 2013-12-10 (annotated)
- Committer:
- GAT27
- Date:
- Tue Dec 10 19:46:49 2013 +0000
- Revision:
- 0:ee5445096838
v1.00
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GAT27 | 0:ee5445096838 | 1 | #include "mbed.h" |
GAT27 | 0:ee5445096838 | 2 | #include "SI1143.h" |
GAT27 | 0:ee5445096838 | 3 | #include "TextLCD.h" |
GAT27 | 0:ee5445096838 | 4 | |
GAT27 | 0:ee5445096838 | 5 | #define PI 3.14159265 |
GAT27 | 0:ee5445096838 | 6 | |
GAT27 | 0:ee5445096838 | 7 | DigitalOut led1(LED1); |
GAT27 | 0:ee5445096838 | 8 | SI1143 sensor(p28, p27); |
GAT27 | 0:ee5445096838 | 9 | PwmOut spin(p21); |
GAT27 | 0:ee5445096838 | 10 | PwmOut shoulder(p22); |
GAT27 | 0:ee5445096838 | 11 | PwmOut elbow(p25); |
GAT27 | 0:ee5445096838 | 12 | PwmOut grip(p26); |
GAT27 | 0:ee5445096838 | 13 | TextLCD lcd(p15, p16, p17, p23, p19, p24); |
GAT27 | 0:ee5445096838 | 14 | Serial pc(USBTX, USBRX); |
GAT27 | 0:ee5445096838 | 15 | |
GAT27 | 0:ee5445096838 | 16 | int main() |
GAT27 | 0:ee5445096838 | 17 | { |
GAT27 | 0:ee5445096838 | 18 | //LED sensor, bottom-left, top-left, bottom-right |
GAT27 | 0:ee5445096838 | 19 | int s1,s2,s3; |
GAT27 | 0:ee5445096838 | 20 | double sense1,sense2,sense3; |
GAT27 | 0:ee5445096838 | 21 | |
GAT27 | 0:ee5445096838 | 22 | //distance between sensors in cm |
GAT27 | 0:ee5445096838 | 23 | double diff = 5; |
GAT27 | 0:ee5445096838 | 24 | |
GAT27 | 0:ee5445096838 | 25 | //sensed and stored coordinates |
GAT27 | 0:ee5445096838 | 26 | double xs,ys,zs; |
GAT27 | 0:ee5445096838 | 27 | double x=45,y=5,z=50; |
GAT27 | 0:ee5445096838 | 28 | |
GAT27 | 0:ee5445096838 | 29 | //arbitrary position for arm |
GAT27 | 0:ee5445096838 | 30 | double stx=60,sty=25,stz=100; |
GAT27 | 0:ee5445096838 | 31 | |
GAT27 | 0:ee5445096838 | 32 | //length of arm segment |
GAT27 | 0:ee5445096838 | 33 | double len12=7.5, len23=15; |
GAT27 | 0:ee5445096838 | 34 | |
GAT27 | 0:ee5445096838 | 35 | //polar distance |
GAT27 | 0:ee5445096838 | 36 | double r; |
GAT27 | 0:ee5445096838 | 37 | |
GAT27 | 0:ee5445096838 | 38 | //check values |
GAT27 | 0:ee5445096838 | 39 | double root, tproot, wproot; |
GAT27 | 0:ee5445096838 | 40 | int timer=0; |
GAT27 | 0:ee5445096838 | 41 | |
GAT27 | 0:ee5445096838 | 42 | //motor 1,2,3 |
GAT27 | 0:ee5445096838 | 43 | double angle_sp,angle_sh,angle_el; |
GAT27 | 0:ee5445096838 | 44 | |
GAT27 | 0:ee5445096838 | 45 | //Setup |
GAT27 | 0:ee5445096838 | 46 | lcd.cls(); |
GAT27 | 0:ee5445096838 | 47 | led1 = 0; |
GAT27 | 0:ee5445096838 | 48 | grip.pulsewidth_us(700); |
GAT27 | 0:ee5445096838 | 49 | sensor.bias(4,20); |
GAT27 | 0:ee5445096838 | 50 | wait(1); |
GAT27 | 0:ee5445096838 | 51 | |
GAT27 | 0:ee5445096838 | 52 | while(1) |
GAT27 | 0:ee5445096838 | 53 | { |
GAT27 | 0:ee5445096838 | 54 | //Read each led sensor |
GAT27 | 0:ee5445096838 | 55 | s1 = sensor.get_ps1(4); |
GAT27 | 0:ee5445096838 | 56 | s2 = sensor.get_ps2(4); |
GAT27 | 0:ee5445096838 | 57 | s3 = sensor.get_ps3(4); |
GAT27 | 0:ee5445096838 | 58 | sense1 = /*1000 - */1500/s1; |
GAT27 | 0:ee5445096838 | 59 | sense2 = /*1000 - */1500/s2; |
GAT27 | 0:ee5445096838 | 60 | sense3 = /*1000 - */1200/s3; |
GAT27 | 0:ee5445096838 | 61 | |
GAT27 | 0:ee5445096838 | 62 | //Control for gripper |
GAT27 | 0:ee5445096838 | 63 | if (((s1>800) || (s2>600) || (s3>600)) && !timer) |
GAT27 | 0:ee5445096838 | 64 | { |
GAT27 | 0:ee5445096838 | 65 | timer=20; |
GAT27 | 0:ee5445096838 | 66 | if (!led1) |
GAT27 | 0:ee5445096838 | 67 | { |
GAT27 | 0:ee5445096838 | 68 | led1 = 1; |
GAT27 | 0:ee5445096838 | 69 | grip.pulsewidth_us(1350); |
GAT27 | 0:ee5445096838 | 70 | } |
GAT27 | 0:ee5445096838 | 71 | else |
GAT27 | 0:ee5445096838 | 72 | { |
GAT27 | 0:ee5445096838 | 73 | led1 = 0; |
GAT27 | 0:ee5445096838 | 74 | grip.pulsewidth_us(700); |
GAT27 | 0:ee5445096838 | 75 | } |
GAT27 | 0:ee5445096838 | 76 | } |
GAT27 | 0:ee5445096838 | 77 | timer--; |
GAT27 | 0:ee5445096838 | 78 | if (timer < 0) timer=0; |
GAT27 | 0:ee5445096838 | 79 | |
GAT27 | 0:ee5445096838 | 80 | //Set outer limit |
GAT27 | 0:ee5445096838 | 81 | if (sense1>110) sense1 = 110; |
GAT27 | 0:ee5445096838 | 82 | if (sense2>110) sense2 = 110; |
GAT27 | 0:ee5445096838 | 83 | if (sense3>110) sense3 = 110; |
GAT27 | 0:ee5445096838 | 84 | |
GAT27 | 0:ee5445096838 | 85 | //Trilateration for x |
GAT27 | 0:ee5445096838 | 86 | xs = 40 + ((pow(sense1,2) - pow(sense3,2) + pow(diff,2)) / (2*diff)); |
GAT27 | 0:ee5445096838 | 87 | if (xs<5) xs = 10; |
GAT27 | 0:ee5445096838 | 88 | if (xs>95) xs = 90; |
GAT27 | 0:ee5445096838 | 89 | |
GAT27 | 0:ee5445096838 | 90 | //Trilateration for y |
GAT27 | 0:ee5445096838 | 91 | ys = -sty + ((pow(sense1,2) - pow(sense2,2) + pow(diff,2)) / (2*diff)); |
GAT27 | 0:ee5445096838 | 92 | if (ys<-50) ys = -40; |
GAT27 | 0:ee5445096838 | 93 | if (ys>50) ys = 40; |
GAT27 | 0:ee5445096838 | 94 | |
GAT27 | 0:ee5445096838 | 95 | //Trilateration for z |
GAT27 | 0:ee5445096838 | 96 | root = pow(sense1+60,2) - pow(y,2) - pow(x,2); |
GAT27 | 0:ee5445096838 | 97 | zs = sqrt(root); |
GAT27 | 0:ee5445096838 | 98 | if (zs>70) zs = 70; |
GAT27 | 0:ee5445096838 | 99 | |
GAT27 | 0:ee5445096838 | 100 | //Tolerance level to store x,y,z |
GAT27 | 0:ee5445096838 | 101 | if (abs(x-xs) <= 7) x = xs; |
GAT27 | 0:ee5445096838 | 102 | if (abs(y-ys) <= 10) y = ys; |
GAT27 | 0:ee5445096838 | 103 | if (abs(z-zs) <= 6) z = zs; |
GAT27 | 0:ee5445096838 | 104 | |
GAT27 | 0:ee5445096838 | 105 | //Used for debugging |
GAT27 | 0:ee5445096838 | 106 | //x=60; |
GAT27 | 0:ee5445096838 | 107 | //z=15; |
GAT27 | 0:ee5445096838 | 108 | //for(y=-90;y<=90;y+=10){wait(1); |
GAT27 | 0:ee5445096838 | 109 | |
GAT27 | 0:ee5445096838 | 110 | //Angle for spin motor, converted into degrees |
GAT27 | 0:ee5445096838 | 111 | angle_sp = atan((stz - z)/(x - stx)) * 180/PI; |
GAT27 | 0:ee5445096838 | 112 | if (angle_sp < 0) |
GAT27 | 0:ee5445096838 | 113 | angle_sp += 180; |
GAT27 | 0:ee5445096838 | 114 | angle_sp = 180 - angle_sp; |
GAT27 | 0:ee5445096838 | 115 | |
GAT27 | 0:ee5445096838 | 116 | //Polar distance and arcosine wrapping |
GAT27 | 0:ee5445096838 | 117 | r = sqrt(pow(x - stx,2) + pow(stz - z,2)); |
GAT27 | 0:ee5445096838 | 118 | wproot = 2*pow(r,2) - pow(len12,2) - pow(len23,2); |
GAT27 | 0:ee5445096838 | 119 | while (wproot > 225) |
GAT27 | 0:ee5445096838 | 120 | wproot -= 225; |
GAT27 | 0:ee5445096838 | 121 | while (wproot < -225) |
GAT27 | 0:ee5445096838 | 122 | wproot += 225; |
GAT27 | 0:ee5445096838 | 123 | |
GAT27 | 0:ee5445096838 | 124 | //Angles for elbow motor first, then shoulder motor |
GAT27 | 0:ee5445096838 | 125 | angle_el = acos(wproot/(2*len12*len23)); |
GAT27 | 0:ee5445096838 | 126 | tproot = len12 + len23*cos(angle_el); |
GAT27 | 0:ee5445096838 | 127 | angle_sh = (-r*len23*sin(angle_el) + y*tproot)/(y*len23*sin(angle_el) + r*tproot); |
GAT27 | 0:ee5445096838 | 128 | |
GAT27 | 0:ee5445096838 | 129 | //Angles for shoulder and elbow motor, converted into degrees |
GAT27 | 0:ee5445096838 | 130 | angle_sh = 180 - (angle_sh * 180/PI); |
GAT27 | 0:ee5445096838 | 131 | angle_el = 90 + (angle_el * 180/PI); |
GAT27 | 0:ee5445096838 | 132 | |
GAT27 | 0:ee5445096838 | 133 | //Covert degrees to pulses to appropriate motors |
GAT27 | 0:ee5445096838 | 134 | spin.pulsewidth_us(800 + (angle_sp*20)/3); |
GAT27 | 0:ee5445096838 | 135 | shoulder.pulsewidth_us(800 + (angle_sh*20)/3); |
GAT27 | 0:ee5445096838 | 136 | elbow.pulsewidth_us(2300 - (angle_el*95)/9); |
GAT27 | 0:ee5445096838 | 137 | |
GAT27 | 0:ee5445096838 | 138 | //Debug info |
GAT27 | 0:ee5445096838 | 139 | printf("%.1f %.1f %.1f\r\n",xs,ys,zs); |
GAT27 | 0:ee5445096838 | 140 | //printf("%.1f %.1f %.1f\r\n",x,y,z); |
GAT27 | 0:ee5445096838 | 141 | //printf("%d %d %d\r\n",s1,s2,s3); |
GAT27 | 0:ee5445096838 | 142 | lcd.cls(); |
GAT27 | 0:ee5445096838 | 143 | lcd.printf("sp:%.0f sh:%.0f\nel:%.0f ",angle_sp,180-angle_sh,angle_el); |
GAT27 | 0:ee5445096838 | 144 | lcd.printf("%.0f,%.0f,%.0f",x,y,z);//} |
GAT27 | 0:ee5445096838 | 145 | //printf("%.2f %.2f %.2f\r\n\n",angle_sp,angle_sh,angle_el); |
GAT27 | 0:ee5445096838 | 146 | } |
GAT27 | 0:ee5445096838 | 147 | } |