Kim Bruil / Mbed 2 deprecated r_robot

robot positie error test ding

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

main.cpp@4:89f7d7f3a7ca, 2016-10-25 (annotated)

Committer:
kbruil
Date:
Tue Oct 25 15:23:25 2016 +0000
Revision:
4:89f7d7f3a7ca
Parent:
3:339b6f68b317
Child:
5:ca4f81348c30
PID controller version first draft

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