Kim Bruil / Mbed 2 deprecated r_robot

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?

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