robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
main.cpp@11:4382c567e0d4, 2016-10-28 (annotated)
- Committer:
- kbruil
- Date:
- Fri Oct 28 07:35:36 2016 +0000
- Revision:
- 11:4382c567e0d4
- Parent:
- 10:27fdd43f3b85
- Child:
- 12:e591729e854a
Opgeschoond, motoren eigen class, basis voor EMG input
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 | 4:89f7d7f3a7ca | 5 | #include "QEI.h" |
kbruil | 5:ca4f81348c30 | 6 | #include "BiQuad.h" |
kbruil | 11:4382c567e0d4 | 7 | #include "motor.h" |
kbruil | 0:86fe02a64f0e | 8 | // Angle limits 215, 345 lower arm, 0, 145 upper arm |
kbruil | 0:86fe02a64f0e | 9 | // Robot arm x,y limits (limit to table top) |
kbruil | 0:86fe02a64f0e | 10 | |
kbruil | 0:86fe02a64f0e | 11 | |
kbruil | 0:86fe02a64f0e | 12 | #define M_PI 3.141592653589793238462643383279502884L // Pi |
kbruil | 10:27fdd43f3b85 | 13 | |
kbruil | 10:27fdd43f3b85 | 14 | /* |
kbruil | 10:27fdd43f3b85 | 15 | ** State variable |
kbruil | 10:27fdd43f3b85 | 16 | */ |
kbruil | 4:89f7d7f3a7ca | 17 | enum r_states{R_INIT, R_HORIZONTAL, R_VERTICAL, R_GRIPPER}; |
kbruil | 4:89f7d7f3a7ca | 18 | r_states state; |
kbruil | 4:89f7d7f3a7ca | 19 | |
kbruil | 4:89f7d7f3a7ca | 20 | /* |
kbruil | 10:27fdd43f3b85 | 21 | ** Table surface and robot body limits |
kbruil | 4:89f7d7f3a7ca | 22 | */ |
kbruil | 3:339b6f68b317 | 23 | const double R_XTABLE_LEFT = -30.0; // Robot in the middle, table width=60 |
kbruil | 3:339b6f68b317 | 24 | const double R_XTABLE_RIGHT = 30.0; |
kbruil | 3:339b6f68b317 | 25 | const double R_YTABLE_BOTTOM = -18.0; |
kbruil | 3:339b6f68b317 | 26 | const double R_YTABLE_TOP = 82.0; |
kbruil | 3:339b6f68b317 | 27 | const double R_ROBOT_LEFT = -8.0; // Robot limited from moving onto itself (20x20 base) |
kbruil | 3:339b6f68b317 | 28 | const double R_ROBOT_RIGHT = 8.0; |
kbruil | 3:339b6f68b317 | 29 | const double R_ROBOT_BOTTOM = -18.0; |
kbruil | 3:339b6f68b317 | 30 | const double R_ROBOT_TOP = 2.0; |
kbruil | 1:ffc1a2d6a824 | 31 | |
kbruil | 4:89f7d7f3a7ca | 32 | /* |
kbruil | 4:89f7d7f3a7ca | 33 | ** Robot arm angle limits |
kbruil | 4:89f7d7f3a7ca | 34 | */ |
kbruil | 4:89f7d7f3a7ca | 35 | const double ARM1_ALIMLO = (2*M_PI/360)*125; |
kbruil | 4:89f7d7f3a7ca | 36 | const double ARM1_ALIMHI = -(2*M_PI/360)*125; |
kbruil | 1:ffc1a2d6a824 | 37 | const double ARM2_ALIMLO = 0; |
kbruil | 1:ffc1a2d6a824 | 38 | const double ARM2_ALIMHI = (2*M_PI/360)*145; |
kbruil | 0:86fe02a64f0e | 39 | |
kbruil | 4:89f7d7f3a7ca | 40 | /* |
kbruil | 10:27fdd43f3b85 | 41 | ** Ticker variables |
kbruil | 4:89f7d7f3a7ca | 42 | */ |
kbruil | 4:89f7d7f3a7ca | 43 | bool flag_switch; |
kbruil | 4:89f7d7f3a7ca | 44 | bool flag_PID; |
kbruil | 4:89f7d7f3a7ca | 45 | bool flag_output; |
kbruil | 11:4382c567e0d4 | 46 | bool flag_EMG; |
kbruil | 4:89f7d7f3a7ca | 47 | |
kbruil | 4:89f7d7f3a7ca | 48 | Ticker mainticker; // Main ticker |
kbruil | 4:89f7d7f3a7ca | 49 | |
kbruil | 4:89f7d7f3a7ca | 50 | /* |
kbruil | 4:89f7d7f3a7ca | 51 | ** Motor variables |
kbruil | 4:89f7d7f3a7ca | 52 | */ |
kbruil | 11:4382c567e0d4 | 53 | motor motor1(D6, D7, D13, D12, 0.8, 0.2, 0.1, true); |
kbruil | 11:4382c567e0d4 | 54 | motor motor2(D5, D4, D10, D11, 0.8, 0.2, 0.1, false); |
kbruil | 11:4382c567e0d4 | 55 | |
kbruil | 9:19dafcb4a098 | 56 | /* |
kbruil | 10:27fdd43f3b85 | 57 | ** Gripper variables |
kbruil | 9:19dafcb4a098 | 58 | */ |
kbruil | 10:27fdd43f3b85 | 59 | PwmOut gripperpwm(D3); // Gripper pwm port |
kbruil | 10:27fdd43f3b85 | 60 | bool gripperclosed=true; // Gripper open/close flag |
kbruil | 4:89f7d7f3a7ca | 61 | |
kbruil | 11:4382c567e0d4 | 62 | /* |
kbruil | 11:4382c567e0d4 | 63 | ** Setpoint variables |
kbruil | 11:4382c567e0d4 | 64 | */ |
kbruil | 10:27fdd43f3b85 | 65 | float vx=0; // Setpoint x speed |
kbruil | 10:27fdd43f3b85 | 66 | float vy=0; // Setpoint y speed |
kbruil | 10:27fdd43f3b85 | 67 | float ax=0.01; // Setpoint accelleration x direction |
kbruil | 10:27fdd43f3b85 | 68 | float ay=0.01; // Setpoint accelleration y diraction |
kbruil | 11:4382c567e0d4 | 69 | const double maxspeed=0.5f; // Setpoint maximum speed |
kbruil | 10:27fdd43f3b85 | 70 | bool moveleft; // Flag set to true if setpoint moving left |
kbruil | 10:27fdd43f3b85 | 71 | bool movedown; // Flag set to true if setpoint moving down |
kbruil | 10:27fdd43f3b85 | 72 | |
kbruil | 10:27fdd43f3b85 | 73 | |
kbruil | 0:86fe02a64f0e | 74 | // Struct r_link: |
kbruil | 10:27fdd43f3b85 | 75 | // Defines a robot link (arm or end effector). |
kbruil | 0:86fe02a64f0e | 76 | struct r_link { |
kbruil | 0:86fe02a64f0e | 77 | float length; // link length |
kbruil | 0:86fe02a64f0e | 78 | float x; // link x position |
kbruil | 0:86fe02a64f0e | 79 | float y; // link y position |
kbruil | 0:86fe02a64f0e | 80 | float theta; // link angle |
kbruil | 0:86fe02a64f0e | 81 | }; |
kbruil | 0:86fe02a64f0e | 82 | |
kbruil | 0:86fe02a64f0e | 83 | MODSERIAL pc(USBTX, USBRX); |
kbruil | 0:86fe02a64f0e | 84 | DigitalIn switch1(SW3); |
kbruil | 0:86fe02a64f0e | 85 | DigitalIn switch2(SW2); |
kbruil | 6:3dcd9ce47900 | 86 | DigitalIn switch3(D9); |
kbruil | 6:3dcd9ce47900 | 87 | bool switch3down=false; |
kbruil | 0:86fe02a64f0e | 88 | DigitalOut ledr(LED_RED); |
kbruil | 0:86fe02a64f0e | 89 | DigitalOut ledg(LED_GREEN); |
kbruil | 0:86fe02a64f0e | 90 | |
kbruil | 0:86fe02a64f0e | 91 | |
kbruil | 0:86fe02a64f0e | 92 | r_link arm1; // Robot upper arm |
kbruil | 0:86fe02a64f0e | 93 | r_link arm2; // Robot lower arm |
kbruil | 0:86fe02a64f0e | 94 | r_link end; // Robot end effector |
kbruil | 0:86fe02a64f0e | 95 | |
kbruil | 3:339b6f68b317 | 96 | /* |
kbruil | 3:339b6f68b317 | 97 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 98 | ** robot_applySetpointLimits |
kbruil | 3:339b6f68b317 | 99 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 100 | ** Function: |
kbruil | 3:339b6f68b317 | 101 | ** Limits the cartesian coordinates to table, length of robot and prevents |
kbruil | 3:339b6f68b317 | 102 | ** the robot arm crossing it's own base. |
kbruil | 3:339b6f68b317 | 103 | ** Input: |
kbruil | 3:339b6f68b317 | 104 | ** float x, float y: position in cartesian coordinates |
kbruil | 3:339b6f68b317 | 105 | ** Output: |
kbruil | 3:339b6f68b317 | 106 | ** float x, float y: limited position in cartesian coordinates |
kbruil | 3:339b6f68b317 | 107 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 108 | */ |
kbruil | 3:339b6f68b317 | 109 | void robot_applySetpointLimits (float& x, float& y){ |
kbruil | 0:86fe02a64f0e | 110 | float z; |
kbruil | 0:86fe02a64f0e | 111 | float angle; |
kbruil | 0:86fe02a64f0e | 112 | |
kbruil | 0:86fe02a64f0e | 113 | // Limit x,y to table top size |
kbruil | 3:339b6f68b317 | 114 | if (x<R_XTABLE_LEFT) { x=R_XTABLE_LEFT; vx=0;} |
kbruil | 3:339b6f68b317 | 115 | else if (x>R_XTABLE_RIGHT) { x=R_XTABLE_RIGHT; vx=0;} |
kbruil | 3:339b6f68b317 | 116 | if (y<R_YTABLE_BOTTOM) { y=R_YTABLE_BOTTOM; vy=0;} |
kbruil | 3:339b6f68b317 | 117 | else if (y>R_YTABLE_TOP) { y=R_YTABLE_TOP; vy=0; } |
kbruil | 3:339b6f68b317 | 118 | |
kbruil | 3:339b6f68b317 | 119 | // Limit x,y to maximum reach of robot arm |
kbruil | 0:86fe02a64f0e | 120 | z = sqrt(pow(x,2)+pow(y,2)); |
kbruil | 0:86fe02a64f0e | 121 | angle = atan2(y,x); |
kbruil | 0:86fe02a64f0e | 122 | if (z>(arm1.length+arm2.length)){ |
kbruil | 0:86fe02a64f0e | 123 | z = arm1.length+arm2.length; |
kbruil | 0:86fe02a64f0e | 124 | x = z*cos(angle); |
kbruil | 0:86fe02a64f0e | 125 | y = z*sin(angle); |
kbruil | 0:86fe02a64f0e | 126 | } |
kbruil | 3:339b6f68b317 | 127 | |
kbruil | 3:339b6f68b317 | 128 | // Prevent x,y from entering robot body |
kbruil | 3:339b6f68b317 | 129 | if (end.x > R_ROBOT_LEFT && end.x < R_ROBOT_RIGHT && end.y >= R_ROBOT_TOP && y < R_ROBOT_TOP) { |
kbruil | 3:339b6f68b317 | 130 | y = R_ROBOT_TOP; |
kbruil | 3:339b6f68b317 | 131 | } |
kbruil | 3:339b6f68b317 | 132 | if (end.x <= R_ROBOT_LEFT && x > R_ROBOT_LEFT && y < R_ROBOT_TOP) { |
kbruil | 3:339b6f68b317 | 133 | x = R_ROBOT_LEFT; |
kbruil | 3:339b6f68b317 | 134 | } |
kbruil | 3:339b6f68b317 | 135 | if (end.x >= R_ROBOT_RIGHT && x < R_ROBOT_RIGHT && y < R_ROBOT_TOP) { |
kbruil | 3:339b6f68b317 | 136 | x = R_ROBOT_RIGHT; |
kbruil | 3:339b6f68b317 | 137 | } |
kbruil | 0:86fe02a64f0e | 138 | } |
kbruil | 0:86fe02a64f0e | 139 | |
kbruil | 3:339b6f68b317 | 140 | /* |
kbruil | 3:339b6f68b317 | 141 | ** ============================================================================= |
kbruil | 4:89f7d7f3a7ca | 142 | ** robot_applyAngleLimits |
kbruil | 4:89f7d7f3a7ca | 143 | ** ============================================================================= |
kbruil | 4:89f7d7f3a7ca | 144 | ** Function: |
kbruil | 4:89f7d7f3a7ca | 145 | ** Limits the angles of the robot's arms to prevent them from moving into |
kbruil | 4:89f7d7f3a7ca | 146 | ** themselves |
kbruil | 4:89f7d7f3a7ca | 147 | ** Input: - |
kbruil | 4:89f7d7f3a7ca | 148 | ** Output: - |
kbruil | 4:89f7d7f3a7ca | 149 | ** ============================================================================= |
kbruil | 4:89f7d7f3a7ca | 150 | */ |
kbruil | 4:89f7d7f3a7ca | 151 | void robot_applyAngleLimits(){ |
kbruil | 4:89f7d7f3a7ca | 152 | if (arm1.theta>ARM1_ALIMLO){ // Limit angle arm1 |
kbruil | 4:89f7d7f3a7ca | 153 | arm1.theta = ARM1_ALIMLO; |
kbruil | 4:89f7d7f3a7ca | 154 | } |
kbruil | 4:89f7d7f3a7ca | 155 | else if (arm1.theta<ARM1_ALIMHI){ |
kbruil | 4:89f7d7f3a7ca | 156 | arm1.theta = ARM1_ALIMHI; |
kbruil | 4:89f7d7f3a7ca | 157 | } |
kbruil | 4:89f7d7f3a7ca | 158 | |
kbruil | 4:89f7d7f3a7ca | 159 | if (arm2.theta>ARM2_ALIMHI){ // Limit angle arm 2 |
kbruil | 4:89f7d7f3a7ca | 160 | arm2.theta = ARM2_ALIMHI; |
kbruil | 4:89f7d7f3a7ca | 161 | } else if (arm2.theta < ARM2_ALIMLO) { |
kbruil | 4:89f7d7f3a7ca | 162 | arm2.theta = ARM2_ALIMLO; |
kbruil | 4:89f7d7f3a7ca | 163 | } |
kbruil | 4:89f7d7f3a7ca | 164 | } |
kbruil | 4:89f7d7f3a7ca | 165 | |
kbruil | 4:89f7d7f3a7ca | 166 | /* |
kbruil | 4:89f7d7f3a7ca | 167 | ** ============================================================================= |
kbruil | 4:89f7d7f3a7ca | 168 | ** robot_setSetpoint |
kbruil | 3:339b6f68b317 | 169 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 170 | ** Function: |
kbruil | 3:339b6f68b317 | 171 | ** Transforms cartesian coordinates into rotadions of the joints of the robot's |
kbruil | 3:339b6f68b317 | 172 | ** arms. |
kbruil | 3:339b6f68b317 | 173 | ** Input: |
kbruil | 4:89f7d7f3a7ca | 174 | ** float x, float y: setpoint position in cartesian coordinates |
kbruil | 4:89f7d7f3a7ca | 175 | ** ============================================================================= |
kbruil | 4:89f7d7f3a7ca | 176 | ** (x,y) |
kbruil | 4:89f7d7f3a7ca | 177 | ** |\ |
kbruil | 4:89f7d7f3a7ca | 178 | ** | \ |
kbruil | 4:89f7d7f3a7ca | 179 | ** |b2\ |
kbruil | 4:89f7d7f3a7ca | 180 | ** z2 | \arm2.length |
kbruil | 4:89f7d7f3a7ca | 181 | ** | \ |
kbruil | 4:89f7d7f3a7ca | 182 | ** z |_ _ _\ |
kbruil | 4:89f7d7f3a7ca | 183 | ** | / |
kbruil | 4:89f7d7f3a7ca | 184 | ** | / |
kbruil | 4:89f7d7f3a7ca | 185 | ** z1 | /arm1.length |
kbruil | 4:89f7d7f3a7ca | 186 | ** |b1/ |
kbruil | 4:89f7d7f3a7ca | 187 | ** | / |
kbruil | 4:89f7d7f3a7ca | 188 | ** |/ |
kbruil | 3:339b6f68b317 | 189 | ** ============================================================================= |
kbruil | 3:339b6f68b317 | 190 | */ |
kbruil | 3:339b6f68b317 | 191 | void robot_setSetpoint (float x,float y){ |
kbruil | 3:339b6f68b317 | 192 | float z; // length z of vector (x,y) |
kbruil | 3:339b6f68b317 | 193 | float z1; // part z1 |
kbruil | 3:339b6f68b317 | 194 | float z2; // part z2 |
kbruil | 3:339b6f68b317 | 195 | |
kbruil | 3:339b6f68b317 | 196 | float angle; // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0! |
kbruil | 3:339b6f68b317 | 197 | float b1,b2; // angle b1 and b2 (see drawing) |
kbruil | 0:86fe02a64f0e | 198 | |
kbruil | 3:339b6f68b317 | 199 | robot_applySetpointLimits(x,y); // Apply limits |
kbruil | 1:ffc1a2d6a824 | 200 | |
kbruil | 4:89f7d7f3a7ca | 201 | z = sqrt(pow(x,2)+pow(y,2)); // Calculate vector (x,y) length |
kbruil | 3:339b6f68b317 | 202 | |
kbruil | 4:89f7d7f3a7ca | 203 | // Calculate vector (x,y) angle |
kbruil | 3:339b6f68b317 | 204 | angle = atan2(y,x); // Calculate (x,y) angle using atan 2 |
kbruil | 3:339b6f68b317 | 205 | 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 | 206 | |
kbruil | 1:ffc1a2d6a824 | 207 | angle = angle - 0.5*M_PI; |
kbruil | 0:86fe02a64f0e | 208 | } |
kbruil | 1:ffc1a2d6a824 | 209 | else if (angle<M_PI && angle>0.5*M_PI){ |
kbruil | 1:ffc1a2d6a824 | 210 | angle = angle - 0.5*M_PI; |
kbruil | 0:86fe02a64f0e | 211 | } |
kbruil | 1:ffc1a2d6a824 | 212 | else if (angle<0 && angle>-0.5*M_PI){ |
kbruil | 1:ffc1a2d6a824 | 213 | angle = angle - 0.5 * M_PI; |
kbruil | 1:ffc1a2d6a824 | 214 | } |
kbruil | 1:ffc1a2d6a824 | 215 | else{ |
kbruil | 1:ffc1a2d6a824 | 216 | angle = angle + 1.5*M_PI; |
kbruil | 1:ffc1a2d6a824 | 217 | } |
kbruil | 3:339b6f68b317 | 218 | 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 | 219 | 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 | 220 | |
kbruil | 3:339b6f68b317 | 221 | z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z); // Calculate z1 |
kbruil | 3:339b6f68b317 | 222 | z2 = z-z1; // Calculate z2 |
kbruil | 3:339b6f68b317 | 223 | // Roundoff errors sometimes make z1 and z2 slightly higher than the arm lengths, this causes erratic behaviour in the acos function, |
kbruil | 3:339b6f68b317 | 224 | // so it is countered here by an extra check and fix. |
kbruil | 1:ffc1a2d6a824 | 225 | if (z1>arm1.length) {z1 = arm1.length;} |
kbruil | 1:ffc1a2d6a824 | 226 | else if (z1<-arm1.length) { z1 = -arm1.length;} |
kbruil | 1:ffc1a2d6a824 | 227 | if (z2>arm2.length) {z2 = arm2.length;} |
kbruil | 1:ffc1a2d6a824 | 228 | else if (z2<-arm2.length) { z2 = -arm2.length;} |
kbruil | 0:86fe02a64f0e | 229 | |
kbruil | 3:339b6f68b317 | 230 | b1 = acos(z1/arm1.length); // Calculate angle b1 |
kbruil | 3:339b6f68b317 | 231 | b2 = acos(z2/arm2.length); // Calculate angle b2 |
kbruil | 0:86fe02a64f0e | 232 | |
kbruil | 3:339b6f68b317 | 233 | arm1.theta = angle - b1; // Calculate new angle arm 1 |
kbruil | 3:339b6f68b317 | 234 | arm2.theta = b1 + b2; // Calculate new angle arm 2 |
kbruil | 4:89f7d7f3a7ca | 235 | robot_applyAngleLimits(); |
kbruil | 4:89f7d7f3a7ca | 236 | |
kbruil | 3:339b6f68b317 | 237 | arm2.x = arm1.length*-sin(arm1.theta); // Calculate new x position arm 2 |
kbruil | 3:339b6f68b317 | 238 | arm2.y = arm1.length*cos(arm1.theta); // Calculate new y position arm 2 |
kbruil | 3:339b6f68b317 | 239 | |
kbruil | 3:339b6f68b317 | 240 | end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector |
kbruil | 3:339b6f68b317 | 241 | end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y; // Calculate new y position end effector |
kbruil | 0:86fe02a64f0e | 242 | } |
kbruil | 0:86fe02a64f0e | 243 | |
kbruil | 10:27fdd43f3b85 | 244 | /* |
kbruil | 10:27fdd43f3b85 | 245 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 246 | ** robot_init |
kbruil | 10:27fdd43f3b85 | 247 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 248 | ** Function: |
kbruil | 10:27fdd43f3b85 | 249 | ** Initialise robot parameters |
kbruil | 10:27fdd43f3b85 | 250 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 251 | */ |
kbruil | 0:86fe02a64f0e | 252 | void robot_init() { |
kbruil | 9:19dafcb4a098 | 253 | // Set pwm motor periods |
kbruil | 9:19dafcb4a098 | 254 | gripperpwm.period_ms(20); |
kbruil | 9:19dafcb4a098 | 255 | |
kbruil | 9:19dafcb4a098 | 256 | pc.baud(115200); // Set serial communication speed |
kbruil | 9:19dafcb4a098 | 257 | |
kbruil | 9:19dafcb4a098 | 258 | // Initialise robot segments |
kbruil | 4:89f7d7f3a7ca | 259 | state = R_HORIZONTAL; |
kbruil | 0:86fe02a64f0e | 260 | // Init arm1 (upper arm) first link |
kbruil | 0:86fe02a64f0e | 261 | arm1.length = 28.0f; |
kbruil | 4:89f7d7f3a7ca | 262 | arm1.x = 0; |
kbruil | 4:89f7d7f3a7ca | 263 | arm1.y = 0; |
kbruil | 1:ffc1a2d6a824 | 264 | arm1.theta = 0; |
kbruil | 0:86fe02a64f0e | 265 | |
kbruil | 0:86fe02a64f0e | 266 | // Init arm2 (lower arm) second link |
kbruil | 0:86fe02a64f0e | 267 | arm2.length = 35.0f; |
kbruil | 4:89f7d7f3a7ca | 268 | arm2.x = 0; |
kbruil | 4:89f7d7f3a7ca | 269 | arm2.y = arm1.length; |
kbruil | 1:ffc1a2d6a824 | 270 | arm2.theta = 0; |
kbruil | 0:86fe02a64f0e | 271 | |
kbruil | 0:86fe02a64f0e | 272 | // Init end (end effector) third/last link |
kbruil | 4:89f7d7f3a7ca | 273 | end.length = 5.0f; |
kbruil | 4:89f7d7f3a7ca | 274 | end.x = 0; |
kbruil | 4:89f7d7f3a7ca | 275 | end.y = arm1.length+arm2.length; |
kbruil | 9:19dafcb4a098 | 276 | end.theta = 0; |
kbruil | 0:86fe02a64f0e | 277 | } |
kbruil | 0:86fe02a64f0e | 278 | |
kbruil | 10:27fdd43f3b85 | 279 | /* |
kbruil | 10:27fdd43f3b85 | 280 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 281 | ** r_moveHorizontal |
kbruil | 10:27fdd43f3b85 | 282 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 283 | ** Function: |
kbruil | 10:27fdd43f3b85 | 284 | ** Moves setpoint left or right depending on which switch is active. |
kbruil | 10:27fdd43f3b85 | 285 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 286 | */ |
kbruil | 9:19dafcb4a098 | 287 | void r_moveHorizontal(){ |
kbruil | 9:19dafcb4a098 | 288 | if (flag_switch){ |
kbruil | 9:19dafcb4a098 | 289 | if (!switch1.read()){ |
kbruil | 9:19dafcb4a098 | 290 | moveleft = true; |
kbruil | 10:27fdd43f3b85 | 291 | vx = (vx<-maxspeed)?-maxspeed:(vx-ax); |
kbruil | 0:86fe02a64f0e | 292 | } |
kbruil | 9:19dafcb4a098 | 293 | else { |
kbruil | 9:19dafcb4a098 | 294 | vx = moveleft?0:vx; |
kbruil | 0:86fe02a64f0e | 295 | } |
kbruil | 9:19dafcb4a098 | 296 | if (!switch2.read()){ |
kbruil | 9:19dafcb4a098 | 297 | moveleft = false; |
kbruil | 10:27fdd43f3b85 | 298 | vx = (vx>maxspeed)?maxspeed:(vx+ax); |
kbruil | 0:86fe02a64f0e | 299 | } |
kbruil | 9:19dafcb4a098 | 300 | else { |
kbruil | 9:19dafcb4a098 | 301 | vx = !moveleft?0:vx; |
kbruil | 1:ffc1a2d6a824 | 302 | } |
kbruil | 9:19dafcb4a098 | 303 | robot_setSetpoint(end.x+vx, end.y); |
kbruil | 9:19dafcb4a098 | 304 | } |
kbruil | 0:86fe02a64f0e | 305 | } |
kbruil | 0:86fe02a64f0e | 306 | |
kbruil | 10:27fdd43f3b85 | 307 | /* |
kbruil | 10:27fdd43f3b85 | 308 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 309 | ** r_moveVertical |
kbruil | 10:27fdd43f3b85 | 310 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 311 | ** Function: |
kbruil | 10:27fdd43f3b85 | 312 | ** Moves setpoint up or down depending on which switch is active. |
kbruil | 10:27fdd43f3b85 | 313 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 314 | */ |
kbruil | 9:19dafcb4a098 | 315 | void r_moveVertical(){ |
kbruil | 9:19dafcb4a098 | 316 | if (flag_switch){ |
kbruil | 9:19dafcb4a098 | 317 | if (!switch1.read()){ |
kbruil | 9:19dafcb4a098 | 318 | movedown = true; |
kbruil | 10:27fdd43f3b85 | 319 | vy = (vy<-maxspeed)?-maxspeed:(vy-ay); |
kbruil | 9:19dafcb4a098 | 320 | } |
kbruil | 9:19dafcb4a098 | 321 | else { |
kbruil | 9:19dafcb4a098 | 322 | vy = movedown?0:vy; |
kbruil | 9:19dafcb4a098 | 323 | } |
kbruil | 9:19dafcb4a098 | 324 | if (!switch2.read()){ |
kbruil | 9:19dafcb4a098 | 325 | movedown = false; |
kbruil | 10:27fdd43f3b85 | 326 | vy = (vy>maxspeed)?maxspeed:(vy+ay); |
kbruil | 9:19dafcb4a098 | 327 | } |
kbruil | 9:19dafcb4a098 | 328 | else { |
kbruil | 9:19dafcb4a098 | 329 | vy = !movedown?0:vy; |
kbruil | 9:19dafcb4a098 | 330 | } |
kbruil | 9:19dafcb4a098 | 331 | robot_setSetpoint(end.x, end.y+vy); |
kbruil | 9:19dafcb4a098 | 332 | } |
kbruil | 9:19dafcb4a098 | 333 | } |
kbruil | 6:3dcd9ce47900 | 334 | |
kbruil | 10:27fdd43f3b85 | 335 | /* |
kbruil | 10:27fdd43f3b85 | 336 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 337 | ** r_moveGripper |
kbruil | 10:27fdd43f3b85 | 338 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 339 | ** Function: |
kbruil | 10:27fdd43f3b85 | 340 | ** Opens and closes the gripper depending on which switch is active. |
kbruil | 10:27fdd43f3b85 | 341 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 342 | */ |
kbruil | 9:19dafcb4a098 | 343 | void r_moveGripper(){ |
kbruil | 9:19dafcb4a098 | 344 | if(flag_switch){ |
kbruil | 9:19dafcb4a098 | 345 | if(!switch1.read() && !gripperclosed){ |
kbruil | 9:19dafcb4a098 | 346 | gripperpwm.pulsewidth_us(1035); // Close gripper |
kbruil | 9:19dafcb4a098 | 347 | gripperclosed = true; |
kbruil | 9:19dafcb4a098 | 348 | } else if(!switch2.read() && gripperclosed){ |
kbruil | 9:19dafcb4a098 | 349 | gripperpwm.pulsewidth_us(1500); // Open gripper |
kbruil | 9:19dafcb4a098 | 350 | gripperclosed = false; |
kbruil | 9:19dafcb4a098 | 351 | } |
kbruil | 9:19dafcb4a098 | 352 | } |
kbruil | 9:19dafcb4a098 | 353 | } |
kbruil | 9:19dafcb4a098 | 354 | |
kbruil | 10:27fdd43f3b85 | 355 | /* |
kbruil | 10:27fdd43f3b85 | 356 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 357 | ** r_processStateSwitch |
kbruil | 10:27fdd43f3b85 | 358 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 359 | ** Function: |
kbruil | 10:27fdd43f3b85 | 360 | ** Processes presses of the state switch. |
kbruil | 10:27fdd43f3b85 | 361 | ** This switch switches between control states of the robot. |
kbruil | 10:27fdd43f3b85 | 362 | ** States are horizontal movement, vertical movement and gripper open/close. |
kbruil | 10:27fdd43f3b85 | 363 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 364 | */ |
kbruil | 9:19dafcb4a098 | 365 | void r_processStateSwitch(){ |
kbruil | 9:19dafcb4a098 | 366 | if(flag_switch){ |
kbruil | 6:3dcd9ce47900 | 367 | if(switch3.read()){ |
kbruil | 6:3dcd9ce47900 | 368 | switch3down = false; |
kbruil | 6:3dcd9ce47900 | 369 | } |
kbruil | 6:3dcd9ce47900 | 370 | else { |
kbruil | 6:3dcd9ce47900 | 371 | if (switch3down==false){ |
kbruil | 6:3dcd9ce47900 | 372 | switch3down = true; |
kbruil | 6:3dcd9ce47900 | 373 | switch(state){ |
kbruil | 6:3dcd9ce47900 | 374 | case R_HORIZONTAL: |
kbruil | 6:3dcd9ce47900 | 375 | state = R_VERTICAL; |
kbruil | 6:3dcd9ce47900 | 376 | break; |
kbruil | 6:3dcd9ce47900 | 377 | case R_VERTICAL: |
kbruil | 9:19dafcb4a098 | 378 | state = R_GRIPPER; |
kbruil | 9:19dafcb4a098 | 379 | break; |
kbruil | 9:19dafcb4a098 | 380 | case R_GRIPPER: |
kbruil | 6:3dcd9ce47900 | 381 | state = R_HORIZONTAL; |
kbruil | 6:3dcd9ce47900 | 382 | break; |
kbruil | 9:19dafcb4a098 | 383 | |
kbruil | 6:3dcd9ce47900 | 384 | } |
kbruil | 6:3dcd9ce47900 | 385 | } |
kbruil | 6:3dcd9ce47900 | 386 | } |
kbruil | 9:19dafcb4a098 | 387 | } |
kbruil | 9:19dafcb4a098 | 388 | } |
kbruil | 5:ca4f81348c30 | 389 | |
kbruil | 10:27fdd43f3b85 | 390 | /* |
kbruil | 10:27fdd43f3b85 | 391 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 392 | ** r_switchFlagReset |
kbruil | 10:27fdd43f3b85 | 393 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 394 | ** Function: |
kbruil | 10:27fdd43f3b85 | 395 | ** Resets the switch ticker flag. |
kbruil | 10:27fdd43f3b85 | 396 | ** Call near the end of the main loop, after all switches have been read. |
kbruil | 10:27fdd43f3b85 | 397 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 398 | */ |
kbruil | 9:19dafcb4a098 | 399 | void r_switchFlagReset(){ |
kbruil | 9:19dafcb4a098 | 400 | if (flag_switch){ |
kbruil | 9:19dafcb4a098 | 401 | flag_switch = false; |
kbruil | 4:89f7d7f3a7ca | 402 | } |
kbruil | 4:89f7d7f3a7ca | 403 | } |
kbruil | 4:89f7d7f3a7ca | 404 | |
kbruil | 6:3dcd9ce47900 | 405 | |
kbruil | 10:27fdd43f3b85 | 406 | /* |
kbruil | 10:27fdd43f3b85 | 407 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 408 | ** r_doPID |
kbruil | 10:27fdd43f3b85 | 409 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 410 | ** Function: |
kbruil | 10:27fdd43f3b85 | 411 | ** Runs the PID controller using the setpoint and sets the motorspeeds. |
kbruil | 10:27fdd43f3b85 | 412 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 413 | */ |
kbruil | 4:89f7d7f3a7ca | 414 | void r_doPID(){ |
kbruil | 9:19dafcb4a098 | 415 | double dt = 1/50.0; // PID time step |
kbruil | 11:4382c567e0d4 | 416 | |
kbruil | 10:27fdd43f3b85 | 417 | if (flag_PID){ |
kbruil | 10:27fdd43f3b85 | 418 | flag_PID = false; |
kbruil | 10:27fdd43f3b85 | 419 | motor1.PID(arm1.theta, dt); |
kbruil | 10:27fdd43f3b85 | 420 | motor2.PID(arm2.theta, dt); |
kbruil | 11:4382c567e0d4 | 421 | } |
kbruil | 4:89f7d7f3a7ca | 422 | } |
kbruil | 4:89f7d7f3a7ca | 423 | |
kbruil | 10:27fdd43f3b85 | 424 | /* |
kbruil | 10:27fdd43f3b85 | 425 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 426 | ** r_outputToMatlab |
kbruil | 10:27fdd43f3b85 | 427 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 428 | ** Function: |
kbruil | 10:27fdd43f3b85 | 429 | ** Outputs data to the serial interface to be read in matlab (or putty). |
kbruil | 10:27fdd43f3b85 | 430 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 431 | */ |
kbruil | 4:89f7d7f3a7ca | 432 | void r_outputToMatlab(){ |
kbruil | 4:89f7d7f3a7ca | 433 | if (flag_output){ |
kbruil | 4:89f7d7f3a7ca | 434 | flag_output = false; |
kbruil | 4:89f7d7f3a7ca | 435 | printf("Angle1\n\r"); |
kbruil | 5:ca4f81348c30 | 436 | printf("%f\n\r", arm1.theta); |
kbruil | 4:89f7d7f3a7ca | 437 | printf("Angle2\n\r"); |
kbruil | 5:ca4f81348c30 | 438 | printf("%f\n\r", arm2.theta); |
kbruil | 5:ca4f81348c30 | 439 | printf("Pos1\n\r"); |
kbruil | 11:4382c567e0d4 | 440 | printf("%f\n\r", motor1.getPosition()); |
kbruil | 5:ca4f81348c30 | 441 | printf("Pos2\n\r"); |
kbruil | 11:4382c567e0d4 | 442 | printf("%f\n\r", motor2.getPosition()); |
kbruil | 5:ca4f81348c30 | 443 | |
kbruil | 4:89f7d7f3a7ca | 444 | printf("x: %f | y: %f\n\r", end.x, end.y); |
kbruil | 4:89f7d7f3a7ca | 445 | } |
kbruil | 4:89f7d7f3a7ca | 446 | } |
kbruil | 4:89f7d7f3a7ca | 447 | |
kbruil | 9:19dafcb4a098 | 448 | /* |
kbruil | 10:27fdd43f3b85 | 449 | ** ============================================================================= |
kbruil | 11:4382c567e0d4 | 450 | ** r_processEMG |
kbruil | 11:4382c567e0d4 | 451 | ** ============================================================================= |
kbruil | 11:4382c567e0d4 | 452 | ** Function: |
kbruil | 11:4382c567e0d4 | 453 | ** Processes the EMG signals at ticker frequency |
kbruil | 11:4382c567e0d4 | 454 | ** ============================================================================= |
kbruil | 11:4382c567e0d4 | 455 | */ |
kbruil | 11:4382c567e0d4 | 456 | void r_processEMG(){ |
kbruil | 11:4382c567e0d4 | 457 | if(flag_EMG){ |
kbruil | 11:4382c567e0d4 | 458 | flag_EMG = false; |
kbruil | 11:4382c567e0d4 | 459 | // emg1.tick(); |
kbruil | 11:4382c567e0d4 | 460 | // emg2.tick(); |
kbruil | 11:4382c567e0d4 | 461 | // emg3.tick(); |
kbruil | 11:4382c567e0d4 | 462 | } |
kbruil | 11:4382c567e0d4 | 463 | } |
kbruil | 11:4382c567e0d4 | 464 | /* |
kbruil | 11:4382c567e0d4 | 465 | ** ============================================================================= |
kbruil | 9:19dafcb4a098 | 466 | ** maintickerfunction |
kbruil | 10:27fdd43f3b85 | 467 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 468 | ** Function: |
kbruil | 10:27fdd43f3b85 | 469 | ** Ticker call back. Sets flags at appropriate timer intervals for |
kbruil | 9:19dafcb4a098 | 470 | ** reading switches, calculating PID, outputting information via serial and |
kbruil | 9:19dafcb4a098 | 471 | ** reading EMG signals. |
kbruil | 10:27fdd43f3b85 | 472 | ** ============================================================================= |
kbruil | 9:19dafcb4a098 | 473 | */ |
kbruil | 4:89f7d7f3a7ca | 474 | void maintickerfunction (){ |
kbruil | 4:89f7d7f3a7ca | 475 | static int cnt=0; |
kbruil | 10:27fdd43f3b85 | 476 | if (cnt%20 == 0){ // Read and react to switches 50 times per second |
kbruil | 4:89f7d7f3a7ca | 477 | flag_switch = true; |
kbruil | 4:89f7d7f3a7ca | 478 | } |
kbruil | 10:27fdd43f3b85 | 479 | if (cnt%20 == 0){ // Run PID controller 50 times per second |
kbruil | 4:89f7d7f3a7ca | 480 | flag_PID = true; |
kbruil | 4:89f7d7f3a7ca | 481 | } |
kbruil | 10:27fdd43f3b85 | 482 | if (cnt%100 == 0){ // Output data to serial bus 10 times per second |
kbruil | 4:89f7d7f3a7ca | 483 | flag_output = true; |
kbruil | 4:89f7d7f3a7ca | 484 | } |
kbruil | 11:4382c567e0d4 | 485 | flag_EMG = true; |
kbruil | 5:ca4f81348c30 | 486 | cnt++; |
kbruil | 4:89f7d7f3a7ca | 487 | } |
kbruil | 4:89f7d7f3a7ca | 488 | |
kbruil | 10:27fdd43f3b85 | 489 | /* |
kbruil | 10:27fdd43f3b85 | 490 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 491 | ** main |
kbruil | 10:27fdd43f3b85 | 492 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 493 | ** Function: |
kbruil | 10:27fdd43f3b85 | 494 | ** Contains the main loop with state machine. |
kbruil | 10:27fdd43f3b85 | 495 | ** ============================================================================= |
kbruil | 10:27fdd43f3b85 | 496 | */ |
kbruil | 0:86fe02a64f0e | 497 | int main(){ |
kbruil | 9:19dafcb4a098 | 498 | // Initialise main ticker |
kbruil | 4:89f7d7f3a7ca | 499 | mainticker.attach(&maintickerfunction,0.001f); |
kbruil | 9:19dafcb4a098 | 500 | |
kbruil | 0:86fe02a64f0e | 501 | robot_init(); |
kbruil | 0:86fe02a64f0e | 502 | while(true){ |
kbruil | 4:89f7d7f3a7ca | 503 | switch(state){ |
kbruil | 4:89f7d7f3a7ca | 504 | case R_INIT: |
kbruil | 4:89f7d7f3a7ca | 505 | break; |
kbruil | 4:89f7d7f3a7ca | 506 | case R_HORIZONTAL: |
kbruil | 4:89f7d7f3a7ca | 507 | r_moveHorizontal(); |
kbruil | 4:89f7d7f3a7ca | 508 | break; |
kbruil | 6:3dcd9ce47900 | 509 | case R_VERTICAL: |
kbruil | 6:3dcd9ce47900 | 510 | r_moveVertical(); |
kbruil | 6:3dcd9ce47900 | 511 | break; |
kbruil | 9:19dafcb4a098 | 512 | case R_GRIPPER: |
kbruil | 9:19dafcb4a098 | 513 | r_moveGripper(); |
kbruil | 9:19dafcb4a098 | 514 | break; |
kbruil | 4:89f7d7f3a7ca | 515 | } |
kbruil | 11:4382c567e0d4 | 516 | r_processEMG(); |
kbruil | 9:19dafcb4a098 | 517 | r_processStateSwitch(); |
kbruil | 9:19dafcb4a098 | 518 | r_switchFlagReset(); |
kbruil | 4:89f7d7f3a7ca | 519 | r_doPID(); |
kbruil | 4:89f7d7f3a7ca | 520 | r_outputToMatlab(); |
kbruil | 0:86fe02a64f0e | 521 | } |
kbruil | 0:86fe02a64f0e | 522 | } |