Kim Bruil / Mbed 2 deprecated r_robot

robot positie error test ding

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

main.cpp@14:d0e5894aa352, 2016-10-31 (annotated)

Committer:
kbruil
Date:
Mon Oct 31 12:32:54 2016 +0000
Revision:
14:d0e5894aa352
Parent:
13:4f426186cd19 
Code biorobotics 31-10-2016

Who changed what in which revision?

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