robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
main.cpp@3:339b6f68b317, 2016-10-25 (annotated)
- Committer:
- kbruil
- Date:
- Tue Oct 25 12:36:58 2016 +0000
- Revision:
- 3:339b6f68b317
- Parent:
- 2:027d19cda97b
- Child:
- 4:89f7d7f3a7ca
Bugs fixed, cleaning code
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 | 3:339b6f68b317 | 10 | const double R_XTABLE_LEFT = -30.0; // Robot in the middle, table width=60 |
kbruil | 3:339b6f68b317 | 11 | const double R_XTABLE_RIGHT = 30.0; |
kbruil | 3:339b6f68b317 | 12 | const double R_YTABLE_BOTTOM = -18.0; |
kbruil | 3:339b6f68b317 | 13 | const double R_YTABLE_TOP = 82.0; |
kbruil | 3:339b6f68b317 | 14 | const double R_ROBOT_LEFT = -8.0; // Robot limited from moving onto itself (20x20 base) |
kbruil | 3:339b6f68b317 | 15 | const double R_ROBOT_RIGHT = 8.0; |
kbruil | 3:339b6f68b317 | 16 | const double R_ROBOT_BOTTOM = -18.0; |
kbruil | 3:339b6f68b317 | 17 | const double R_ROBOT_TOP = 2.0; |
kbruil | 1:ffc1a2d6a824 | 18 | |
kbruil | 3:339b6f68b317 | 19 | const double ARM1_ALIMLO = (2*M_PI/360)*120; |
kbruil | 3:339b6f68b317 | 20 | const double ARM1_ALIMHI = -(2*M_PI/360)*120; |
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 | 2:027d19cda97b | 24 | float vx=0; |
kbruil | 2:027d19cda97b | 25 | float vy=0; |
kbruil | 0:86fe02a64f0e | 26 | // Struct r_link: |
kbruil | 0:86fe02a64f0e | 27 | // Defines a robot link type (arm or end effector). |
kbruil | 0:86fe02a64f0e | 28 | struct r_link { |
kbruil | 0:86fe02a64f0e | 29 | float length; // link length |
kbruil | 0:86fe02a64f0e | 30 | float x; // link x position |
kbruil | 0:86fe02a64f0e | 31 | float y; // link y position |
kbruil | 0:86fe02a64f0e | 32 | float vx; // link x speed |
kbruil | 0:86fe02a64f0e | 33 | float vy; // link y speed |
kbruil | 0:86fe02a64f0e | 34 | float ax; // link x accelleration |
kbruil | 0:86fe02a64f0e | 35 | float ay; // link y accelleration |
kbruil | 0:86fe02a64f0e | 36 | float theta; // link angle |
kbruil | 0:86fe02a64f0e | 37 | float omega; // link angular velocity |
kbruil | 0:86fe02a64f0e | 38 | float alpha; // link angular accelleration |
kbruil | 0:86fe02a64f0e | 39 | }; |
kbruil | 0:86fe02a64f0e | 40 | |
kbruil | 0:86fe02a64f0e | 41 | MODSERIAL pc(USBTX, USBRX); |
kbruil | 0:86fe02a64f0e | 42 | DigitalIn switch1(SW3); |
kbruil | 0:86fe02a64f0e | 43 | DigitalIn switch2(SW2); |
kbruil | 0:86fe02a64f0e | 44 | DigitalIn switch3(D7); |
kbruil | 0:86fe02a64f0e | 45 | DigitalIn switch4(D9); |
kbruil | 0:86fe02a64f0e | 46 | DigitalOut ledr(LED_RED); |
kbruil | 0:86fe02a64f0e | 47 | DigitalOut ledg(LED_GREEN); |
kbruil | 0:86fe02a64f0e | 48 | |
kbruil | 0:86fe02a64f0e | 49 | |
kbruil | 0:86fe02a64f0e | 50 | r_link arm1; // Robot upper arm |
kbruil | 0:86fe02a64f0e | 51 | r_link arm2; // Robot lower arm |
kbruil | 0:86fe02a64f0e | 52 | r_link end; // Robot end effector |
kbruil | 0:86fe02a64f0e | 53 | |
kbruil | 3:339b6f68b317 | 54 | /* |
kbruil | 3:339b6f68b317 | 55 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 56 | ** robot_applySetpointLimits |
kbruil | 3:339b6f68b317 | 57 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 58 | ** Function: |
kbruil | 3:339b6f68b317 | 59 | ** Limits the cartesian coordinates to table, length of robot and prevents |
kbruil | 3:339b6f68b317 | 60 | ** the robot arm crossing it's own base. |
kbruil | 3:339b6f68b317 | 61 | ** Input: |
kbruil | 3:339b6f68b317 | 62 | ** float x, float y: position in cartesian coordinates |
kbruil | 3:339b6f68b317 | 63 | ** Output: |
kbruil | 3:339b6f68b317 | 64 | ** float x, float y: limited position in cartesian coordinates |
kbruil | 3:339b6f68b317 | 65 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 66 | */ |
kbruil | 3:339b6f68b317 | 67 | void robot_applySetpointLimits (float& x, float& y){ |
kbruil | 0:86fe02a64f0e | 68 | float z; |
kbruil | 0:86fe02a64f0e | 69 | float angle; |
kbruil | 0:86fe02a64f0e | 70 | |
kbruil | 0:86fe02a64f0e | 71 | // Limit x,y to table top size |
kbruil | 3:339b6f68b317 | 72 | if (x<R_XTABLE_LEFT) { x=R_XTABLE_LEFT; vx=0;} |
kbruil | 3:339b6f68b317 | 73 | else if (x>R_XTABLE_RIGHT) { x=R_XTABLE_RIGHT; vx=0;} |
kbruil | 3:339b6f68b317 | 74 | if (y<R_YTABLE_BOTTOM) { y=R_YTABLE_BOTTOM; vy=0;} |
kbruil | 3:339b6f68b317 | 75 | else if (y>R_YTABLE_TOP) { y=R_YTABLE_TOP; vy=0; } |
kbruil | 3:339b6f68b317 | 76 | |
kbruil | 3:339b6f68b317 | 77 | // Limit x,y to maximum reach of robot arm |
kbruil | 0:86fe02a64f0e | 78 | z = sqrt(pow(x,2)+pow(y,2)); |
kbruil | 0:86fe02a64f0e | 79 | angle = atan2(y,x); |
kbruil | 0:86fe02a64f0e | 80 | if (z>(arm1.length+arm2.length)){ |
kbruil | 0:86fe02a64f0e | 81 | z = arm1.length+arm2.length; |
kbruil | 0:86fe02a64f0e | 82 | x = z*cos(angle); |
kbruil | 0:86fe02a64f0e | 83 | y = z*sin(angle); |
kbruil | 0:86fe02a64f0e | 84 | } |
kbruil | 3:339b6f68b317 | 85 | |
kbruil | 3:339b6f68b317 | 86 | // Prevent x,y from entering robot body |
kbruil | 3:339b6f68b317 | 87 | if (end.x > R_ROBOT_LEFT && end.x < R_ROBOT_RIGHT && end.y >= R_ROBOT_TOP && y < R_ROBOT_TOP) { |
kbruil | 3:339b6f68b317 | 88 | y = R_ROBOT_TOP; |
kbruil | 3:339b6f68b317 | 89 | } |
kbruil | 3:339b6f68b317 | 90 | if (end.x <= R_ROBOT_LEFT && x > R_ROBOT_LEFT && y < R_ROBOT_TOP) { |
kbruil | 3:339b6f68b317 | 91 | x = R_ROBOT_LEFT; |
kbruil | 3:339b6f68b317 | 92 | } |
kbruil | 3:339b6f68b317 | 93 | if (end.x >= R_ROBOT_RIGHT && x < R_ROBOT_RIGHT && y < R_ROBOT_TOP) { |
kbruil | 3:339b6f68b317 | 94 | x = R_ROBOT_RIGHT; |
kbruil | 3:339b6f68b317 | 95 | } |
kbruil | 0:86fe02a64f0e | 96 | } |
kbruil | 0:86fe02a64f0e | 97 | |
kbruil | 3:339b6f68b317 | 98 | /* |
kbruil | 3:339b6f68b317 | 99 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 100 | ** robot_setposition |
kbruil | 3:339b6f68b317 | 101 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 102 | ** Function: |
kbruil | 3:339b6f68b317 | 103 | ** Transforms cartesian coordinates into rotadions of the joints of the robot's |
kbruil | 3:339b6f68b317 | 104 | ** arms. |
kbruil | 3:339b6f68b317 | 105 | ** Input: |
kbruil | 3:339b6f68b317 | 106 | ** float x, float y: position in cartesian coordinates |
kbruil | 3:339b6f68b317 | 107 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 108 | */ |
kbruil | 3:339b6f68b317 | 109 | void robot_setSetpoint (float x,float y){ |
kbruil | 3:339b6f68b317 | 110 | /* |
kbruil | 3:339b6f68b317 | 111 | (x,y) |
kbruil | 3:339b6f68b317 | 112 | |\ |
kbruil | 3:339b6f68b317 | 113 | | \ |
kbruil | 3:339b6f68b317 | 114 | |b2\ |
kbruil | 3:339b6f68b317 | 115 | z2 | \arm2.length |
kbruil | 3:339b6f68b317 | 116 | | \ |
kbruil | 3:339b6f68b317 | 117 | z |_ _ _\ |
kbruil | 3:339b6f68b317 | 118 | | / |
kbruil | 3:339b6f68b317 | 119 | | / |
kbruil | 3:339b6f68b317 | 120 | z1 | /arm1.length |
kbruil | 3:339b6f68b317 | 121 | |b1/ |
kbruil | 3:339b6f68b317 | 122 | | / |
kbruil | 3:339b6f68b317 | 123 | |/ |
kbruil | 0:86fe02a64f0e | 124 | |
kbruil | 3:339b6f68b317 | 125 | */ |
kbruil | 3:339b6f68b317 | 126 | float z; // length z of vector (x,y) |
kbruil | 3:339b6f68b317 | 127 | float z1; // part z1 |
kbruil | 3:339b6f68b317 | 128 | float z2; // part z2 |
kbruil | 3:339b6f68b317 | 129 | |
kbruil | 3:339b6f68b317 | 130 | float angle; // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0! |
kbruil | 3:339b6f68b317 | 131 | float b1,b2; // angle b1 and b2 (see drawing) |
kbruil | 0:86fe02a64f0e | 132 | |
kbruil | 3:339b6f68b317 | 133 | float old; // 'old' helper variable used for calculating speeds |
kbruil | 3:339b6f68b317 | 134 | |
kbruil | 3:339b6f68b317 | 135 | |
kbruil | 3:339b6f68b317 | 136 | robot_applySetpointLimits(x,y); // Apply limits |
kbruil | 1:ffc1a2d6a824 | 137 | |
kbruil | 3:339b6f68b317 | 138 | z = sqrt(pow(x,2)+pow(y,2)); // Calculate (x,y) length |
kbruil | 3:339b6f68b317 | 139 | |
kbruil | 3:339b6f68b317 | 140 | // Calculate (x,y) angle |
kbruil | 3:339b6f68b317 | 141 | angle = atan2(y,x); // Calculate (x,y) angle using atan 2 |
kbruil | 3:339b6f68b317 | 142 | if (angle<0.5*M_PI && angle>0){ // Rotate result of atan2 90 degrees counter clockwise, so up pointing vector (x==0, y>0) has a zero degree rotation instead of 90 degrees. |
kbruil | 1:ffc1a2d6a824 | 143 | |
kbruil | 1:ffc1a2d6a824 | 144 | angle = angle - 0.5*M_PI; |
kbruil | 0:86fe02a64f0e | 145 | } |
kbruil | 1:ffc1a2d6a824 | 146 | else if (angle<M_PI && angle>0.5*M_PI){ |
kbruil | 1:ffc1a2d6a824 | 147 | angle = angle - 0.5*M_PI; |
kbruil | 0:86fe02a64f0e | 148 | } |
kbruil | 1:ffc1a2d6a824 | 149 | else if (angle<0 && angle>-0.5*M_PI){ |
kbruil | 1:ffc1a2d6a824 | 150 | angle = angle - 0.5 * M_PI; |
kbruil | 1:ffc1a2d6a824 | 151 | } |
kbruil | 1:ffc1a2d6a824 | 152 | else{ |
kbruil | 1:ffc1a2d6a824 | 153 | angle = angle + 1.5*M_PI; |
kbruil | 1:ffc1a2d6a824 | 154 | } |
kbruil | 3:339b6f68b317 | 155 | if (y==0 && x<0) { angle = 0.5*M_PI; } // Special case correction: y=0, x < 0 Normally this returns PI, now it must return PI/2 |
kbruil | 3:339b6f68b317 | 156 | if (y==0 && x>0) { angle = -0.5*M_PI; } // Special case correction: y=0, x > 0 Normally this returns -PI, now it must return -PI/2 |
kbruil | 2:027d19cda97b | 157 | |
kbruil | 3:339b6f68b317 | 158 | z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z); // Calculate z1 |
kbruil | 3:339b6f68b317 | 159 | z2 = z-z1; // Calculate z2 |
kbruil | 3:339b6f68b317 | 160 | // Roundoff errors sometimes make z1 and z2 slightly higher than the arm lengths, this causes erratic behaviour in the acos function, |
kbruil | 3:339b6f68b317 | 161 | // so it is countered here by an extra check and fix. |
kbruil | 1:ffc1a2d6a824 | 162 | if (z1>arm1.length) {z1 = arm1.length;} |
kbruil | 1:ffc1a2d6a824 | 163 | else if (z1<-arm1.length) { z1 = -arm1.length;} |
kbruil | 1:ffc1a2d6a824 | 164 | if (z2>arm2.length) {z2 = arm2.length;} |
kbruil | 1:ffc1a2d6a824 | 165 | else if (z2<-arm2.length) { z2 = -arm2.length;} |
kbruil | 0:86fe02a64f0e | 166 | |
kbruil | 3:339b6f68b317 | 167 | b1 = acos(z1/arm1.length); // Calculate angle b1 |
kbruil | 3:339b6f68b317 | 168 | b2 = acos(z2/arm2.length); // Calculate angle b2 |
kbruil | 0:86fe02a64f0e | 169 | |
kbruil | 3:339b6f68b317 | 170 | // Arm 1 angle and rotational speed |
kbruil | 3:339b6f68b317 | 171 | old = arm1.theta; // Preserve old angle arm 1 |
kbruil | 3:339b6f68b317 | 172 | arm1.theta = angle - b1; // Calculate new angle arm 1 |
kbruil | 0:86fe02a64f0e | 173 | |
kbruil | 3:339b6f68b317 | 174 | if (arm1.theta>ARM1_ALIMLO){ // Limit angle arm1 |
kbruil | 3:339b6f68b317 | 175 | arm1.theta = ARM1_ALIMLO; |
kbruil | 0:86fe02a64f0e | 176 | } |
kbruil | 0:86fe02a64f0e | 177 | else if (arm1.theta<ARM1_ALIMHI){ |
kbruil | 3:339b6f68b317 | 178 | arm1.theta = ARM1_ALIMHI; |
kbruil | 0:86fe02a64f0e | 179 | } |
kbruil | 1:ffc1a2d6a824 | 180 | |
kbruil | 3:339b6f68b317 | 181 | arm1.omega = arm1.theta - old; // Calculate rotational speed arm 1 |
kbruil | 3:339b6f68b317 | 182 | |
kbruil | 3:339b6f68b317 | 183 | // Arm 2 angle and rotational speed |
kbruil | 3:339b6f68b317 | 184 | old = arm2.theta; // Preserve old angle arm 2 |
kbruil | 3:339b6f68b317 | 185 | arm2.theta = b1 + b2; // Calculate new angle arm 2 |
kbruil | 3:339b6f68b317 | 186 | if (arm2.theta>ARM2_ALIMHI){ // Limit angle arm 2 |
kbruil | 0:86fe02a64f0e | 187 | arm2.theta = ARM2_ALIMHI; |
kbruil | 1:ffc1a2d6a824 | 188 | } else if (arm2.theta < 0) { |
kbruil | 1:ffc1a2d6a824 | 189 | arm2.theta = 0; |
kbruil | 0:86fe02a64f0e | 190 | } |
kbruil | 1:ffc1a2d6a824 | 191 | |
kbruil | 3:339b6f68b317 | 192 | arm2.omega = arm2.theta - old; // Calculate rotational speed arm 2 |
kbruil | 0:86fe02a64f0e | 193 | |
kbruil | 3:339b6f68b317 | 194 | // Arm 2 position |
kbruil | 3:339b6f68b317 | 195 | old = arm2.x; // Preserve old x position arm 2 |
kbruil | 3:339b6f68b317 | 196 | arm2.x = arm1.length*-sin(arm1.theta); // Calculate new x position arm 2 |
kbruil | 3:339b6f68b317 | 197 | arm2.vx = arm2.x - old; // Calculate new x speed arm 2 |
kbruil | 3:339b6f68b317 | 198 | old = arm2.y; // Preserve old y position arm 2 |
kbruil | 3:339b6f68b317 | 199 | arm2.y = arm1.length*cos(arm1.theta); // Calculate new y position arm 2 |
kbruil | 3:339b6f68b317 | 200 | arm2.vy = arm2.y - old; // Calculate new y speed arm 2 |
kbruil | 3:339b6f68b317 | 201 | |
kbruil | 3:339b6f68b317 | 202 | old = end.x; // Preserve old x position end effector |
kbruil | 3:339b6f68b317 | 203 | end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector |
kbruil | 3:339b6f68b317 | 204 | end.vx = end.x - old; // Calculate new x speed end effector |
kbruil | 3:339b6f68b317 | 205 | old = end.y; // Preserve old y position end effector |
kbruil | 3:339b6f68b317 | 206 | end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y; // Calculate new y position end effector |
kbruil | 3:339b6f68b317 | 207 | end.vy = old - end.y; // Calculate new y speed end effector |
kbruil | 1:ffc1a2d6a824 | 208 | |
kbruil | 0:86fe02a64f0e | 209 | } |
kbruil | 0:86fe02a64f0e | 210 | |
kbruil | 0:86fe02a64f0e | 211 | // robot_init |
kbruil | 0:86fe02a64f0e | 212 | // Initialise robot |
kbruil | 0:86fe02a64f0e | 213 | void robot_init() { |
kbruil | 0:86fe02a64f0e | 214 | // Init arm1 (upper arm) first link |
kbruil | 0:86fe02a64f0e | 215 | arm1.length = 28.0f; |
kbruil | 0:86fe02a64f0e | 216 | arm1.x = 0; arm1.y = 0; |
kbruil | 0:86fe02a64f0e | 217 | arm1.vx = 0; arm1.vy = 0; |
kbruil | 0:86fe02a64f0e | 218 | arm1.ax = 0; arm1.ay = 0; |
kbruil | 1:ffc1a2d6a824 | 219 | arm1.theta = 0; |
kbruil | 0:86fe02a64f0e | 220 | arm1.omega = 0; |
kbruil | 0:86fe02a64f0e | 221 | arm1.alpha = 0; |
kbruil | 0:86fe02a64f0e | 222 | |
kbruil | 0:86fe02a64f0e | 223 | // Init arm2 (lower arm) second link |
kbruil | 0:86fe02a64f0e | 224 | arm2.length = 35.0f; |
kbruil | 0:86fe02a64f0e | 225 | arm2.x = 0; arm2.y = arm1.length; |
kbruil | 0:86fe02a64f0e | 226 | arm2.vx = 0; arm2.vy = 0; |
kbruil | 0:86fe02a64f0e | 227 | arm2.ax = 0; arm2.ay = 0; |
kbruil | 1:ffc1a2d6a824 | 228 | arm2.theta = 0; |
kbruil | 0:86fe02a64f0e | 229 | arm2.omega = 0; |
kbruil | 0:86fe02a64f0e | 230 | arm2.alpha = 0; |
kbruil | 0:86fe02a64f0e | 231 | |
kbruil | 0:86fe02a64f0e | 232 | // Init end (end effector) third/last link |
kbruil | 0:86fe02a64f0e | 233 | end.length = 30.0f; |
kbruil | 0:86fe02a64f0e | 234 | end.x = 0; end.y = arm1.length+arm2.length; |
kbruil | 0:86fe02a64f0e | 235 | end.vx = 0; end.vy = 0; |
kbruil | 0:86fe02a64f0e | 236 | end.ax = 0; end.ay = 0; |
kbruil | 0:86fe02a64f0e | 237 | end.theta = 0; |
kbruil | 0:86fe02a64f0e | 238 | end.omega = 0; |
kbruil | 0:86fe02a64f0e | 239 | end.alpha = 0; |
kbruil | 0:86fe02a64f0e | 240 | } |
kbruil | 0:86fe02a64f0e | 241 | |
kbruil | 0:86fe02a64f0e | 242 | int lr_state=0; |
kbruil | 0:86fe02a64f0e | 243 | int ud_state=0; |
kbruil | 0:86fe02a64f0e | 244 | int sw2_state=0; |
kbruil | 0:86fe02a64f0e | 245 | float ax=0.1; |
kbruil | 0:86fe02a64f0e | 246 | float ay=0.1; |
kbruil | 2:027d19cda97b | 247 | |
kbruil | 1:ffc1a2d6a824 | 248 | float spd=1.0; |
kbruil | 0:86fe02a64f0e | 249 | |
kbruil | 0:86fe02a64f0e | 250 | void inputswitches() |
kbruil | 0:86fe02a64f0e | 251 | { |
kbruil | 2:027d19cda97b | 252 | if(switch1.read()) |
kbruil | 0:86fe02a64f0e | 253 | { |
kbruil | 0:86fe02a64f0e | 254 | if (lr_state==0){ |
kbruil | 0:86fe02a64f0e | 255 | vx=0; |
kbruil | 0:86fe02a64f0e | 256 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 257 | } |
kbruil | 0:86fe02a64f0e | 258 | } |
kbruil | 0:86fe02a64f0e | 259 | else |
kbruil | 0:86fe02a64f0e | 260 | { |
kbruil | 0:86fe02a64f0e | 261 | lr_state=0; |
kbruil | 0:86fe02a64f0e | 262 | vx = vx - ax; |
kbruil | 0:86fe02a64f0e | 263 | if (vx<-3.0f){ |
kbruil | 0:86fe02a64f0e | 264 | vx=-3.0f; |
kbruil | 0:86fe02a64f0e | 265 | } |
kbruil | 0:86fe02a64f0e | 266 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 267 | } |
kbruil | 0:86fe02a64f0e | 268 | |
kbruil | 0:86fe02a64f0e | 269 | if(switch2.read()) |
kbruil | 0:86fe02a64f0e | 270 | { |
kbruil | 0:86fe02a64f0e | 271 | if(lr_state==1){ |
kbruil | 0:86fe02a64f0e | 272 | vx=0; |
kbruil | 0:86fe02a64f0e | 273 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 274 | } |
kbruil | 0:86fe02a64f0e | 275 | } |
kbruil | 0:86fe02a64f0e | 276 | else |
kbruil | 0:86fe02a64f0e | 277 | { |
kbruil | 0:86fe02a64f0e | 278 | lr_state=1; |
kbruil | 0:86fe02a64f0e | 279 | vx = vx + ax; |
kbruil | 0:86fe02a64f0e | 280 | if (vx>3.0f){ |
kbruil | 0:86fe02a64f0e | 281 | vx=3.0f; |
kbruil | 0:86fe02a64f0e | 282 | } |
kbruil | 0:86fe02a64f0e | 283 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 284 | } |
kbruil | 0:86fe02a64f0e | 285 | if(switch3.read()) |
kbruil | 0:86fe02a64f0e | 286 | { |
kbruil | 0:86fe02a64f0e | 287 | if (ud_state==0){ |
kbruil | 0:86fe02a64f0e | 288 | vy=0; |
kbruil | 0:86fe02a64f0e | 289 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 290 | } |
kbruil | 0:86fe02a64f0e | 291 | } |
kbruil | 0:86fe02a64f0e | 292 | else |
kbruil | 0:86fe02a64f0e | 293 | { |
kbruil | 0:86fe02a64f0e | 294 | ud_state=0; |
kbruil | 0:86fe02a64f0e | 295 | vy = vy - ay; |
kbruil | 0:86fe02a64f0e | 296 | if (vy<-3.0f){ |
kbruil | 0:86fe02a64f0e | 297 | vy=-3.0f; |
kbruil | 0:86fe02a64f0e | 298 | } |
kbruil | 0:86fe02a64f0e | 299 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 300 | } |
kbruil | 0:86fe02a64f0e | 301 | |
kbruil | 0:86fe02a64f0e | 302 | if(switch4.read()) |
kbruil | 0:86fe02a64f0e | 303 | { |
kbruil | 0:86fe02a64f0e | 304 | if(ud_state==1){ |
kbruil | 0:86fe02a64f0e | 305 | vy=0; |
kbruil | 0:86fe02a64f0e | 306 | ledr.write(1); |
kbruil | 0:86fe02a64f0e | 307 | } |
kbruil | 0:86fe02a64f0e | 308 | } |
kbruil | 0:86fe02a64f0e | 309 | else |
kbruil | 0:86fe02a64f0e | 310 | { |
kbruil | 0:86fe02a64f0e | 311 | ud_state=1; |
kbruil | 0:86fe02a64f0e | 312 | vy = vy + ay; |
kbruil | 0:86fe02a64f0e | 313 | if (vy>3.0f){ |
kbruil | 0:86fe02a64f0e | 314 | vy=3.0f; |
kbruil | 0:86fe02a64f0e | 315 | } |
kbruil | 0:86fe02a64f0e | 316 | ledr.write(0); |
kbruil | 0:86fe02a64f0e | 317 | } |
kbruil | 2:027d19cda97b | 318 | /* |
kbruil | 1:ffc1a2d6a824 | 319 | if (switch1.read()){ |
kbruil | 1:ffc1a2d6a824 | 320 | end.x += spd; |
kbruil | 1:ffc1a2d6a824 | 321 | } |
kbruil | 1:ffc1a2d6a824 | 322 | if (switch2.read()){ |
kbruil | 1:ffc1a2d6a824 | 323 | end.x -= spd; |
kbruil | 1:ffc1a2d6a824 | 324 | } |
kbruil | 1:ffc1a2d6a824 | 325 | if (switch3.read()){ |
kbruil | 1:ffc1a2d6a824 | 326 | end.y += spd; |
kbruil | 1:ffc1a2d6a824 | 327 | } |
kbruil | 1:ffc1a2d6a824 | 328 | if (switch4.read()){ |
kbruil | 1:ffc1a2d6a824 | 329 | end.y -= spd; |
kbruil | 1:ffc1a2d6a824 | 330 | } |
kbruil | 2:027d19cda97b | 331 | */ |
kbruil | 0:86fe02a64f0e | 332 | } |
kbruil | 0:86fe02a64f0e | 333 | |
kbruil | 0:86fe02a64f0e | 334 | int main(){ |
kbruil | 0:86fe02a64f0e | 335 | pc.baud(115200); |
kbruil | 0:86fe02a64f0e | 336 | robot_init(); |
kbruil | 0:86fe02a64f0e | 337 | while(true){ |
kbruil | 0:86fe02a64f0e | 338 | inputswitches(); |
kbruil | 3:339b6f68b317 | 339 | robot_setSetpoint(end.x+vx,end.y+vy); |
kbruil | 0:86fe02a64f0e | 340 | printf("Angle1\n\r"); |
kbruil | 0:86fe02a64f0e | 341 | printf("%f\n\r", arm1.theta); |
kbruil | 0:86fe02a64f0e | 342 | printf("Angle2\n\r"); |
kbruil | 0:86fe02a64f0e | 343 | printf("%f\n\r", arm2.theta); |
kbruil | 0:86fe02a64f0e | 344 | printf("x: %f | y: %f\n\r", end.x, end.y); |
kbruil | 1:ffc1a2d6a824 | 345 | // printf("%f || %f", ARM1_ALIMLO, ARM1_ALIMHI); |
kbruil | 0:86fe02a64f0e | 346 | |
kbruil | 0:86fe02a64f0e | 347 | // for(int i=0;i<30;i++) |
kbruil | 0:86fe02a64f0e | 348 | // { |
kbruil | 0:86fe02a64f0e | 349 | // printf("\n"); |
kbruil | 0:86fe02a64f0e | 350 | // } |
kbruil | 0:86fe02a64f0e | 351 | /* |
kbruil | 0:86fe02a64f0e | 352 | printf("speed %f\n\r",vx); |
kbruil | 0:86fe02a64f0e | 353 | printf("end(x,y,theta) %f.2 : %f.2 : %f.2\n\r",end.x, end.y, arm2.theta); |
kbruil | 0:86fe02a64f0e | 354 | printf("arm2(x,y,theta) %f.2 : %f.2 : %f.2\n\r",arm2.x,arm2.y, arm1.theta); |
kbruil | 0:86fe02a64f0e | 355 | printf("end(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", end.vx, end.vy, arm2.omega); |
kbruil | 0:86fe02a64f0e | 356 | printf("arm2(vx, vy, theta) %f.2 : %f.2 : %f.2\n\r", arm2.vx, arm2.vy, arm1.omega); |
kbruil | 0:86fe02a64f0e | 357 | */ |
kbruil | 0:86fe02a64f0e | 358 | wait(0.1f); |
kbruil | 0:86fe02a64f0e | 359 | } |
kbruil | 0:86fe02a64f0e | 360 | } |