Kim Bruil / Mbed 2 deprecated r_robot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

main.cpp@6:3dcd9ce47900, 2016-10-25 (annotated)

Committer:
kbruil
Date:
Tue Oct 25 18:51:23 2016 +0000
Revision:
6:3dcd9ce47900
Parent:
5:ca4f81348c30
Child:
7:b09bfde57cba
PID control working

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 4:89f7d7f3a7ca 50 QEI motor1sense(D13,D12,NC, 8400);
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 5:ca4f81348c30 56 QEI motor2sense(D10,D11,NC, 8400);
kbruil 5:ca4f81348c30 57 double motor2pos=0;
kbruil 5:ca4f81348c30 58 double motor2int=0;
kbruil 5:ca4f81348c30 59 double motor2olderr=0;
kbruil 4:89f7d7f3a7ca 60
kbruil 6:3dcd9ce47900 61 BiQuadChain motor1bqc_derivative;
kbruil 6:3dcd9ce47900 62 BiQuadChain motor2bqc_derivative;
kbruil 6:3dcd9ce47900 63 BiQuad bq1(2.533015E-01, 5.066030E-01, 2.533015E-01, 1.000000E+00, -4.531195E-01, 4.663256E-01);
kbruil 6:3dcd9ce47900 64 BiQuad bq2(1.839030E-01, 3.678060E-01, 1.839030E-01, 1.000000E+00, -3.289757E-01, 6.458765E-02);
kbruil 6:3dcd9ce47900 65 BiQuad bq3(2.533015E-01, 5.066030E-01, 2.533015E-01, 1.000000E+00, -4.531195E-01, 4.663256E-01);
kbruil 6:3dcd9ce47900 66 BiQuad bq4(1.839030E-01, 3.678060E-01, 1.839030E-01, 1.000000E+00, -3.289757E-01, 6.458765E-02);
kbruil 6:3dcd9ce47900 67
kbruil 4:89f7d7f3a7ca 68
kbruil 2:027d19cda97b 69 float vx=0;
kbruil 2:027d19cda97b 70 float vy=0;
kbruil 0:86fe02a64f0e 71 // Struct r_link:
kbruil 0:86fe02a64f0e 72 // Defines a robot link type (arm or end effector).
kbruil 0:86fe02a64f0e 73 struct r_link {
kbruil 0:86fe02a64f0e 74 float length; // link length
kbruil 0:86fe02a64f0e 75 float x; // link x position
kbruil 0:86fe02a64f0e 76 float y; // link y position
kbruil 0:86fe02a64f0e 77 float theta; // link angle
kbruil 0:86fe02a64f0e 78 };
kbruil 0:86fe02a64f0e 79
kbruil 0:86fe02a64f0e 80 MODSERIAL pc(USBTX, USBRX);
kbruil 0:86fe02a64f0e 81 DigitalIn switch1(SW3);
kbruil 0:86fe02a64f0e 82 DigitalIn switch2(SW2);
kbruil 6:3dcd9ce47900 83 DigitalIn switch3(D9);
kbruil 6:3dcd9ce47900 84 bool switch3down=false;
kbruil 5:ca4f81348c30 85 //DigitalIn switch4(D9);
kbruil 0:86fe02a64f0e 86 DigitalOut ledr(LED_RED);
kbruil 0:86fe02a64f0e 87 DigitalOut ledg(LED_GREEN);
kbruil 0:86fe02a64f0e 88
kbruil 0:86fe02a64f0e 89
kbruil 0:86fe02a64f0e 90 r_link arm1; // Robot upper arm
kbruil 0:86fe02a64f0e 91 r_link arm2; // Robot lower arm
kbruil 0:86fe02a64f0e 92 r_link end; // Robot end effector
kbruil 0:86fe02a64f0e 93
kbruil 3:339b6f68b317 94 /*
kbruil 3:339b6f68b317 95 ** =============================================================================
kbruil 3:339b6f68b317 96 ** robot_applySetpointLimits
kbruil 3:339b6f68b317 97 ** =============================================================================
kbruil 3:339b6f68b317 98 ** Function:
kbruil 3:339b6f68b317 99 ** Limits the cartesian coordinates to table, length of robot and prevents
kbruil 3:339b6f68b317 100 ** the robot arm crossing it's own base.
kbruil 3:339b6f68b317 101 ** Input:
kbruil 3:339b6f68b317 102 ** float x, float y: position in cartesian coordinates
kbruil 3:339b6f68b317 103 ** Output:
kbruil 3:339b6f68b317 104 ** float x, float y: limited position in cartesian coordinates
kbruil 3:339b6f68b317 105 ** =============================================================================
kbruil 3:339b6f68b317 106 */
kbruil 3:339b6f68b317 107 void robot_applySetpointLimits (float& x, float& y){
kbruil 0:86fe02a64f0e 108 float z;
kbruil 0:86fe02a64f0e 109 float angle;
kbruil 0:86fe02a64f0e 110
kbruil 0:86fe02a64f0e 111 // Limit x,y to table top size
kbruil 3:339b6f68b317 112 if (x<R_XTABLE_LEFT) { x=R_XTABLE_LEFT; vx=0;}
kbruil 3:339b6f68b317 113 else if (x>R_XTABLE_RIGHT) { x=R_XTABLE_RIGHT; vx=0;}
kbruil 3:339b6f68b317 114 if (y<R_YTABLE_BOTTOM) { y=R_YTABLE_BOTTOM; vy=0;}
kbruil 3:339b6f68b317 115 else if (y>R_YTABLE_TOP) { y=R_YTABLE_TOP; vy=0; }
kbruil 3:339b6f68b317 116
kbruil 3:339b6f68b317 117 // Limit x,y to maximum reach of robot arm
kbruil 0:86fe02a64f0e 118 z = sqrt(pow(x,2)+pow(y,2));
kbruil 0:86fe02a64f0e 119 angle = atan2(y,x);
kbruil 0:86fe02a64f0e 120 if (z>(arm1.length+arm2.length)){
kbruil 0:86fe02a64f0e 121 z = arm1.length+arm2.length;
kbruil 0:86fe02a64f0e 122 x = z*cos(angle);
kbruil 0:86fe02a64f0e 123 y = z*sin(angle);
kbruil 0:86fe02a64f0e 124 }
kbruil 3:339b6f68b317 125
kbruil 3:339b6f68b317 126 // Prevent x,y from entering robot body
kbruil 3:339b6f68b317 127 if (end.x > R_ROBOT_LEFT && end.x < R_ROBOT_RIGHT && end.y >= R_ROBOT_TOP && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 128 y = R_ROBOT_TOP;
kbruil 3:339b6f68b317 129 }
kbruil 3:339b6f68b317 130 if (end.x <= R_ROBOT_LEFT && x > R_ROBOT_LEFT && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 131 x = R_ROBOT_LEFT;
kbruil 3:339b6f68b317 132 }
kbruil 3:339b6f68b317 133 if (end.x >= R_ROBOT_RIGHT && x < R_ROBOT_RIGHT && y < R_ROBOT_TOP) {
kbruil 3:339b6f68b317 134 x = R_ROBOT_RIGHT;
kbruil 3:339b6f68b317 135 }
kbruil 0:86fe02a64f0e 136 }
kbruil 0:86fe02a64f0e 137
kbruil 3:339b6f68b317 138 /*
kbruil 3:339b6f68b317 139 ** =============================================================================
kbruil 4:89f7d7f3a7ca 140 ** robot_applyAngleLimits
kbruil 4:89f7d7f3a7ca 141 ** =============================================================================
kbruil 4:89f7d7f3a7ca 142 ** Function:
kbruil 4:89f7d7f3a7ca 143 ** Limits the angles of the robot's arms to prevent them from moving into
kbruil 4:89f7d7f3a7ca 144 ** themselves
kbruil 4:89f7d7f3a7ca 145 ** Input: -
kbruil 4:89f7d7f3a7ca 146 ** Output: -
kbruil 4:89f7d7f3a7ca 147 ** =============================================================================
kbruil 4:89f7d7f3a7ca 148 */
kbruil 4:89f7d7f3a7ca 149 void robot_applyAngleLimits(){
kbruil 4:89f7d7f3a7ca 150 if (arm1.theta>ARM1_ALIMLO){ // Limit angle arm1
kbruil 4:89f7d7f3a7ca 151 arm1.theta = ARM1_ALIMLO;
kbruil 4:89f7d7f3a7ca 152 }
kbruil 4:89f7d7f3a7ca 153 else if (arm1.theta<ARM1_ALIMHI){
kbruil 4:89f7d7f3a7ca 154 arm1.theta = ARM1_ALIMHI;
kbruil 4:89f7d7f3a7ca 155 }
kbruil 4:89f7d7f3a7ca 156
kbruil 4:89f7d7f3a7ca 157 if (arm2.theta>ARM2_ALIMHI){ // Limit angle arm 2
kbruil 4:89f7d7f3a7ca 158 arm2.theta = ARM2_ALIMHI;
kbruil 4:89f7d7f3a7ca 159 } else if (arm2.theta < ARM2_ALIMLO) {
kbruil 4:89f7d7f3a7ca 160 arm2.theta = ARM2_ALIMLO;
kbruil 4:89f7d7f3a7ca 161 }
kbruil 4:89f7d7f3a7ca 162 }
kbruil 4:89f7d7f3a7ca 163
kbruil 4:89f7d7f3a7ca 164 /*
kbruil 4:89f7d7f3a7ca 165 ** =============================================================================
kbruil 4:89f7d7f3a7ca 166 ** robot_setSetpoint
kbruil 3:339b6f68b317 167 ** =============================================================================
kbruil 3:339b6f68b317 168 ** Function:
kbruil 3:339b6f68b317 169 ** Transforms cartesian coordinates into rotadions of the joints of the robot's
kbruil 3:339b6f68b317 170 ** arms.
kbruil 3:339b6f68b317 171 ** Input:
kbruil 4:89f7d7f3a7ca 172 ** float x, float y: setpoint position in cartesian coordinates
kbruil 4:89f7d7f3a7ca 173 ** =============================================================================
kbruil 4:89f7d7f3a7ca 174 ** (x,y)
kbruil 4:89f7d7f3a7ca 175 ** |\
kbruil 4:89f7d7f3a7ca 176 ** | \
kbruil 4:89f7d7f3a7ca 177 ** |b2\
kbruil 4:89f7d7f3a7ca 178 ** z2 | \arm2.length
kbruil 4:89f7d7f3a7ca 179 ** | \
kbruil 4:89f7d7f3a7ca 180 ** z |_ _ _\
kbruil 4:89f7d7f3a7ca 181 ** | /
kbruil 4:89f7d7f3a7ca 182 ** | /
kbruil 4:89f7d7f3a7ca 183 ** z1 | /arm1.length
kbruil 4:89f7d7f3a7ca 184 ** |b1/
kbruil 4:89f7d7f3a7ca 185 ** | /
kbruil 4:89f7d7f3a7ca 186 ** |/
kbruil 3:339b6f68b317 187 ** =============================================================================
kbruil 3:339b6f68b317 188 */
kbruil 3:339b6f68b317 189 void robot_setSetpoint (float x,float y){
kbruil 3:339b6f68b317 190 float z; // length z of vector (x,y)
kbruil 3:339b6f68b317 191 float z1; // part z1
kbruil 3:339b6f68b317 192 float z2; // part z2
kbruil 3:339b6f68b317 193
kbruil 3:339b6f68b317 194 float angle; // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0!
kbruil 3:339b6f68b317 195 float b1,b2; // angle b1 and b2 (see drawing)
kbruil 0:86fe02a64f0e 196
kbruil 3:339b6f68b317 197 robot_applySetpointLimits(x,y); // Apply limits
kbruil 1:ffc1a2d6a824 198
kbruil 4:89f7d7f3a7ca 199 z = sqrt(pow(x,2)+pow(y,2)); // Calculate vector (x,y) length
kbruil 3:339b6f68b317 200
kbruil 4:89f7d7f3a7ca 201 // Calculate vector (x,y) angle
kbruil 3:339b6f68b317 202 angle = atan2(y,x); // Calculate (x,y) angle using atan 2
kbruil 3:339b6f68b317 203 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 204
kbruil 1:ffc1a2d6a824 205 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 206 }
kbruil 1:ffc1a2d6a824 207 else if (angle<M_PI && angle>0.5*M_PI){
kbruil 1:ffc1a2d6a824 208 angle = angle - 0.5*M_PI;
kbruil 0:86fe02a64f0e 209 }
kbruil 1:ffc1a2d6a824 210 else if (angle<0 && angle>-0.5*M_PI){
kbruil 1:ffc1a2d6a824 211 angle = angle - 0.5 * M_PI;
kbruil 1:ffc1a2d6a824 212 }
kbruil 1:ffc1a2d6a824 213 else{
kbruil 1:ffc1a2d6a824 214 angle = angle + 1.5*M_PI;
kbruil 1:ffc1a2d6a824 215 }
kbruil 3:339b6f68b317 216 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 217 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 218
kbruil 3:339b6f68b317 219 z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z); // Calculate z1
kbruil 3:339b6f68b317 220 z2 = z-z1; // Calculate z2
kbruil 3:339b6f68b317 221 // Roundoff errors sometimes make z1 and z2 slightly higher than the arm lengths, this causes erratic behaviour in the acos function,
kbruil 3:339b6f68b317 222 // so it is countered here by an extra check and fix.
kbruil 1:ffc1a2d6a824 223 if (z1>arm1.length) {z1 = arm1.length;}
kbruil 1:ffc1a2d6a824 224 else if (z1<-arm1.length) { z1 = -arm1.length;}
kbruil 1:ffc1a2d6a824 225 if (z2>arm2.length) {z2 = arm2.length;}
kbruil 1:ffc1a2d6a824 226 else if (z2<-arm2.length) { z2 = -arm2.length;}
kbruil 0:86fe02a64f0e 227
kbruil 3:339b6f68b317 228 b1 = acos(z1/arm1.length); // Calculate angle b1
kbruil 3:339b6f68b317 229 b2 = acos(z2/arm2.length); // Calculate angle b2
kbruil 0:86fe02a64f0e 230
kbruil 3:339b6f68b317 231 arm1.theta = angle - b1; // Calculate new angle arm 1
kbruil 3:339b6f68b317 232 arm2.theta = b1 + b2; // Calculate new angle arm 2
kbruil 4:89f7d7f3a7ca 233 robot_applyAngleLimits();
kbruil 4:89f7d7f3a7ca 234
kbruil 3:339b6f68b317 235 arm2.x = arm1.length*-sin(arm1.theta); // Calculate new x position arm 2
kbruil 3:339b6f68b317 236 arm2.y = arm1.length*cos(arm1.theta); // Calculate new y position arm 2
kbruil 3:339b6f68b317 237
kbruil 3:339b6f68b317 238 end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector
kbruil 3:339b6f68b317 239 end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y; // Calculate new y position end effector
kbruil 0:86fe02a64f0e 240 }
kbruil 0:86fe02a64f0e 241
kbruil 0:86fe02a64f0e 242 // robot_init
kbruil 0:86fe02a64f0e 243 // Initialise robot
kbruil 0:86fe02a64f0e 244 void robot_init() {
kbruil 4:89f7d7f3a7ca 245 state = R_HORIZONTAL;
kbruil 0:86fe02a64f0e 246 // Init arm1 (upper arm) first link
kbruil 0:86fe02a64f0e 247 arm1.length = 28.0f;
kbruil 4:89f7d7f3a7ca 248 arm1.x = 0;
kbruil 4:89f7d7f3a7ca 249 arm1.y = 0;
kbruil 1:ffc1a2d6a824 250 arm1.theta = 0;
kbruil 0:86fe02a64f0e 251
kbruil 0:86fe02a64f0e 252 // Init arm2 (lower arm) second link
kbruil 0:86fe02a64f0e 253 arm2.length = 35.0f;
kbruil 4:89f7d7f3a7ca 254 arm2.x = 0;
kbruil 4:89f7d7f3a7ca 255 arm2.y = arm1.length;
kbruil 1:ffc1a2d6a824 256 arm2.theta = 0;
kbruil 0:86fe02a64f0e 257
kbruil 0:86fe02a64f0e 258 // Init end (end effector) third/last link
kbruil 4:89f7d7f3a7ca 259 end.length = 5.0f;
kbruil 4:89f7d7f3a7ca 260 end.x = 0;
kbruil 4:89f7d7f3a7ca 261 end.y = arm1.length+arm2.length;
kbruil 0:86fe02a64f0e 262 end.theta = 0;
kbruil 5:ca4f81348c30 263
kbruil 5:ca4f81348c30 264 motor1sense.reset();
kbruil 5:ca4f81348c30 265 motor2sense.reset();
kbruil 0:86fe02a64f0e 266 }
kbruil 0:86fe02a64f0e 267
kbruil 0:86fe02a64f0e 268 int lr_state=0;
kbruil 0:86fe02a64f0e 269 int ud_state=0;
kbruil 0:86fe02a64f0e 270 int sw2_state=0;
kbruil 0:86fe02a64f0e 271 float ax=0.1;
kbruil 0:86fe02a64f0e 272 float ay=0.1;
kbruil 2:027d19cda97b 273
kbruil 1:ffc1a2d6a824 274 float spd=1.0;
kbruil 0:86fe02a64f0e 275
kbruil 0:86fe02a64f0e 276 void inputswitches()
kbruil 0:86fe02a64f0e 277 {
kbruil 5:ca4f81348c30 278 /* if(switch1.read())
kbruil 0:86fe02a64f0e 279 {
kbruil 0:86fe02a64f0e 280 if (lr_state==0){
kbruil 0:86fe02a64f0e 281 vx=0;
kbruil 0:86fe02a64f0e 282 ledr.write(1);
kbruil 0:86fe02a64f0e 283 }
kbruil 0:86fe02a64f0e 284 }
kbruil 0:86fe02a64f0e 285 else
kbruil 0:86fe02a64f0e 286 {
kbruil 0:86fe02a64f0e 287 lr_state=0;
kbruil 0:86fe02a64f0e 288 vx = vx - ax;
kbruil 0:86fe02a64f0e 289 if (vx<-3.0f){
kbruil 0:86fe02a64f0e 290 vx=-3.0f;
kbruil 0:86fe02a64f0e 291 }
kbruil 0:86fe02a64f0e 292 ledr.write(0);
kbruil 0:86fe02a64f0e 293 }
kbruil 0:86fe02a64f0e 294
kbruil 0:86fe02a64f0e 295 if(switch2.read())
kbruil 0:86fe02a64f0e 296 {
kbruil 0:86fe02a64f0e 297 if(lr_state==1){
kbruil 0:86fe02a64f0e 298 vx=0;
kbruil 0:86fe02a64f0e 299 ledr.write(1);
kbruil 0:86fe02a64f0e 300 }
kbruil 0:86fe02a64f0e 301 }
kbruil 0:86fe02a64f0e 302 else
kbruil 0:86fe02a64f0e 303 {
kbruil 0:86fe02a64f0e 304 lr_state=1;
kbruil 0:86fe02a64f0e 305 vx = vx + ax;
kbruil 0:86fe02a64f0e 306 if (vx>3.0f){
kbruil 0:86fe02a64f0e 307 vx=3.0f;
kbruil 0:86fe02a64f0e 308 }
kbruil 0:86fe02a64f0e 309 ledr.write(0);
kbruil 0:86fe02a64f0e 310 }
kbruil 0:86fe02a64f0e 311 if(switch3.read())
kbruil 0:86fe02a64f0e 312 {
kbruil 0:86fe02a64f0e 313 if (ud_state==0){
kbruil 0:86fe02a64f0e 314 vy=0;
kbruil 0:86fe02a64f0e 315 ledr.write(1);
kbruil 0:86fe02a64f0e 316 }
kbruil 0:86fe02a64f0e 317 }
kbruil 0:86fe02a64f0e 318 else
kbruil 0:86fe02a64f0e 319 {
kbruil 0:86fe02a64f0e 320 ud_state=0;
kbruil 0:86fe02a64f0e 321 vy = vy - ay;
kbruil 0:86fe02a64f0e 322 if (vy<-3.0f){
kbruil 0:86fe02a64f0e 323 vy=-3.0f;
kbruil 0:86fe02a64f0e 324 }
kbruil 0:86fe02a64f0e 325 ledr.write(0);
kbruil 0:86fe02a64f0e 326 }
kbruil 0:86fe02a64f0e 327
kbruil 0:86fe02a64f0e 328 if(switch4.read())
kbruil 0:86fe02a64f0e 329 {
kbruil 0:86fe02a64f0e 330 if(ud_state==1){
kbruil 0:86fe02a64f0e 331 vy=0;
kbruil 0:86fe02a64f0e 332 ledr.write(1);
kbruil 0:86fe02a64f0e 333 }
kbruil 0:86fe02a64f0e 334 }
kbruil 0:86fe02a64f0e 335 else
kbruil 0:86fe02a64f0e 336 {
kbruil 0:86fe02a64f0e 337 ud_state=1;
kbruil 0:86fe02a64f0e 338 vy = vy + ay;
kbruil 0:86fe02a64f0e 339 if (vy>3.0f){
kbruil 0:86fe02a64f0e 340 vy=3.0f;
kbruil 0:86fe02a64f0e 341 }
kbruil 0:86fe02a64f0e 342 ledr.write(0);
kbruil 0:86fe02a64f0e 343 }
kbruil 5:ca4f81348c30 344 */
kbruil 2:027d19cda97b 345 /*
kbruil 1:ffc1a2d6a824 346 if (switch1.read()){
kbruil 1:ffc1a2d6a824 347 end.x += spd;
kbruil 1:ffc1a2d6a824 348 }
kbruil 1:ffc1a2d6a824 349 if (switch2.read()){
kbruil 1:ffc1a2d6a824 350 end.x -= spd;
kbruil 1:ffc1a2d6a824 351 }
kbruil 1:ffc1a2d6a824 352 if (switch3.read()){
kbruil 1:ffc1a2d6a824 353 end.y += spd;
kbruil 1:ffc1a2d6a824 354 }
kbruil 1:ffc1a2d6a824 355 if (switch4.read()){
kbruil 1:ffc1a2d6a824 356 end.y -= spd;
kbruil 1:ffc1a2d6a824 357 }
kbruil 2:027d19cda97b 358 */
kbruil 0:86fe02a64f0e 359 }
kbruil 0:86fe02a64f0e 360
kbruil 6:3dcd9ce47900 361 const double maxspeed=0.5f;
kbruil 6:3dcd9ce47900 362
kbruil 4:89f7d7f3a7ca 363 void r_moveHorizontal (){
kbruil 4:89f7d7f3a7ca 364 if (flag_switch){ // Read switches and set setpoint once per switch ticker milliseconds
kbruil 4:89f7d7f3a7ca 365 flag_switch = false;
kbruil 6:3dcd9ce47900 366 if(switch3.read()){
kbruil 6:3dcd9ce47900 367 switch3down = false;
kbruil 6:3dcd9ce47900 368 }
kbruil 6:3dcd9ce47900 369 else {
kbruil 6:3dcd9ce47900 370 if (switch3down==false){
kbruil 6:3dcd9ce47900 371 switch3down = true;
kbruil 6:3dcd9ce47900 372 switch(state){
kbruil 6:3dcd9ce47900 373 case R_HORIZONTAL:
kbruil 6:3dcd9ce47900 374 state = R_VERTICAL;
kbruil 6:3dcd9ce47900 375 break;
kbruil 6:3dcd9ce47900 376 case R_VERTICAL:
kbruil 6:3dcd9ce47900 377 state = R_HORIZONTAL;
kbruil 6:3dcd9ce47900 378 break;
kbruil 6:3dcd9ce47900 379 }
kbruil 6:3dcd9ce47900 380 }
kbruil 6:3dcd9ce47900 381 }
kbruil 4:89f7d7f3a7ca 382 if(switch1.read())
kbruil 4:89f7d7f3a7ca 383 {
kbruil 4:89f7d7f3a7ca 384 if (lr_state==0){
kbruil 4:89f7d7f3a7ca 385 vx=0;
kbruil 4:89f7d7f3a7ca 386 ledr.write(1);
kbruil 4:89f7d7f3a7ca 387 }
kbruil 4:89f7d7f3a7ca 388 }
kbruil 4:89f7d7f3a7ca 389 else
kbruil 4:89f7d7f3a7ca 390 {
kbruil 4:89f7d7f3a7ca 391 lr_state=0;
kbruil 5:ca4f81348c30 392 motor1int = 0;
kbruil 5:ca4f81348c30 393 motor2int = 0;
kbruil 5:ca4f81348c30 394
kbruil 4:89f7d7f3a7ca 395 vx = vx - ax;
kbruil 6:3dcd9ce47900 396 if (vx<-maxspeed){
kbruil 6:3dcd9ce47900 397 vx=-maxspeed;
kbruil 4:89f7d7f3a7ca 398 }
kbruil 4:89f7d7f3a7ca 399 ledr.write(0);
kbruil 4:89f7d7f3a7ca 400 }
kbruil 4:89f7d7f3a7ca 401
kbruil 4:89f7d7f3a7ca 402 if(switch2.read())
kbruil 4:89f7d7f3a7ca 403 {
kbruil 4:89f7d7f3a7ca 404 if(lr_state==1){
kbruil 4:89f7d7f3a7ca 405 vx=0;
kbruil 5:ca4f81348c30 406
kbruil 4:89f7d7f3a7ca 407 ledr.write(1);
kbruil 4:89f7d7f3a7ca 408 }
kbruil 4:89f7d7f3a7ca 409 }
kbruil 4:89f7d7f3a7ca 410 else
kbruil 4:89f7d7f3a7ca 411 {
kbruil 4:89f7d7f3a7ca 412 lr_state=1;
kbruil 5:ca4f81348c30 413 motor1int = 0;
kbruil 5:ca4f81348c30 414 motor2int = 0;
kbruil 4:89f7d7f3a7ca 415 vx = vx + ax;
kbruil 6:3dcd9ce47900 416 if (vx>maxspeed){
kbruil 6:3dcd9ce47900 417 vx=maxspeed;
kbruil 4:89f7d7f3a7ca 418 }
kbruil 4:89f7d7f3a7ca 419 ledr.write(0);
kbruil 4:89f7d7f3a7ca 420 }
kbruil 5:ca4f81348c30 421 robot_setSetpoint(end.x+vx, end.y);
kbruil 4:89f7d7f3a7ca 422 }
kbruil 4:89f7d7f3a7ca 423 }
kbruil 4:89f7d7f3a7ca 424
kbruil 6:3dcd9ce47900 425 void r_moveVertical (){
kbruil 6:3dcd9ce47900 426 if (flag_switch){ // Read switches and set setpoint once per switch ticker milliseconds
kbruil 6:3dcd9ce47900 427 flag_switch = false;
kbruil 6:3dcd9ce47900 428 if(switch3.read()){
kbruil 6:3dcd9ce47900 429 switch3down = false;
kbruil 6:3dcd9ce47900 430 }
kbruil 6:3dcd9ce47900 431 else {
kbruil 6:3dcd9ce47900 432 if (switch3down==false){
kbruil 6:3dcd9ce47900 433 switch3down = true;
kbruil 6:3dcd9ce47900 434 switch(state){
kbruil 6:3dcd9ce47900 435 case R_HORIZONTAL:
kbruil 6:3dcd9ce47900 436 state = R_VERTICAL;
kbruil 6:3dcd9ce47900 437 break;
kbruil 6:3dcd9ce47900 438 case R_VERTICAL:
kbruil 6:3dcd9ce47900 439 state = R_HORIZONTAL;
kbruil 6:3dcd9ce47900 440 break;
kbruil 6:3dcd9ce47900 441 }
kbruil 6:3dcd9ce47900 442 }
kbruil 6:3dcd9ce47900 443 }
kbruil 6:3dcd9ce47900 444
kbruil 6:3dcd9ce47900 445 vx = 0;
kbruil 6:3dcd9ce47900 446 if(switch1.read())
kbruil 6:3dcd9ce47900 447 {
kbruil 6:3dcd9ce47900 448 if (ud_state==0){
kbruil 6:3dcd9ce47900 449 vy=0;
kbruil 6:3dcd9ce47900 450 ledr.write(1);
kbruil 6:3dcd9ce47900 451 }
kbruil 6:3dcd9ce47900 452 }
kbruil 6:3dcd9ce47900 453 else
kbruil 6:3dcd9ce47900 454 {
kbruil 6:3dcd9ce47900 455 ud_state=0;
kbruil 6:3dcd9ce47900 456 motor1int = 0;
kbruil 6:3dcd9ce47900 457 motor2int = 0;
kbruil 6:3dcd9ce47900 458
kbruil 6:3dcd9ce47900 459 vy = vy - ay;
kbruil 6:3dcd9ce47900 460 if (vy<-maxspeed){
kbruil 6:3dcd9ce47900 461 vy=-maxspeed;
kbruil 6:3dcd9ce47900 462 }
kbruil 6:3dcd9ce47900 463 ledr.write(0);
kbruil 6:3dcd9ce47900 464 }
kbruil 6:3dcd9ce47900 465
kbruil 6:3dcd9ce47900 466 if(switch2.read())
kbruil 6:3dcd9ce47900 467 {
kbruil 6:3dcd9ce47900 468 if(ud_state==1){
kbruil 6:3dcd9ce47900 469 vy=0;
kbruil 6:3dcd9ce47900 470
kbruil 6:3dcd9ce47900 471 ledr.write(1);
kbruil 6:3dcd9ce47900 472 }
kbruil 6:3dcd9ce47900 473 }
kbruil 6:3dcd9ce47900 474 else
kbruil 6:3dcd9ce47900 475 {
kbruil 6:3dcd9ce47900 476 ud_state=1;
kbruil 6:3dcd9ce47900 477 motor1int = 0;
kbruil 6:3dcd9ce47900 478 motor2int = 0;
kbruil 6:3dcd9ce47900 479 vy = vy + ay;
kbruil 6:3dcd9ce47900 480 if (vy>maxspeed){
kbruil 6:3dcd9ce47900 481 vy=maxspeed;
kbruil 6:3dcd9ce47900 482 }
kbruil 6:3dcd9ce47900 483 ledr.write(0);
kbruil 6:3dcd9ce47900 484 }
kbruil 6:3dcd9ce47900 485 robot_setSetpoint(end.x, end.y+vy);
kbruil 6:3dcd9ce47900 486 }
kbruil 6:3dcd9ce47900 487 }
kbruil 6:3dcd9ce47900 488
kbruil 4:89f7d7f3a7ca 489 void setmotor1(float val){
kbruil 4:89f7d7f3a7ca 490 motor1dir.write(val>=0?1:0);
kbruil 5:ca4f81348c30 491 motor1pwm.write(fabs(val)>1?1.0f:sqrt(fabs(val)));
kbruil 4:89f7d7f3a7ca 492 }
kbruil 4:89f7d7f3a7ca 493 void setmotor2(float val){
kbruil 4:89f7d7f3a7ca 494 motor2dir.write(val>=0?1:0);
kbruil 5:ca4f81348c30 495 motor2pwm.write(fabs(val)>1?1.0f:sqrt(fabs(val)));
kbruil 4:89f7d7f3a7ca 496 }
kbruil 4:89f7d7f3a7ca 497
kbruil 4:89f7d7f3a7ca 498
kbruil 6:3dcd9ce47900 499 double PID_control(double& olderr, double position, double setpoint, double& integrator, BiQuadChain& filt, double dt, double P, double I, double D){
kbruil 5:ca4f81348c30 500 double error;
kbruil 5:ca4f81348c30 501 double derivative;
kbruil 4:89f7d7f3a7ca 502 error = setpoint - position;
kbruil 4:89f7d7f3a7ca 503 integrator += error * dt;
kbruil 6:3dcd9ce47900 504 derivative = filt.step((error - olderr) / dt);
kbruil 4:89f7d7f3a7ca 505 olderr = error;
kbruil 4:89f7d7f3a7ca 506 return (P * error + I * integrator + D * derivative);
kbruil 4:89f7d7f3a7ca 507 }
kbruil 4:89f7d7f3a7ca 508
kbruil 4:89f7d7f3a7ca 509 void r_doPID(){
kbruil 5:ca4f81348c30 510 double dt = 1/50.0;
kbruil 5:ca4f81348c30 511 double m1;
kbruil 5:ca4f81348c30 512 double m2;
kbruil 5:ca4f81348c30 513
kbruil 4:89f7d7f3a7ca 514 if (flag_PID){
kbruil 4:89f7d7f3a7ca 515 flag_PID = false;
kbruil 5:ca4f81348c30 516 motor1pos = (double)motor1sense.getPulses()*(2*M_PI/8400.0);
kbruil 5:ca4f81348c30 517 motor2pos = (double)motor2sense.getPulses()*(2*M_PI/8400.0);
kbruil 6:3dcd9ce47900 518 m1 = PID_control(motor1olderr, motor1pos, arm1.theta, motor1int, motor1bqc_derivative, dt, 0.2, 0.1, 0.1);
kbruil 6:3dcd9ce47900 519 m2 = PID_control(motor2olderr, motor2pos, arm2.theta, motor2int, motor2bqc_derivative, dt, 0.2, 0.1, 0.1);
kbruil 6:3dcd9ce47900 520
kbruil 4:89f7d7f3a7ca 521 setmotor1(m1);
kbruil 4:89f7d7f3a7ca 522 setmotor2(m2);
kbruil 6:3dcd9ce47900 523 /* printf("e1: %f, e2: %f\n\r", motor1olderr, motor2olderr);
kbruil 5:ca4f81348c30 524 printf("p1: %f, p2: %f\n\r", motor1pos, motor2pos);
kbruil 5:ca4f81348c30 525 printf("m1: %f, m2: %f\n\r", m1, m2);
kbruil 6:3dcd9ce47900 526 */
kbruil 4:89f7d7f3a7ca 527 }
kbruil 4:89f7d7f3a7ca 528 }
kbruil 4:89f7d7f3a7ca 529
kbruil 4:89f7d7f3a7ca 530 void r_outputToMatlab(){
kbruil 4:89f7d7f3a7ca 531 if (flag_output){
kbruil 4:89f7d7f3a7ca 532 flag_output = false;
kbruil 4:89f7d7f3a7ca 533 printf("Angle1\n\r");
kbruil 5:ca4f81348c30 534 printf("%f\n\r", arm1.theta);
kbruil 4:89f7d7f3a7ca 535 printf("Angle2\n\r");
kbruil 5:ca4f81348c30 536 printf("%f\n\r", arm2.theta);
kbruil 5:ca4f81348c30 537 printf("Pos1\n\r");
kbruil 5:ca4f81348c30 538 printf("%f\n\r", motor1pos);
kbruil 5:ca4f81348c30 539 printf("Pos2\n\r");
kbruil 4:89f7d7f3a7ca 540 printf("%f\n\r", motor2pos);
kbruil 5:ca4f81348c30 541
kbruil 4:89f7d7f3a7ca 542 printf("x: %f | y: %f\n\r", end.x, end.y);
kbruil 4:89f7d7f3a7ca 543 }
kbruil 4:89f7d7f3a7ca 544 }
kbruil 4:89f7d7f3a7ca 545
kbruil 4:89f7d7f3a7ca 546 void maintickerfunction (){
kbruil 4:89f7d7f3a7ca 547 static int cnt=0;
kbruil 4:89f7d7f3a7ca 548 if (cnt%20 == 0){ // 50 times per second
kbruil 4:89f7d7f3a7ca 549 flag_switch = true;
kbruil 4:89f7d7f3a7ca 550 }
kbruil 6:3dcd9ce47900 551 if (cnt%20 == 0){ // 50 times per second
kbruil 4:89f7d7f3a7ca 552 flag_PID = true;
kbruil 4:89f7d7f3a7ca 553 }
kbruil 4:89f7d7f3a7ca 554 if (cnt%100 == 0){ // 10 times per second
kbruil 4:89f7d7f3a7ca 555 flag_output = true;
kbruil 4:89f7d7f3a7ca 556 }
kbruil 5:ca4f81348c30 557 cnt++;
kbruil 4:89f7d7f3a7ca 558 }
kbruil 4:89f7d7f3a7ca 559
kbruil 6:3dcd9ce47900 560
kbruil 0:86fe02a64f0e 561 int main(){
kbruil 6:3dcd9ce47900 562 motor1bqc_derivative.add(&bq1).add(&bq2);
kbruil 6:3dcd9ce47900 563 motor2bqc_derivative.add(&bq3).add(&bq4);
kbruil 4:89f7d7f3a7ca 564 mainticker.attach(&maintickerfunction,0.001f);
kbruil 0:86fe02a64f0e 565 pc.baud(115200);
kbruil 0:86fe02a64f0e 566 robot_init();
kbruil 0:86fe02a64f0e 567 while(true){
kbruil 4:89f7d7f3a7ca 568 switch(state){
kbruil 4:89f7d7f3a7ca 569 case R_INIT:
kbruil 4:89f7d7f3a7ca 570 break;
kbruil 4:89f7d7f3a7ca 571 case R_HORIZONTAL:
kbruil 4:89f7d7f3a7ca 572 r_moveHorizontal();
kbruil 4:89f7d7f3a7ca 573 break;
kbruil 6:3dcd9ce47900 574 case R_VERTICAL:
kbruil 6:3dcd9ce47900 575 r_moveVertical();
kbruil 6:3dcd9ce47900 576 break;
kbruil 4:89f7d7f3a7ca 577 }
kbruil 4:89f7d7f3a7ca 578 r_doPID();
kbruil 4:89f7d7f3a7ca 579 r_outputToMatlab();
kbruil 0:86fe02a64f0e 580 }
kbruil 0:86fe02a64f0e 581 }