robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
main.cpp@1:ffc1a2d6a824, 2016-10-24 (annotated)
- Committer:
- kbruil
- Date:
- Mon Oct 24 19:27:39 2016 +0000
- Revision:
- 1:ffc1a2d6a824
- Parent:
- 0:86fe02a64f0e
- Child:
- 2:027d19cda97b
resolving annoying atan2 and acos issues;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kbruil | 0:86fe02a64f0e | 1 | #include "mbed.h" |
kbruil | 0:86fe02a64f0e | 2 | #include "math.h" |
kbruil | 0:86fe02a64f0e | 3 | #include "stdio.h" |
kbruil | 0:86fe02a64f0e | 4 | #include "MODSERIAL.h" |
kbruil | 0:86fe02a64f0e | 5 | // Angle limits 215, 345 lower arm, 0, 145 upper arm |
kbruil | 0:86fe02a64f0e | 6 | // Robot arm x,y limits (limit to table top) |
kbruil | 0:86fe02a64f0e | 7 | |
kbruil | 0:86fe02a64f0e | 8 | |
kbruil | 0:86fe02a64f0e | 9 | #define M_PI 3.141592653589793238462643383279502884L // Pi |
kbruil | 1:ffc1a2d6a824 | 10 | const double R_XTABLE_LO = -30.0; // Robot in the middle, table width=60 |
kbruil | 1:ffc1a2d6a824 | 11 | const double R_XTABLE_HI = 30.0; |
kbruil | 1:ffc1a2d6a824 | 12 | const double R_YTABLE_LO = -18.0; |
kbruil | 1:ffc1a2d6a824 | 13 | const double R_YTABLE_HI = 82.0; |
kbruil | 1:ffc1a2d6a824 | 14 | const double R_XROBOT_LO = -8.0; // Robot limited from moving onto itself (20x20 base) |
kbruil | 1:ffc1a2d6a824 | 15 | const double R_XROBOT_HI = 8.0; |
kbruil | 1:ffc1a2d6a824 | 16 | const double R_YROBOT_LO = -18.0; |
kbruil | 1:ffc1a2d6a824 | 17 | const double R_YROBOT_HI = 2.0; |
kbruil | 1:ffc1a2d6a824 | 18 | |
kbruil | 1:ffc1a2d6a824 | 19 | const double ARM1_ALIMLO = (2*M_PI/360)*125; |
kbruil | 1:ffc1a2d6a824 | 20 | const double ARM1_ALIMHI = -(2*M_PI/360)*125; |
kbruil | 1:ffc1a2d6a824 | 21 | const double ARM2_ALIMLO = 0; |
kbruil | 1:ffc1a2d6a824 | 22 | const double ARM2_ALIMHI = (2*M_PI/360)*145; |
kbruil | 0:86fe02a64f0e | 23 | |
kbruil | 0:86fe02a64f0e | 24 | // Struct r_link: |
kbruil | 0:86fe02a64f0e | 25 | // Defines a robot link type (arm or end effector). |
kbruil | 0:86fe02a64f0e | 26 | struct r_link { |
kbruil | 0:86fe02a64f0e | 27 | float length; // link length |
kbruil | 0:86fe02a64f0e | 28 | float x; // link x position |
kbruil | 0:86fe02a64f0e | 29 | float y; // link y position |
kbruil | 0:86fe02a64f0e | 30 | float vx; // link x speed |
kbruil | 0:86fe02a64f0e | 31 | float vy; // link y speed |
kbruil | 0:86fe02a64f0e | 32 | float ax; // link x accelleration |
kbruil | 0:86fe02a64f0e | 33 | float ay; // link y accelleration |
kbruil | 0:86fe02a64f0e | 34 | float theta; // link angle |
kbruil | 0:86fe02a64f0e | 35 | float omega; // link angular velocity |
kbruil | 0:86fe02a64f0e | 36 | float alpha; // link angular accelleration |
kbruil | 0:86fe02a64f0e | 37 | }; |
kbruil | 0:86fe02a64f0e | 38 | |
kbruil | 0:86fe02a64f0e | 39 | MODSERIAL pc(USBTX, USBRX); |
kbruil | 0:86fe02a64f0e | 40 | DigitalIn switch1(SW3); |
kbruil | 0:86fe02a64f0e | 41 | DigitalIn switch2(SW2); |
kbruil | 0:86fe02a64f0e | 42 | DigitalIn switch3(D7); |
kbruil | 0:86fe02a64f0e | 43 | DigitalIn switch4(D9); |
kbruil | 0:86fe02a64f0e | 44 | DigitalOut ledr(LED_RED); |
kbruil | 0:86fe02a64f0e | 45 | DigitalOut ledg(LED_GREEN); |
kbruil | 0:86fe02a64f0e | 46 | |
kbruil | 0:86fe02a64f0e | 47 | |
kbruil | 0:86fe02a64f0e | 48 | r_link arm1; // Robot upper arm |
kbruil | 0:86fe02a64f0e | 49 | r_link arm2; // Robot lower arm |
kbruil | 0:86fe02a64f0e | 50 | r_link end; // Robot end effector |
kbruil | 0:86fe02a64f0e | 51 | |
kbruil | 0:86fe02a64f0e | 52 | // robot_applylinmits |
kbruil | 0:86fe02a64f0e | 53 | // Input: x,y |
kbruil | 0:86fe02a64f0e | 54 | // Output: x,y limited to limits set by constants |
kbruil | 0:86fe02a64f0e | 55 | void robot_applylimits (float& x, float& y){ |
kbruil | 0:86fe02a64f0e | 56 | float z; |
kbruil | 0:86fe02a64f0e | 57 | float angle; |
kbruil | 0:86fe02a64f0e | 58 | |
kbruil | 0:86fe02a64f0e | 59 | // Limit x,y to table top size |
kbruil | 0:86fe02a64f0e | 60 | if (x<R_XTABLE_LO) { x=R_XTABLE_LO; } |
kbruil | 0:86fe02a64f0e | 61 | else if (x>R_XTABLE_HI) { x=R_XTABLE_HI; } |
kbruil | 0:86fe02a64f0e | 62 | if (y<R_YTABLE_LO) { y=R_YTABLE_LO; } |
kbruil | 0:86fe02a64f0e | 63 | else if (y>R_YTABLE_HI) { y=R_YTABLE_HI; } |
kbruil | 0:86fe02a64f0e | 64 | |
kbruil | 0:86fe02a64f0e | 65 | // Limit x,y from moving into the robot base |
kbruil | 0:86fe02a64f0e | 66 | /* |
kbruil | 0:86fe02a64f0e | 67 | if (x>R_XROBOT_LO && x<R_XROBOT_HI && y>R_YROBOT_LO && y<R_YROBOT_HI) |
kbruil | 0:86fe02a64f0e | 68 | { |
kbruil | 0:86fe02a64f0e | 69 | if (x<=0){ |
kbruil | 0:86fe02a64f0e | 70 | x=R_XROBOT_LO; |
kbruil | 0:86fe02a64f0e | 71 | } |
kbruil | 0:86fe02a64f0e | 72 | else { |
kbruil | 0:86fe02a64f0e | 73 | x=R_XROBOT_HI; |
kbruil | 0:86fe02a64f0e | 74 | } |
kbruil | 1:ffc1a2d6a824 | 75 | if (y>R_YROBOT_LO && y<R_YROBOT_HI){ |
kbruil | 0:86fe02a64f0e | 76 | y=R_YROBOT_HI; |
kbruil | 0:86fe02a64f0e | 77 | } |
kbruil | 0:86fe02a64f0e | 78 | } |
kbruil | 0:86fe02a64f0e | 79 | */ |
kbruil | 0:86fe02a64f0e | 80 | // Limit x,y to maximum size of robot arm |
kbruil | 0:86fe02a64f0e | 81 | z = sqrt(pow(x,2)+pow(y,2)); |
kbruil | 0:86fe02a64f0e | 82 | angle = atan2(y,x); |
kbruil | 1:ffc1a2d6a824 | 83 | // angle = angle + M_PI; |
kbruil | 0:86fe02a64f0e | 84 | if (z>(arm1.length+arm2.length)){ |
kbruil | 0:86fe02a64f0e | 85 | z = arm1.length+arm2.length; |
kbruil | 0:86fe02a64f0e | 86 | x = z*cos(angle); |
kbruil | 0:86fe02a64f0e | 87 | y = z*sin(angle); |
kbruil | 0:86fe02a64f0e | 88 | } |
kbruil | 0:86fe02a64f0e | 89 | } |
kbruil | 0:86fe02a64f0e | 90 | |
kbruil | 0:86fe02a64f0e | 91 | // robot_setposition: |
kbruil | 0:86fe02a64f0e | 92 | // Input: x,y cartesian coordinates |
kbruil | 0:86fe02a64f0e | 93 | // Function: calculate arm angles based on cartesian coordinates |
kbruil | 0:86fe02a64f0e | 94 | void robot_setposition (float x,float y){ |
kbruil | 0:86fe02a64f0e | 95 | float z; |
kbruil | 0:86fe02a64f0e | 96 | float z1; |
kbruil | 0:86fe02a64f0e | 97 | float z2; |
kbruil | 0:86fe02a64f0e | 98 | |
kbruil | 0:86fe02a64f0e | 99 | float angle; |
kbruil | 0:86fe02a64f0e | 100 | float b1,b2; |
kbruil | 0:86fe02a64f0e | 101 | |
kbruil | 0:86fe02a64f0e | 102 | float old; |
kbruil | 0:86fe02a64f0e | 103 | |
kbruil | 0:86fe02a64f0e | 104 | robot_applylimits(x,y); |
kbruil | 0:86fe02a64f0e | 105 | // end.x = x; |
kbruil | 0:86fe02a64f0e | 106 | // end.y = y; |
kbruil | 0:86fe02a64f0e | 107 | |
kbruil | 1:ffc1a2d6a824 | 108 | // printf("xy: %f, %f\n\r",x,y); |
kbruil | 0:86fe02a64f0e | 109 | z = sqrt(pow(x,2)+pow(y,2)); |
kbruil | 0:86fe02a64f0e | 110 | angle = atan2(y,x); |
kbruil | 1:ffc1a2d6a824 | 111 | |
kbruil | 1:ffc1a2d6a824 | 112 | // printf("angle voor gekloot: %f\n\r", angle); |
kbruil | 1:ffc1a2d6a824 | 113 | |
kbruil | 1:ffc1a2d6a824 | 114 | if (angle<0.5*M_PI && angle>0){ |
kbruil | 1:ffc1a2d6a824 | 115 | angle = angle - 0.5*M_PI; |
kbruil | 0:86fe02a64f0e | 116 | } |
kbruil | 1:ffc1a2d6a824 | 117 | else if (angle<M_PI && angle>0.5*M_PI){ |
kbruil | 1:ffc1a2d6a824 | 118 | angle = angle - 0.5*M_PI; |
kbruil | 0:86fe02a64f0e | 119 | } |
kbruil | 1:ffc1a2d6a824 | 120 | else if (angle<0 && angle>-0.5*M_PI){ |
kbruil | 1:ffc1a2d6a824 | 121 | angle = angle - 0.5 * M_PI; |
kbruil | 1:ffc1a2d6a824 | 122 | } |
kbruil | 1:ffc1a2d6a824 | 123 | else{ |
kbruil | 1:ffc1a2d6a824 | 124 | angle = angle + 1.5*M_PI; |
kbruil | 1:ffc1a2d6a824 | 125 | } |
kbruil | 1:ffc1a2d6a824 | 126 | if (y==0 && x<0) { angle = 0.5*M_PI; } |
kbruil | 0:86fe02a64f0e | 127 | |
kbruil | 0:86fe02a64f0e | 128 | z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z); |
kbruil | 0:86fe02a64f0e | 129 | z2 = z-z1; |
kbruil | 1:ffc1a2d6a824 | 130 | printf("z1 and z2: %f, %f\n\r", z1, z2); |
kbruil | 1:ffc1a2d6a824 | 131 | if (z1>arm1.length) {z1 = arm1.length;} |
kbruil | 1:ffc1a2d6a824 | 132 | else if (z1<-arm1.length) { z1 = -arm1.length;} |
kbruil | 1:ffc1a2d6a824 | 133 | if (z2>arm2.length) {z2 = arm2.length;} |
kbruil | 1:ffc1a2d6a824 | 134 | else if (z2<-arm2.length) { z2 = -arm2.length;} |
kbruil | 0:86fe02a64f0e | 135 | |
kbruil | 0:86fe02a64f0e | 136 | b1 = acos(z1/arm1.length); |
kbruil | 0:86fe02a64f0e | 137 | b2 = acos(z2/arm2.length); |
kbruil | 0:86fe02a64f0e | 138 | |
kbruil | 0:86fe02a64f0e | 139 | old = arm1.theta; |
kbruil | 0:86fe02a64f0e | 140 | arm1.theta = angle - b1; |
kbruil | 0:86fe02a64f0e | 141 | |
kbruil | 1:ffc1a2d6a824 | 142 | if (arm1.theta>ARM1_ALIMLO){ |
kbruil | 0:86fe02a64f0e | 143 | arm1.theta = ARM1_ALIMLO; |
kbruil | 0:86fe02a64f0e | 144 | } |
kbruil | 0:86fe02a64f0e | 145 | else if (arm1.theta<ARM1_ALIMHI){ |
kbruil | 0:86fe02a64f0e | 146 | arm1.theta = ARM1_ALIMHI; |
kbruil | 0:86fe02a64f0e | 147 | } |
kbruil | 1:ffc1a2d6a824 | 148 | |
kbruil | 0:86fe02a64f0e | 149 | arm1.omega = arm1.theta - old; |
kbruil | 0:86fe02a64f0e | 150 | old = arm2.theta; |
kbruil | 0:86fe02a64f0e | 151 | arm2.theta = b1 + b2; |
kbruil | 0:86fe02a64f0e | 152 | if (arm2.theta>ARM2_ALIMHI){ |
kbruil | 0:86fe02a64f0e | 153 | arm2.theta = ARM2_ALIMHI; |
kbruil | 1:ffc1a2d6a824 | 154 | } else if (arm2.theta < 0) { |
kbruil | 1:ffc1a2d6a824 | 155 | arm2.theta = 0; |
kbruil | 0:86fe02a64f0e | 156 | } |
kbruil | 1:ffc1a2d6a824 | 157 | |
kbruil | 1:ffc1a2d6a824 | 158 | /* |
kbruil | 0:86fe02a64f0e | 159 | arm2.omega = arm2.theta - old; |
kbruil | 0:86fe02a64f0e | 160 | old = arm2.x; |
kbruil | 0:86fe02a64f0e | 161 | arm2.x = arm1.length*-sin(arm1.theta); |
kbruil | 0:86fe02a64f0e | 162 | arm2.vx = arm2.x - old; |
kbruil | 0:86fe02a64f0e | 163 | old = arm2.y; |
kbruil | 0:86fe02a64f0e | 164 | arm2.y = arm1.length*cos(arm1.theta); |
kbruil | 0:86fe02a64f0e | 165 | arm2.vy = arm2.y - old; |
kbruil | 0:86fe02a64f0e | 166 | |
kbruil | 0:86fe02a64f0e | 167 | old = end.x; |
kbruil | 0:86fe02a64f0e | 168 | end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; |
kbruil | 0:86fe02a64f0e | 169 | end.vx = end.x - old; |
kbruil | 0:86fe02a64f0e | 170 | old = end.y; |
kbruil | 0:86fe02a64f0e | 171 | end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y; |
kbruil | 0:86fe02a64f0e | 172 | end.vy = old - end.y; |
kbruil | 1:ffc1a2d6a824 | 173 | */ |
kbruil | 1:ffc1a2d6a824 | 174 | end.x = x; |
kbruil | 1:ffc1a2d6a824 | 175 | end.y = y; |
kbruil | 1:ffc1a2d6a824 | 176 | |
kbruil | 0:86fe02a64f0e | 177 | } |
kbruil | 0:86fe02a64f0e | 178 | |
kbruil | 0:86fe02a64f0e | 179 | // robot_init |
kbruil | 0:86fe02a64f0e | 180 | // Initialise robot |
kbruil | 0:86fe02a64f0e | 181 | void robot_init() { |
kbruil | 0:86fe02a64f0e | 182 | // Init arm1 (upper arm) first link |
kbruil | 0:86fe02a64f0e | 183 | arm1.length = 28.0f; |
kbruil | 0:86fe02a64f0e | 184 | arm1.x = 0; arm1.y = 0; |
kbruil | 0:86fe02a64f0e | 185 | arm1.vx = 0; arm1.vy = 0; |
kbruil | 0:86fe02a64f0e | 186 | arm1.ax = 0; arm1.ay = 0; |
kbruil | 1:ffc1a2d6a824 | 187 | arm1.theta = 0; |
kbruil | 0:86fe02a64f0e | 188 | arm1.omega = 0; |
kbruil | 0:86fe02a64f0e | 189 | arm1.alpha = 0; |
kbruil | 0:86fe02a64f0e | 190 | |
kbruil | 0:86fe02a64f0e | 191 | // Init arm2 (lower arm) second link |
kbruil | 0:86fe02a64f0e | 192 | arm2.length = 35.0f; |
kbruil | 0:86fe02a64f0e | 193 | arm2.x = 0; arm2.y = arm1.length; |
kbruil | 0:86fe02a64f0e | 194 | arm2.vx = 0; arm2.vy = 0; |
kbruil | 0:86fe02a64f0e | 195 | arm2.ax = 0; arm2.ay = 0; |
kbruil | 1:ffc1a2d6a824 | 196 | arm2.theta = 0; |
kbruil | 0:86fe02a64f0e | 197 | arm2.omega = 0; |
kbruil | 0:86fe02a64f0e | 198 | arm2.alpha = 0; |
kbruil | 0:86fe02a64f0e | 199 | |
kbruil | 0:86fe02a64f0e | 200 | // Init end (end effector) third/last link |
kbruil | 0:86fe02a64f0e | 201 | end.length = 30.0f; |
kbruil | 0:86fe02a64f0e | 202 | end.x = 0; end.y = arm1.length+arm2.length; |
kbruil | 0:86fe02a64f0e | 203 | end.vx = 0; end.vy = 0; |
kbruil | 0:86fe02a64f0e | 204 | end.ax = 0; end.ay = 0; |
kbruil | 0:86fe02a64f0e | 205 | end.theta = 0; |
kbruil | 0:86fe02a64f0e | 206 | end.omega = 0; |
kbruil | 0:86fe02a64f0e | 207 | end.alpha = 0; |
kbruil | 0:86fe02a64f0e | 208 | } |
kbruil | 0:86fe02a64f0e | 209 | |
kbruil | 0:86fe02a64f0e | 210 | int lr_state=0; |
kbruil | 0:86fe02a64f0e | 211 | int ud_state=0; |
kbruil | 0:86fe02a64f0e | 212 | int sw2_state=0; |
kbruil | 0:86fe02a64f0e | 213 | float ax=0.1; |
kbruil | 0:86fe02a64f0e | 214 | float ay=0.1; |
kbruil | 0:86fe02a64f0e | 215 | float vx=0; |
kbruil | 0:86fe02a64f0e | 216 | float vy=0; |
kbruil | 1:ffc1a2d6a824 | 217 | float spd=1.0; |
kbruil | 0:86fe02a64f0e | 218 | |
kbruil | 0:86fe02a64f0e | 219 | void inputswitches() |
kbruil | 0:86fe02a64f0e | 220 | { |
kbruil | 1:ffc1a2d6a824 | 221 | /* if(switch1.read()) |
kbruil | 0:86fe02a64f0e | 222 | { |
kbruil | 0:86fe02a64f0e | 223 | if (lr_state==0){ |
kbruil | 0:86fe02a64f0e | 224 | vx=0; |
kbruil | 0:86fe02a64f0e | 225 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 226 | } |
kbruil | 0:86fe02a64f0e | 227 | } |
kbruil | 0:86fe02a64f0e | 228 | else |
kbruil | 0:86fe02a64f0e | 229 | { |
kbruil | 0:86fe02a64f0e | 230 | lr_state=0; |
kbruil | 0:86fe02a64f0e | 231 | vx = vx - ax; |
kbruil | 0:86fe02a64f0e | 232 | if (vx<-3.0f){ |
kbruil | 0:86fe02a64f0e | 233 | vx=-3.0f; |
kbruil | 0:86fe02a64f0e | 234 | } |
kbruil | 0:86fe02a64f0e | 235 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 236 | } |
kbruil | 0:86fe02a64f0e | 237 | |
kbruil | 0:86fe02a64f0e | 238 | if(switch2.read()) |
kbruil | 0:86fe02a64f0e | 239 | { |
kbruil | 0:86fe02a64f0e | 240 | if(lr_state==1){ |
kbruil | 0:86fe02a64f0e | 241 | vx=0; |
kbruil | 0:86fe02a64f0e | 242 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 243 | } |
kbruil | 0:86fe02a64f0e | 244 | } |
kbruil | 0:86fe02a64f0e | 245 | else |
kbruil | 0:86fe02a64f0e | 246 | { |
kbruil | 0:86fe02a64f0e | 247 | lr_state=1; |
kbruil | 0:86fe02a64f0e | 248 | vx = vx + ax; |
kbruil | 0:86fe02a64f0e | 249 | if (vx>3.0f){ |
kbruil | 0:86fe02a64f0e | 250 | vx=3.0f; |
kbruil | 0:86fe02a64f0e | 251 | } |
kbruil | 0:86fe02a64f0e | 252 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 253 | } |
kbruil | 0:86fe02a64f0e | 254 | if(switch3.read()) |
kbruil | 0:86fe02a64f0e | 255 | { |
kbruil | 0:86fe02a64f0e | 256 | if (ud_state==0){ |
kbruil | 0:86fe02a64f0e | 257 | vy=0; |
kbruil | 0:86fe02a64f0e | 258 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 259 | } |
kbruil | 0:86fe02a64f0e | 260 | } |
kbruil | 0:86fe02a64f0e | 261 | else |
kbruil | 0:86fe02a64f0e | 262 | { |
kbruil | 0:86fe02a64f0e | 263 | ud_state=0; |
kbruil | 0:86fe02a64f0e | 264 | vy = vy - ay; |
kbruil | 0:86fe02a64f0e | 265 | if (vy<-3.0f){ |
kbruil | 0:86fe02a64f0e | 266 | vy=-3.0f; |
kbruil | 0:86fe02a64f0e | 267 | } |
kbruil | 0:86fe02a64f0e | 268 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 269 | } |
kbruil | 0:86fe02a64f0e | 270 | |
kbruil | 0:86fe02a64f0e | 271 | if(switch4.read()) |
kbruil | 0:86fe02a64f0e | 272 | { |
kbruil | 0:86fe02a64f0e | 273 | if(ud_state==1){ |
kbruil | 0:86fe02a64f0e | 274 | vy=0; |
kbruil | 0:86fe02a64f0e | 275 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 276 | } |
kbruil | 0:86fe02a64f0e | 277 | } |
kbruil | 0:86fe02a64f0e | 278 | else |
kbruil | 0:86fe02a64f0e | 279 | { |
kbruil | 0:86fe02a64f0e | 280 | ud_state=1; |
kbruil | 0:86fe02a64f0e | 281 | vy = vy + ay; |
kbruil | 0:86fe02a64f0e | 282 | if (vy>3.0f){ |
kbruil | 0:86fe02a64f0e | 283 | vy=3.0f; |
kbruil | 0:86fe02a64f0e | 284 | } |
kbruil | 0:86fe02a64f0e | 285 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 286 | } |
kbruil | 1:ffc1a2d6a824 | 287 | */ |
kbruil | 1:ffc1a2d6a824 | 288 | if (switch1.read()){ |
kbruil | 1:ffc1a2d6a824 | 289 | end.x += spd; |
kbruil | 1:ffc1a2d6a824 | 290 | } |
kbruil | 1:ffc1a2d6a824 | 291 | if (switch2.read()){ |
kbruil | 1:ffc1a2d6a824 | 292 | end.x -= spd; |
kbruil | 1:ffc1a2d6a824 | 293 | } |
kbruil | 1:ffc1a2d6a824 | 294 | if (switch3.read()){ |
kbruil | 1:ffc1a2d6a824 | 295 | end.y += spd; |
kbruil | 1:ffc1a2d6a824 | 296 | } |
kbruil | 1:ffc1a2d6a824 | 297 | if (switch4.read()){ |
kbruil | 1:ffc1a2d6a824 | 298 | end.y -= spd; |
kbruil | 1:ffc1a2d6a824 | 299 | } |
kbruil | 0:86fe02a64f0e | 300 | } |
kbruil | 0:86fe02a64f0e | 301 | |
kbruil | 0:86fe02a64f0e | 302 | int main(){ |
kbruil | 0:86fe02a64f0e | 303 | pc.baud(115200); |
kbruil | 0:86fe02a64f0e | 304 | robot_init(); |
kbruil | 0:86fe02a64f0e | 305 | while(true){ |
kbruil | 0:86fe02a64f0e | 306 | inputswitches(); |
kbruil | 1:ffc1a2d6a824 | 307 | robot_setposition(end.x,end.y); |
kbruil | 0:86fe02a64f0e | 308 | printf("Angle1\n\r"); |
kbruil | 0:86fe02a64f0e | 309 | printf("%f\n\r", arm1.theta); |
kbruil | 0:86fe02a64f0e | 310 | printf("Angle2\n\r"); |
kbruil | 0:86fe02a64f0e | 311 | printf("%f\n\r", arm2.theta); |
kbruil | 0:86fe02a64f0e | 312 | printf("x: %f | y: %f\n\r", end.x, end.y); |
kbruil | 1:ffc1a2d6a824 | 313 | // printf("%f || %f", ARM1_ALIMLO, ARM1_ALIMHI); |
kbruil | 0:86fe02a64f0e | 314 | |
kbruil | 0:86fe02a64f0e | 315 | // for(int i=0;i<30;i++) |
kbruil | 0:86fe02a64f0e | 316 | // { |
kbruil | 0:86fe02a64f0e | 317 | // printf("\n"); |
kbruil | 0:86fe02a64f0e | 318 | // } |
kbruil | 0:86fe02a64f0e | 319 | /* |
kbruil | 0:86fe02a64f0e | 320 | printf("speed %f\n\r",vx); |
kbruil | 0:86fe02a64f0e | 321 | printf("end(x,y,theta) %f.2 : %f.2 : %f.2\n\r",end.x, end.y, arm2.theta); |
kbruil | 0:86fe02a64f0e | 322 | printf("arm2(x,y,theta) %f.2 : %f.2 : %f.2\n\r",arm2.x,arm2.y, arm1.theta); |
kbruil | 0:86fe02a64f0e | 323 | printf("end(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", end.vx, end.vy, arm2.omega); |
kbruil | 0:86fe02a64f0e | 324 | printf("arm2(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", arm2.vx, arm2.vy, arm1.omega); |
kbruil | 0:86fe02a64f0e | 325 | */ |
kbruil | 0:86fe02a64f0e | 326 | wait(0.1f); |
kbruil | 0:86fe02a64f0e | 327 | } |
kbruil | 0:86fe02a64f0e | 328 | } |