Kim Bruil / Mbed 2 deprecated r_robot

robot positie error test ding

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

main.cpp@9:19dafcb4a098, 2016-10-27 (annotated)

Committer:
kbruil
Date:
Thu Oct 27 19:36:12 2016 +0000
Revision:
9:19dafcb4a098
Parent:
8:681151d89a75
Child:
10:27fdd43f3b85
Robot PID cleaned up a bit

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 0:86fe02a64f0e 7 // Angle limits 215, 345 lower arm, 0, 145 upper arm
kbruil 0:86fe02a64f0e 8 // Robot arm x,y limits (limit to table top)
kbruil 0:86fe02a64f0e 9
kbruil 0:86fe02a64f0e 10
kbruil 0:86fe02a64f0e 11 #define M_PI 3.141592653589793238462643383279502884L // Pi
kbruil 4:89f7d7f3a7ca 12 enum r_states{R_INIT, R_HORIZONTAL, R_VERTICAL, R_GRIPPER};
kbruil 4:89f7d7f3a7ca 13 r_states state;
kbruil 4:89f7d7f3a7ca 14
kbruil 4:89f7d7f3a7ca 15 /*
kbruil 4:89f7d7f3a7ca 16 ** Table and robot limits
kbruil 4:89f7d7f3a7ca 17 */
kbruil 3:339b6f68b317 18 const double R_XTABLE_LEFT = -30.0; // Robot in the middle, table width=60
kbruil 3:339b6f68b317 19 const double R_XTABLE_RIGHT = 30.0;
kbruil 3:339b6f68b317 20 const double R_YTABLE_BOTTOM = -18.0;
kbruil 3:339b6f68b317 21 const double R_YTABLE_TOP = 82.0;
kbruil 3:339b6f68b317 22 const double R_ROBOT_LEFT = -8.0; // Robot limited from moving onto itself (20x20 base)
kbruil 3:339b6f68b317 23 const double R_ROBOT_RIGHT = 8.0;
kbruil 3:339b6f68b317 24 const double R_ROBOT_BOTTOM = -18.0;
kbruil 3:339b6f68b317 25 const double R_ROBOT_TOP = 2.0;
kbruil 1:ffc1a2d6a824 26
kbruil 4:89f7d7f3a7ca 27 /*
kbruil 4:89f7d7f3a7ca 28 ** Robot arm angle limits
kbruil 4:89f7d7f3a7ca 29 */
kbruil 4:89f7d7f3a7ca 30 const double ARM1_ALIMLO = (2*M_PI/360)*125;
kbruil 4:89f7d7f3a7ca 31 const double ARM1_ALIMHI = -(2*M_PI/360)*125;
kbruil 1:ffc1a2d6a824 32 const double ARM2_ALIMLO = 0;
kbruil 1:ffc1a2d6a824 33 const double ARM2_ALIMHI = (2*M_PI/360)*145;
kbruil 0:86fe02a64f0e 34
kbruil 4:89f7d7f3a7ca 35 /*
kbruil 4:89f7d7f3a7ca 36 ** Ticker flags
kbruil 4:89f7d7f3a7ca 37 */
kbruil 4:89f7d7f3a7ca 38
kbruil 4:89f7d7f3a7ca 39 bool flag_switch;
kbruil 4:89f7d7f3a7ca 40 bool flag_PID;
kbruil 4:89f7d7f3a7ca 41 bool flag_output;
kbruil 4:89f7d7f3a7ca 42
kbruil 4:89f7d7f3a7ca 43 Ticker mainticker; // Main ticker
kbruil 4:89f7d7f3a7ca 44
kbruil 4:89f7d7f3a7ca 45 /*
kbruil 4:89f7d7f3a7ca 46 ** Motor variables
kbruil 4:89f7d7f3a7ca 47 */
kbruil 4:89f7d7f3a7ca 48 DigitalOut motor1dir(D7);
kbruil 4:89f7d7f3a7ca 49 PwmOut motor1pwm(D6);
kbruil 8:681151d89a75 50 QEI motor1sense(D13,D12,NC, 8400, QEI::X4_ENCODING);
kbruil 5:ca4f81348c30 51 double motor1pos=0;
kbruil 5:ca4f81348c30 52 double motor1int=0;
kbruil 5:ca4f81348c30 53 double motor1olderr=0;
kbruil 4:89f7d7f3a7ca 54 DigitalOut motor2dir(D4);
kbruil 4:89f7d7f3a7ca 55 PwmOut motor2pwm(D5);
kbruil 8:681151d89a75 56 QEI motor2sense(D10,D11,NC, 8400, QEI::X4_ENCODING);
kbruil 5:ca4f81348c30 57 double motor2pos=0;
kbruil 5:ca4f81348c30 58 double motor2int=0;
kbruil 5:ca4f81348c30 59 double motor2olderr=0;
kbruil 9:19dafcb4a098 60 /*
kbruil 9:19dafcb4a098 61 ** Gripper variable
kbruil 9:19dafcb4a098 62 */
kbruil 9:19dafcb4a098 63 PwmOut gripperpwm(D3);
kbruil 9:19dafcb4a098 64 bool gripperclosed=true;
kbruil 4:89f7d7f3a7ca 65
kbruil 6:3dcd9ce47900 66 BiQuadChain motor1bqc_derivative;
kbruil 6:3dcd9ce47900 67 BiQuadChain motor2bqc_derivative;
kbruil 6:3dcd9ce47900 68 BiQuad bq1(2.533015E-01, 5.066030E-01, 2.533015E-01, 1.000000E+00, -4.531195E-01, 4.663256E-01);
kbruil 6:3dcd9ce47900 69 BiQuad bq2(1.839030E-01, 3.678060E-01, 1.839030E-01, 1.000000E+00, -3.289757E-01, 6.458765E-02);
kbruil 6:3dcd9ce47900 70 BiQuad bq3(2.533015E-01, 5.066030E-01, 2.533015E-01, 1.000000E+00, -4.531195E-01, 4.663256E-01);
kbruil 6:3dcd9ce47900 71 BiQuad bq4(1.839030E-01, 3.678060E-01, 1.839030E-01, 1.000000E+00, -3.289757E-01, 6.458765E-02);
kbruil 6:3dcd9ce47900 72
kbruil 4:89f7d7f3a7ca 73
kbruil 2:027d19cda97b 74 float vx=0;
kbruil 2:027d19cda97b 75 float vy=0;
kbruil 0:86fe02a64f0e 76 // Struct r_link:
kbruil 0:86fe02a64f0e 77 // Defines a robot link type (arm or end effector).
kbruil 0:86fe02a64f0e 78 struct r_link {
kbruil 0:86fe02a64f0e 79 float length; // link length
kbruil 0:86fe02a64f0e 80 float x; // link x position
kbruil 0:86fe02a64f0e 81 float y; // link y position
kbruil 0:86fe02a64f0e 82 float theta; // link angle
kbruil 0:86fe02a64f0e 83 };
kbruil 0:86fe02a64f0e 84
kbruil 0:86fe02a64f0e 85 MODSERIAL pc(USBTX, USBRX);
kbruil 0:86fe02a64f0e 86 DigitalIn switch1(SW3);
kbruil 0:86fe02a64f0e 87 DigitalIn switch2(SW2);
kbruil 6:3dcd9ce47900 88 DigitalIn switch3(D9);
kbruil 6:3dcd9ce47900 89 bool switch3down=false;
kbruil 5:ca4f81348c30 90 //DigitalIn switch4(D9);
kbruil 0:86fe02a64f0e 91 DigitalOut ledr(LED_RED);
kbruil 0:86fe02a64f0e 92 DigitalOut ledg(LED_GREEN);
kbruil 0:86fe02a64f0e 93
kbruil 0:86fe02a64f0e 94
kbruil 0:86fe02a64f0e 95 r_link arm1; // Robot upper arm
kbruil 0:86fe02a64f0e 96 r_link arm2; // Robot lower arm
kbruil 0:86fe02a64f0e 97 r_link end; // Robot end effector
kbruil 0:86fe02a64f0e 98
kbruil 3:339b6f68b317 99 /*
kbruil 3:339b6f68b317 100 ** =============================================================================
kbruil 3:339b6f68b317 101 ** robot_applySetpointLimits
kbruil 3:339b6f68b317 102 ** =============================================================================
kbruil 3:339b6f68b317 103 ** Function:
kbruil 3:339b6f68b317 104 ** Limits the cartesian coordinates to table, length of robot and prevents
kbruil 3:339b6f68b317 105 ** the robot arm crossing it's own base.
kbruil 3:339b6f68b317 106 ** Input:
kbruil 3:339b6f68b317 107 ** float x, float y: position in cartesian coordinates
kbruil 3:339b6f68b317 108 ** Output:
kbruil 3:339b6f68b317 109 ** float x, float y: limited position in cartesian coordinates
kbruil 3:339b6f68b317 110 ** =============================================================================
kbruil 3:339b6f68b317 111 */
kbruil 3:339b6f68b317 112 void robot_applySetpointLimits (float& x, float& y){
kbruil 0:86fe02a64f0e 113 float z;
kbruil 0:86fe02a64f0e 114 float angle;
kbruil 0:86fe02a64f0e 115
kbruil 0:86fe02a64f0e 116 // Limit x,y to table top size
kbruil 3:339b6f68b317 117 if (x<R_XTABLE_LEFT) { x=R_XTABLE_LEFT; vx=0;}
kbruil 3:339b6f68b317 118 else if (x>R_XTABLE_RIGHT) { x=R_XTABLE_RIGHT; vx=0;}
kbruil 3:339b6f68b317 119 if (y<R_YTABLE_BOTTOM) { y=R_YTABLE_BOTTOM; vy=0;}
kbruil 3:339b6f68b317 120 else if (y>R_YTABLE_TOP) { y=R_YTABLE_TOP; vy=0; }
kbruil 3:339b6f68b317 121
kbruil 3:339b6f68b317 122 // Limit x,y to maximum reach of robot arm
kbruil 0:86fe02a64f0e 123 z = sqrt(pow(x,2)+pow(y,2));
kbruil 0:86fe02a64f0e 124 angle = atan2(y,x);
kbruil 0:86fe02a64f0e 125 if (z>(arm1.length+arm2.length)){
kbruil 0:86fe02a64f0e 126 z = arm1.length+arm2.length;
kbruil 0:86fe02a64f0e 127 x = z*cos(angle);
kbruil 0:86fe02a64f0e 128 y = z*sin(angle);
kbruil 0:86fe02a64f0e 129 }
kbruil 3:339b6f68b317 130
kbruil 3:339b6f68b317 131 // Prevent x,y from entering robot body
kbruil 3:339b6f68b317 132 if (end.x > R_ROBOT_LEFT && end.x < R_ROBOT_RIGHT && end.y >= R_ROBOT_TOP && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 133 y = R_ROBOT_TOP;
kbruil 3:339b6f68b317 134 }
kbruil 3:339b6f68b317 135 if (end.x <= R_ROBOT_LEFT && x > R_ROBOT_LEFT && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 136 x = R_ROBOT_LEFT;
kbruil 3:339b6f68b317 137 }
kbruil 3:339b6f68b317 138 if (end.x >= R_ROBOT_RIGHT && x < R_ROBOT_RIGHT && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 139 x = R_ROBOT_RIGHT;
kbruil 3:339b6f68b317 140 }
kbruil 0:86fe02a64f0e 141 }
kbruil 0:86fe02a64f0e 142
kbruil 3:339b6f68b317 143 /*
kbruil 3:339b6f68b317 144 ** =============================================================================
kbruil 4:89f7d7f3a7ca 145 ** robot_applyAngleLimits
kbruil 4:89f7d7f3a7ca 146 ** =============================================================================
kbruil 4:89f7d7f3a7ca 147 ** Function:
kbruil 4:89f7d7f3a7ca 148 ** Limits the angles of the robot's arms to prevent them from moving into
kbruil 4:89f7d7f3a7ca 149 ** themselves
kbruil 4:89f7d7f3a7ca 150 ** Input: -
kbruil 4:89f7d7f3a7ca 151 ** Output: -
kbruil 4:89f7d7f3a7ca 152 ** =============================================================================
kbruil 4:89f7d7f3a7ca 153 */
kbruil 4:89f7d7f3a7ca 154 void robot_applyAngleLimits(){
kbruil 4:89f7d7f3a7ca 155 if (arm1.theta>ARM1_ALIMLO){ // Limit angle arm1
kbruil 4:89f7d7f3a7ca 156 arm1.theta = ARM1_ALIMLO;
kbruil 4:89f7d7f3a7ca 157 }
kbruil 4:89f7d7f3a7ca 158 else if (arm1.theta<ARM1_ALIMHI){
kbruil 4:89f7d7f3a7ca 159 arm1.theta = ARM1_ALIMHI;
kbruil 4:89f7d7f3a7ca 160 }
kbruil 4:89f7d7f3a7ca 161
kbruil 4:89f7d7f3a7ca 162 if (arm2.theta>ARM2_ALIMHI){ // Limit angle arm 2
kbruil 4:89f7d7f3a7ca 163 arm2.theta = ARM2_ALIMHI;
kbruil 4:89f7d7f3a7ca 164 } else if (arm2.theta < ARM2_ALIMLO) {
kbruil 4:89f7d7f3a7ca 165 arm2.theta = ARM2_ALIMLO;
kbruil 4:89f7d7f3a7ca 166 }
kbruil 4:89f7d7f3a7ca 167 }
kbruil 4:89f7d7f3a7ca 168
kbruil 4:89f7d7f3a7ca 169 /*
kbruil 4:89f7d7f3a7ca 170 ** =============================================================================
kbruil 4:89f7d7f3a7ca 171 ** robot_setSetpoint
kbruil 3:339b6f68b317 172 ** =============================================================================
kbruil 3:339b6f68b317 173 ** Function:
kbruil 3:339b6f68b317 174 ** Transforms cartesian coordinates into rotadions of the joints of the robot's
kbruil 3:339b6f68b317 175 ** arms.
kbruil 3:339b6f68b317 176 ** Input:
kbruil 4:89f7d7f3a7ca 177 ** float x, float y: setpoint position in cartesian coordinates
kbruil 4:89f7d7f3a7ca 178 ** =============================================================================
kbruil 4:89f7d7f3a7ca 179 ** (x,y)
kbruil 4:89f7d7f3a7ca 180 ** |\
kbruil 4:89f7d7f3a7ca 181 ** | \
kbruil 4:89f7d7f3a7ca 182 ** |b2\
kbruil 4:89f7d7f3a7ca 183 ** z2 | \arm2.length
kbruil 4:89f7d7f3a7ca 184 ** | \
kbruil 4:89f7d7f3a7ca 185 ** z |_ _ _\
kbruil 4:89f7d7f3a7ca 186 ** | /
kbruil 4:89f7d7f3a7ca 187 ** | /
kbruil 4:89f7d7f3a7ca 188 ** z1 | /arm1.length
kbruil 4:89f7d7f3a7ca 189 ** |b1/
kbruil 4:89f7d7f3a7ca 190 ** | /
kbruil 4:89f7d7f3a7ca 191 ** |/
kbruil 3:339b6f68b317 192 ** =============================================================================
kbruil 3:339b6f68b317 193 */
kbruil 3:339b6f68b317 194 void robot_setSetpoint (float x,float y){
kbruil 3:339b6f68b317 195 float z; // length z of vector (x,y)
kbruil 3:339b6f68b317 196 float z1; // part z1
kbruil 3:339b6f68b317 197 float z2; // part z2
kbruil 3:339b6f68b317 198
kbruil 3:339b6f68b317 199 float angle; // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0!
kbruil 3:339b6f68b317 200 float b1,b2; // angle b1 and b2 (see drawing)
kbruil 0:86fe02a64f0e 201
kbruil 3:339b6f68b317 202 robot_applySetpointLimits(x,y); // Apply limits
kbruil 1:ffc1a2d6a824 203
kbruil 4:89f7d7f3a7ca 204 z = sqrt(pow(x,2)+pow(y,2)); // Calculate vector (x,y) length
kbruil 3:339b6f68b317 205
kbruil 4:89f7d7f3a7ca 206 // Calculate vector (x,y) angle
kbruil 3:339b6f68b317 207 angle = atan2(y,x); // Calculate (x,y) angle using atan 2
kbruil 3:339b6f68b317 208 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 209
kbruil 1:ffc1a2d6a824 210 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 211 }
kbruil 1:ffc1a2d6a824 212 else if (angle<M_PI && angle>0.5*M_PI){
kbruil 1:ffc1a2d6a824 213 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 214 }
kbruil 1:ffc1a2d6a824 215 else if (angle<0 && angle>-0.5*M_PI){
kbruil 1:ffc1a2d6a824 216 angle = angle - 0.5 * M_PI;
kbruil 1:ffc1a2d6a824 217 }
kbruil 1:ffc1a2d6a824 218 else{
kbruil 1:ffc1a2d6a824 219 angle = angle + 1.5*M_PI;
kbruil 1:ffc1a2d6a824 220 }
kbruil 3:339b6f68b317 221 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 222 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 223
kbruil 3:339b6f68b317 224 z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z); // Calculate z1
kbruil 3:339b6f68b317 225 z2 = z-z1; // Calculate z2
kbruil 3:339b6f68b317 226 // Roundoff errors sometimes make z1 and z2 slightly higher than the arm lengths, this causes erratic behaviour in the acos function,
kbruil 3:339b6f68b317 227 // so it is countered here by an extra check and fix.
kbruil 1:ffc1a2d6a824 228 if (z1>arm1.length) {z1 = arm1.length;}
kbruil 1:ffc1a2d6a824 229 else if (z1<-arm1.length) { z1 = -arm1.length;}
kbruil 1:ffc1a2d6a824 230 if (z2>arm2.length) {z2 = arm2.length;}
kbruil 1:ffc1a2d6a824 231 else if (z2<-arm2.length) { z2 = -arm2.length;}
kbruil 0:86fe02a64f0e 232
kbruil 3:339b6f68b317 233 b1 = acos(z1/arm1.length); // Calculate angle b1
kbruil 3:339b6f68b317 234 b2 = acos(z2/arm2.length); // Calculate angle b2
kbruil 0:86fe02a64f0e 235
kbruil 3:339b6f68b317 236 arm1.theta = angle - b1; // Calculate new angle arm 1
kbruil 3:339b6f68b317 237 arm2.theta = b1 + b2; // Calculate new angle arm 2
kbruil 4:89f7d7f3a7ca 238 robot_applyAngleLimits();
kbruil 4:89f7d7f3a7ca 239
kbruil 3:339b6f68b317 240 arm2.x = arm1.length*-sin(arm1.theta); // Calculate new x position arm 2
kbruil 3:339b6f68b317 241 arm2.y = arm1.length*cos(arm1.theta); // Calculate new y position arm 2
kbruil 3:339b6f68b317 242
kbruil 3:339b6f68b317 243 end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector
kbruil 3:339b6f68b317 244 end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y; // Calculate new y position end effector
kbruil 0:86fe02a64f0e 245 }
kbruil 0:86fe02a64f0e 246
kbruil 0:86fe02a64f0e 247 // robot_init
kbruil 0:86fe02a64f0e 248 // Initialise robot
kbruil 0:86fe02a64f0e 249 void robot_init() {
kbruil 9:19dafcb4a098 250 // Set pwm motor periods
kbruil 9:19dafcb4a098 251 gripperpwm.period_ms(20);
kbruil 9:19dafcb4a098 252 motor1pwm.period_ms(2);
kbruil 9:19dafcb4a098 253 motor2pwm.period_ms(2);
kbruil 9:19dafcb4a098 254
kbruil 9:19dafcb4a098 255 // Initialise encoders
kbruil 9:19dafcb4a098 256 motor1sense.reset();
kbruil 9:19dafcb4a098 257 motor2sense.reset();
kbruil 9:19dafcb4a098 258
kbruil 9:19dafcb4a098 259
kbruil 9:19dafcb4a098 260 // Set motor PID D-filters
kbruil 9:19dafcb4a098 261 motor1bqc_derivative.add(&bq1).add(&bq2);
kbruil 9:19dafcb4a098 262 motor2bqc_derivative.add(&bq3).add(&bq4);
kbruil 9:19dafcb4a098 263
kbruil 9:19dafcb4a098 264
kbruil 9:19dafcb4a098 265 pc.baud(115200); // Set serial communication speed
kbruil 9:19dafcb4a098 266
kbruil 9:19dafcb4a098 267 // Initialise robot segments
kbruil 4:89f7d7f3a7ca 268 state = R_HORIZONTAL;
kbruil 0:86fe02a64f0e 269 // Init arm1 (upper arm) first link
kbruil 0:86fe02a64f0e 270 arm1.length = 28.0f;
kbruil 4:89f7d7f3a7ca 271 arm1.x = 0;
kbruil 4:89f7d7f3a7ca 272 arm1.y = 0;
kbruil 1:ffc1a2d6a824 273 arm1.theta = 0;
kbruil 0:86fe02a64f0e 274
kbruil 0:86fe02a64f0e 275 // Init arm2 (lower arm) second link
kbruil 0:86fe02a64f0e 276 arm2.length = 35.0f;
kbruil 4:89f7d7f3a7ca 277 arm2.x = 0;
kbruil 4:89f7d7f3a7ca 278 arm2.y = arm1.length;
kbruil 1:ffc1a2d6a824 279 arm2.theta = 0;
kbruil 0:86fe02a64f0e 280
kbruil 0:86fe02a64f0e 281 // Init end (end effector) third/last link
kbruil 4:89f7d7f3a7ca 282 end.length = 5.0f;
kbruil 4:89f7d7f3a7ca 283 end.x = 0;
kbruil 4:89f7d7f3a7ca 284 end.y = arm1.length+arm2.length;
kbruil 9:19dafcb4a098 285 end.theta = 0;
kbruil 0:86fe02a64f0e 286 }
kbruil 0:86fe02a64f0e 287
kbruil 7:b09bfde57cba 288 float ax=0.01;
kbruil 7:b09bfde57cba 289 float ay=0.01;
kbruil 9:19dafcb4a098 290 const double maxspeed=3.0f;
kbruil 9:19dafcb4a098 291 bool moveleft; // Flag set to true if moving to the left
kbruil 9:19dafcb4a098 292 bool movedown; // Flag set to true if moving to the left
kbruil 0:86fe02a64f0e 293
kbruil 9:19dafcb4a098 294 void r_moveHorizontal(){
kbruil 9:19dafcb4a098 295 if (flag_switch){
kbruil 9:19dafcb4a098 296 if (!switch1.read()){
kbruil 9:19dafcb4a098 297 moveleft = true;
kbruil 9:19dafcb4a098 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 0:86fe02a64f0e 302 }
kbruil 9:19dafcb4a098 303 if (!switch2.read()){
kbruil 9:19dafcb4a098 304 moveleft = false;
kbruil 9:19dafcb4a098 305 vx = (vx>maxspeed)?maxspeed:vx+ax;
kbruil 0:86fe02a64f0e 306 }
kbruil 9:19dafcb4a098 307 else {
kbruil 9:19dafcb4a098 308 vx = !moveleft?0:vx;
kbruil 1:ffc1a2d6a824 309 }
kbruil 9:19dafcb4a098 310 robot_setSetpoint(end.x+vx, end.y);
kbruil 9:19dafcb4a098 311 }
kbruil 0:86fe02a64f0e 312 }
kbruil 0:86fe02a64f0e 313
kbruil 9:19dafcb4a098 314 void r_moveVertical(){
kbruil 9:19dafcb4a098 315 if (flag_switch){
kbruil 9:19dafcb4a098 316 if (!switch1.read()){
kbruil 9:19dafcb4a098 317 movedown = true;
kbruil 9:19dafcb4a098 318 vy = (vy<-maxspeed)?-maxspeed:vy-ay;
kbruil 9:19dafcb4a098 319 }
kbruil 9:19dafcb4a098 320 else {
kbruil 9:19dafcb4a098 321 vy = movedown?0:vy;
kbruil 9:19dafcb4a098 322 }
kbruil 9:19dafcb4a098 323 if (!switch2.read()){
kbruil 9:19dafcb4a098 324 movedown = false;
kbruil 9:19dafcb4a098 325 vy = (vy>maxspeed)?maxspeed:vy+ay;
kbruil 9:19dafcb4a098 326 }
kbruil 9:19dafcb4a098 327 else {
kbruil 9:19dafcb4a098 328 vy = !movedown?0:vy;
kbruil 9:19dafcb4a098 329 }
kbruil 9:19dafcb4a098 330 robot_setSetpoint(end.x, end.y+vy);
kbruil 9:19dafcb4a098 331 }
kbruil 9:19dafcb4a098 332 }
kbruil 6:3dcd9ce47900 333
kbruil 9:19dafcb4a098 334 void r_moveGripper(){
kbruil 9:19dafcb4a098 335 if(flag_switch){
kbruil 9:19dafcb4a098 336 if(!switch1.read() && !gripperclosed){
kbruil 9:19dafcb4a098 337 gripperpwm.pulsewidth_us(1035); // Close gripper
kbruil 9:19dafcb4a098 338 gripperclosed = true;
kbruil 9:19dafcb4a098 339 } else if(!switch2.read() && gripperclosed){
kbruil 9:19dafcb4a098 340 gripperpwm.pulsewidth_us(1500); // Open gripper
kbruil 9:19dafcb4a098 341 gripperclosed = false;
kbruil 9:19dafcb4a098 342 }
kbruil 9:19dafcb4a098 343 }
kbruil 9:19dafcb4a098 344 }
kbruil 9:19dafcb4a098 345
kbruil 9:19dafcb4a098 346 void r_processStateSwitch(){
kbruil 9:19dafcb4a098 347 if(flag_switch){
kbruil 6:3dcd9ce47900 348 if(switch3.read()){
kbruil 6:3dcd9ce47900 349 switch3down = false;
kbruil 6:3dcd9ce47900 350 }
kbruil 6:3dcd9ce47900 351 else {
kbruil 6:3dcd9ce47900 352 if (switch3down==false){
kbruil 6:3dcd9ce47900 353 switch3down = true;
kbruil 6:3dcd9ce47900 354 switch(state){
kbruil 6:3dcd9ce47900 355 case R_HORIZONTAL:
kbruil 6:3dcd9ce47900 356 state = R_VERTICAL;
kbruil 6:3dcd9ce47900 357 break;
kbruil 6:3dcd9ce47900 358 case R_VERTICAL:
kbruil 9:19dafcb4a098 359 state = R_GRIPPER;
kbruil 9:19dafcb4a098 360 break;
kbruil 9:19dafcb4a098 361 case R_GRIPPER:
kbruil 6:3dcd9ce47900 362 state = R_HORIZONTAL;
kbruil 6:3dcd9ce47900 363 break;
kbruil 9:19dafcb4a098 364
kbruil 6:3dcd9ce47900 365 }
kbruil 6:3dcd9ce47900 366 }
kbruil 6:3dcd9ce47900 367 }
kbruil 9:19dafcb4a098 368 }
kbruil 9:19dafcb4a098 369 }
kbruil 5:ca4f81348c30 370
kbruil 9:19dafcb4a098 371 void r_switchFlagReset(){
kbruil 9:19dafcb4a098 372 if (flag_switch){
kbruil 9:19dafcb4a098 373 flag_switch = false;
kbruil 4:89f7d7f3a7ca 374 }
kbruil 4:89f7d7f3a7ca 375 }
kbruil 4:89f7d7f3a7ca 376
kbruil 6:3dcd9ce47900 377
kbruil 4:89f7d7f3a7ca 378 void setmotor1(float val){
kbruil 9:19dafcb4a098 379 val = -val; // Inverse value for motor 1 because it rotates the other way
kbruil 4:89f7d7f3a7ca 380 motor1dir.write(val>=0?1:0);
kbruil 9:19dafcb4a098 381 motor1pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75)); // Use a root to make the motor give more power in low end.
kbruil 4:89f7d7f3a7ca 382 }
kbruil 4:89f7d7f3a7ca 383 void setmotor2(float val){
kbruil 4:89f7d7f3a7ca 384 motor2dir.write(val>=0?1:0);
kbruil 9:19dafcb4a098 385 motor2pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75)); // Use a root to make the motor give more power in low end.
kbruil 4:89f7d7f3a7ca 386 }
kbruil 4:89f7d7f3a7ca 387
kbruil 4:89f7d7f3a7ca 388
kbruil 6:3dcd9ce47900 389 double PID_control(double& olderr, double position, double setpoint, double& integrator, BiQuadChain& filt, double dt, double P, double I, double D){
kbruil 5:ca4f81348c30 390 double error;
kbruil 5:ca4f81348c30 391 double derivative;
kbruil 4:89f7d7f3a7ca 392 error = setpoint - position;
kbruil 4:89f7d7f3a7ca 393 integrator += error * dt;
kbruil 6:3dcd9ce47900 394 derivative = filt.step((error - olderr) / dt);
kbruil 4:89f7d7f3a7ca 395 olderr = error;
kbruil 4:89f7d7f3a7ca 396 return (P * error + I * integrator + D * derivative);
kbruil 4:89f7d7f3a7ca 397 }
kbruil 4:89f7d7f3a7ca 398
kbruil 4:89f7d7f3a7ca 399 void r_doPID(){
kbruil 9:19dafcb4a098 400 double dt = 1/50.0; // PID time step
kbruil 5:ca4f81348c30 401 double m1;
kbruil 5:ca4f81348c30 402 double m2;
kbruil 5:ca4f81348c30 403
kbruil 9:19dafcb4a098 404 if (flag_PID){ // Check PID ticker flag
kbruil 4:89f7d7f3a7ca 405 flag_PID = false;
kbruil 9:19dafcb4a098 406 motor1pos = -(double)motor1sense.getPulses()*(2*M_PI/8400.0); // Get motor 1 position. Negative, since motor spins opposite
kbruil 9:19dafcb4a098 407 motor2pos = (double)motor2sense.getPulses()*(2*M_PI/8400.0); // Get motor 2 position.
kbruil 9:19dafcb4a098 408 m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, motor1bqc_derivative, dt, 1, 0.2, 0.1); // PID motor 1
kbruil 9:19dafcb4a098 409 m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, motor2bqc_derivative, dt, 1, 0.2, 0.1); // PID motor 2
kbruil 9:19dafcb4a098 410 setmotor1(m1); // Set motor 1 speed
kbruil 9:19dafcb4a098 411 setmotor2(m2); // Set motor 2 speed
kbruil 4:89f7d7f3a7ca 412 }
kbruil 4:89f7d7f3a7ca 413 }
kbruil 4:89f7d7f3a7ca 414
kbruil 4:89f7d7f3a7ca 415 void r_outputToMatlab(){
kbruil 4:89f7d7f3a7ca 416 if (flag_output){
kbruil 4:89f7d7f3a7ca 417 flag_output = false;
kbruil 4:89f7d7f3a7ca 418 printf("Angle1\n\r");
kbruil 5:ca4f81348c30 419 printf("%f\n\r", arm1.theta);
kbruil 4:89f7d7f3a7ca 420 printf("Angle2\n\r");
kbruil 5:ca4f81348c30 421 printf("%f\n\r", arm2.theta);
kbruil 5:ca4f81348c30 422 printf("Pos1\n\r");
kbruil 5:ca4f81348c30 423 printf("%f\n\r", motor1pos);
kbruil 5:ca4f81348c30 424 printf("Pos2\n\r");
kbruil 4:89f7d7f3a7ca 425 printf("%f\n\r", motor2pos);
kbruil 5:ca4f81348c30 426
kbruil 4:89f7d7f3a7ca 427 printf("x: %f | y: %f\n\r", end.x, end.y);
kbruil 4:89f7d7f3a7ca 428 }
kbruil 4:89f7d7f3a7ca 429 }
kbruil 4:89f7d7f3a7ca 430
kbruil 9:19dafcb4a098 431 /*
kbruil 9:19dafcb4a098 432 ** maintickerfunction
kbruil 9:19dafcb4a098 433 **
kbruil 9:19dafcb4a098 434 ** Function: ticker call back. Sets flags at appropriate timer intervals for
kbruil 9:19dafcb4a098 435 ** reading switches, calculating PID, outputting information via serial and
kbruil 9:19dafcb4a098 436 ** reading EMG signals.
kbruil 9:19dafcb4a098 437 */
kbruil 4:89f7d7f3a7ca 438 void maintickerfunction (){
kbruil 4:89f7d7f3a7ca 439 static int cnt=0;
kbruil 4:89f7d7f3a7ca 440 if (cnt%20 == 0){ // 50 times per second
kbruil 4:89f7d7f3a7ca 441 flag_switch = true;
kbruil 4:89f7d7f3a7ca 442 }
kbruil 6:3dcd9ce47900 443 if (cnt%20 == 0){ // 50 times per second
kbruil 4:89f7d7f3a7ca 444 flag_PID = true;
kbruil 4:89f7d7f3a7ca 445 }
kbruil 4:89f7d7f3a7ca 446 if (cnt%100 == 0){ // 10 times per second
kbruil 4:89f7d7f3a7ca 447 flag_output = true;
kbruil 4:89f7d7f3a7ca 448 }
kbruil 5:ca4f81348c30 449 cnt++;
kbruil 4:89f7d7f3a7ca 450 }
kbruil 4:89f7d7f3a7ca 451
kbruil 6:3dcd9ce47900 452
kbruil 0:86fe02a64f0e 453 int main(){
kbruil 9:19dafcb4a098 454 // Initialise main ticker
kbruil 4:89f7d7f3a7ca 455 mainticker.attach(&maintickerfunction,0.001f);
kbruil 9:19dafcb4a098 456
kbruil 0:86fe02a64f0e 457 robot_init();
kbruil 0:86fe02a64f0e 458 while(true){
kbruil 4:89f7d7f3a7ca 459 switch(state){
kbruil 4:89f7d7f3a7ca 460 case R_INIT:
kbruil 4:89f7d7f3a7ca 461 break;
kbruil 4:89f7d7f3a7ca 462 case R_HORIZONTAL:
kbruil 4:89f7d7f3a7ca 463 r_moveHorizontal();
kbruil 4:89f7d7f3a7ca 464 break;
kbruil 6:3dcd9ce47900 465 case R_VERTICAL:
kbruil 6:3dcd9ce47900 466 r_moveVertical();
kbruil 6:3dcd9ce47900 467 break;
kbruil 9:19dafcb4a098 468 case R_GRIPPER:
kbruil 9:19dafcb4a098 469 r_moveGripper();
kbruil 9:19dafcb4a098 470 break;
kbruil 4:89f7d7f3a7ca 471 }
kbruil 9:19dafcb4a098 472 r_processStateSwitch();
kbruil 9:19dafcb4a098 473 r_switchFlagReset();
kbruil 4:89f7d7f3a7ca 474 r_doPID();
kbruil 4:89f7d7f3a7ca 475 r_outputToMatlab();
kbruil 0:86fe02a64f0e 476 }
kbruil 0:86fe02a64f0e 477 }