Guillermo Torijano / Mbed 2 deprecated Gesture_Arm_Robot

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?

UserRevisionLine numberNew 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 }