groep 16 / Mbed 2 deprecated Switches2

Switches 2.0

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@9:0e838367ab6a, 2019-11-01 (annotated)

Committer:
JonaVonk
Date:
Fri Nov 01 10:23:02 2019 +0000
Revision:
9:0e838367ab6a
Parent:
8:b40067b8a72d
Child:
10:cbcb35182ef1
deze werkt bijna

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
JonaVonk 9:0e838367ab6a 2 #include "FilterDesign.h"
JonaVonk 9:0e838367ab6a 3 #include "HIDScope.h"
JonaVonk 2:96e093a48314 4 #include "QEI.h"
RobertoO 1:b862262a9d14 5 #include "MODSERIAL.h"
JonaVonk 9:0e838367ab6a 6 #include "BiQuad4.h"
JonaVonk 9:0e838367ab6a 7 #include "BiQuad.h"
JonaVonk 9:0e838367ab6a 8 #include "FastPWM.h"
JonaVonk 2:96e093a48314 9 #include <vector>
RobertoO 0:67c50348f842 10
JonaVonk 9:0e838367ab6a 11
JonaVonk 9:0e838367ab6a 12 MODSERIAL pc(USBTX, USBRX);
JonaVonk 9:0e838367ab6a 13
JonaVonk 9:0e838367ab6a 14 // main() runs in its own thread in the OS
JonaVonk 9:0e838367ab6a 15
JonaVonk 9:0e838367ab6a 16 //State machine
JonaVonk 9:0e838367ab6a 17 InterruptIn stateSwitch(SW2);
JonaVonk 9:0e838367ab6a 18 InterruptIn stepSwitch(SW3);
JonaVonk 9:0e838367ab6a 19
JonaVonk 9:0e838367ab6a 20 void stateMachine();
JonaVonk 9:0e838367ab6a 21 void switchStep();
JonaVonk 9:0e838367ab6a 22 void switchState();
JonaVonk 9:0e838367ab6a 23
JonaVonk 9:0e838367ab6a 24 int noStateSteps = 4;
JonaVonk 9:0e838367ab6a 25 int stateStep = 0;
JonaVonk 9:0e838367ab6a 26 int state = 0;
JonaVonk 9:0e838367ab6a 27
JonaVonk 9:0e838367ab6a 28 void EMGState();
JonaVonk 9:0e838367ab6a 29 void demoState();
JonaVonk 2:96e093a48314 30
JonaVonk 9:0e838367ab6a 31 //Shared steps
JonaVonk 9:0e838367ab6a 32 void calibrateMotors();
JonaVonk 9:0e838367ab6a 33 void waiting();
JonaVonk 9:0e838367ab6a 34
JonaVonk 9:0e838367ab6a 35 //Demo steps
JonaVonk 9:0e838367ab6a 36 void playDemo();
JonaVonk 9:0e838367ab6a 37
JonaVonk 9:0e838367ab6a 38
JonaVonk 9:0e838367ab6a 39 //EMG steps
JonaVonk 9:0e838367ab6a 40 void EMGcalibration();
JonaVonk 9:0e838367ab6a 41 void moveToInitialPosition();
JonaVonk 9:0e838367ab6a 42 void moveWithEMG();
JonaVonk 9:0e838367ab6a 43
JonaVonk 9:0e838367ab6a 44 //Calibration
JonaVonk 9:0e838367ab6a 45 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed);
JonaVonk 9:0e838367ab6a 46
JonaVonk 9:0e838367ab6a 47 //play demo
JonaVonk 9:0e838367ab6a 48 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed);
JonaVonk 9:0e838367ab6a 49
JonaVonk 9:0e838367ab6a 50 //Motors
JonaVonk 2:96e093a48314 51 double Pi = 3.14159265359;
JonaVonk 2:96e093a48314 52
JonaVonk 2:96e093a48314 53 QEI Enc1(D12, D13, NC, 32);
JonaVonk 2:96e093a48314 54 QEI Enc2(D10, D11, NC, 32);
JonaVonk 2:96e093a48314 55
JonaVonk 2:96e093a48314 56 DigitalOut M1(D4);
JonaVonk 2:96e093a48314 57 DigitalOut M2(D7);
JonaVonk 2:96e093a48314 58
JonaVonk 2:96e093a48314 59 PwmOut E1(D5);
JonaVonk 2:96e093a48314 60 PwmOut E2(D6);
JonaVonk 2:96e093a48314 61
JonaVonk 7:80baf171503c 62 double gearRatio = 40/9;
JonaVonk 7:80baf171503c 63
JonaVonk 2:96e093a48314 64 double initRot1 = 0;
JonaVonk 7:80baf171503c 65 double initRot2 = -gearRatio*(180 - 48.5)/360;
JonaVonk 2:96e093a48314 66
JonaVonk 9:0e838367ab6a 67 double xCurrent;
JonaVonk 9:0e838367ab6a 68 double yCurrent;
RobertoO 0:67c50348f842 69
JonaVonk 2:96e093a48314 70 double calcRot1(double xDes, double yDes);
JonaVonk 2:96e093a48314 71 double calcRot2(double xDes, double yDes);
JonaVonk 9:0e838367ab6a 72
JonaVonk 9:0e838367ab6a 73 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
JonaVonk 7:80baf171503c 74 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
JonaVonk 9:0e838367ab6a 75 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
JonaVonk 9:0e838367ab6a 76 void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
JonaVonk 9:0e838367ab6a 77 void flipx();
JonaVonk 9:0e838367ab6a 78 void flipy();
JonaVonk 9:0e838367ab6a 79
JonaVonk 9:0e838367ab6a 80 //EMG
JonaVonk 9:0e838367ab6a 81 AnalogIn emg0( A0 );
JonaVonk 9:0e838367ab6a 82 AnalogIn emg1( A3 );
JonaVonk 9:0e838367ab6a 83
JonaVonk 9:0e838367ab6a 84 HIDScope scope( 2 );
JonaVonk 9:0e838367ab6a 85
JonaVonk 9:0e838367ab6a 86 Ticker ticker_calibration; // Ticker to send the EMG signals to screen
JonaVonk 9:0e838367ab6a 87
JonaVonk 9:0e838367ab6a 88 volatile double emg1_cal = 0.8;
JonaVonk 9:0e838367ab6a 89 volatile double emg1_filtered; //measured value of the first emg
JonaVonk 9:0e838367ab6a 90 volatile double emg2_filtered; //measured value of the second emg
JonaVonk 9:0e838367ab6a 91 volatile double emg2_cal = 0.8;
JonaVonk 2:96e093a48314 92
JonaVonk 9:0e838367ab6a 93 double speedy = 3;
JonaVonk 9:0e838367ab6a 94 double speedx = 3;
JonaVonk 9:0e838367ab6a 95
JonaVonk 9:0e838367ab6a 96 double xDir = 1;
JonaVonk 9:0e838367ab6a 97 double yDir = 1;
JonaVonk 9:0e838367ab6a 98
JonaVonk 9:0e838367ab6a 99 vector<double> pidInfo1 (4);
JonaVonk 9:0e838367ab6a 100 vector<double> pidInfo2 (4);
JonaVonk 9:0e838367ab6a 101
JonaVonk 9:0e838367ab6a 102 Timer tEMGMove;
JonaVonk 9:0e838367ab6a 103
JonaVonk 9:0e838367ab6a 104 InterruptIn xSwitch(D8);
JonaVonk 9:0e838367ab6a 105 InterruptIn ySwitch(D9);
JonaVonk 9:0e838367ab6a 106
JonaVonk 9:0e838367ab6a 107 void sample();
JonaVonk 9:0e838367ab6a 108
JonaVonk 9:0e838367ab6a 109 //Waiting
JonaVonk 9:0e838367ab6a 110 DigitalOut r_led(LED_RED);
JonaVonk 9:0e838367ab6a 111
RobertoO 0:67c50348f842 112 int main()
RobertoO 0:67c50348f842 113 {
JonaVonk 9:0e838367ab6a 114 stateSwitch.rise(switchState);
JonaVonk 9:0e838367ab6a 115 stepSwitch.rise(switchStep);
JonaVonk 9:0e838367ab6a 116
JonaVonk 9:0e838367ab6a 117 xSwitch.mode(PullUp);
JonaVonk 9:0e838367ab6a 118 ySwitch.mode(PullUp);
JonaVonk 9:0e838367ab6a 119
JonaVonk 9:0e838367ab6a 120 xSwitch.fall(flipx);
JonaVonk 9:0e838367ab6a 121 ySwitch.fall(flipy);
JonaVonk 9:0e838367ab6a 122
RobertoO 0:67c50348f842 123 pc.baud(115200);
JonaVonk 8:b40067b8a72d 124 while(1) {
JonaVonk 9:0e838367ab6a 125 stateMachine();
JonaVonk 8:b40067b8a72d 126 }
JonaVonk 8:b40067b8a72d 127
JonaVonk 8:b40067b8a72d 128 }
JonaVonk 8:b40067b8a72d 129
JonaVonk 9:0e838367ab6a 130 void switchState()
JonaVonk 9:0e838367ab6a 131 {
JonaVonk 9:0e838367ab6a 132 state++;
JonaVonk 9:0e838367ab6a 133 state = state%2;
JonaVonk 9:0e838367ab6a 134 stateStep = 0;
JonaVonk 9:0e838367ab6a 135 switch(state) {
JonaVonk 9:0e838367ab6a 136 //demo mode
JonaVonk 9:0e838367ab6a 137 case 0:
JonaVonk 9:0e838367ab6a 138 tEMGMove.stop();
JonaVonk 9:0e838367ab6a 139 pc.printf("demo\n\r");
JonaVonk 9:0e838367ab6a 140 noStateSteps = 4;
JonaVonk 9:0e838367ab6a 141 break;
JonaVonk 9:0e838367ab6a 142 case 1:
JonaVonk 9:0e838367ab6a 143 pc.printf("EMG\n\r");
JonaVonk 9:0e838367ab6a 144 noStateSteps = 7;
JonaVonk 9:0e838367ab6a 145 fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
JonaVonk 9:0e838367ab6a 146 fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
JonaVonk 9:0e838367ab6a 147 tEMGMove.reset();
JonaVonk 9:0e838367ab6a 148 tEMGMove.start();
JonaVonk 9:0e838367ab6a 149 break;
JonaVonk 9:0e838367ab6a 150 }
JonaVonk 9:0e838367ab6a 151 }
JonaVonk 9:0e838367ab6a 152
JonaVonk 9:0e838367ab6a 153 void switchStep()
JonaVonk 9:0e838367ab6a 154 {
JonaVonk 9:0e838367ab6a 155 stateStep++;
JonaVonk 9:0e838367ab6a 156 stateStep = stateStep%noStateSteps;
JonaVonk 9:0e838367ab6a 157 }
JonaVonk 9:0e838367ab6a 158
JonaVonk 9:0e838367ab6a 159 void stateMachine()
JonaVonk 9:0e838367ab6a 160 {
JonaVonk 9:0e838367ab6a 161 switch (state) {
JonaVonk 9:0e838367ab6a 162 case 0:
JonaVonk 9:0e838367ab6a 163 demoState();
JonaVonk 9:0e838367ab6a 164 break;
JonaVonk 9:0e838367ab6a 165 case 1:
JonaVonk 9:0e838367ab6a 166 EMGState();
JonaVonk 9:0e838367ab6a 167 break;
JonaVonk 9:0e838367ab6a 168 }
JonaVonk 9:0e838367ab6a 169 }
JonaVonk 9:0e838367ab6a 170
JonaVonk 9:0e838367ab6a 171 void demoState()
JonaVonk 9:0e838367ab6a 172 {
JonaVonk 9:0e838367ab6a 173 pc.printf("demo %d\n\r", stateStep);
JonaVonk 9:0e838367ab6a 174 switch (stateStep) {
JonaVonk 9:0e838367ab6a 175 case 0:
JonaVonk 9:0e838367ab6a 176 waiting();
JonaVonk 9:0e838367ab6a 177 break;
JonaVonk 9:0e838367ab6a 178 case 1:
JonaVonk 9:0e838367ab6a 179 calibrateMotors();
JonaVonk 9:0e838367ab6a 180 stateStep++;
JonaVonk 9:0e838367ab6a 181 break;
JonaVonk 9:0e838367ab6a 182 case 2:
JonaVonk 9:0e838367ab6a 183 waiting();
JonaVonk 9:0e838367ab6a 184 break;
JonaVonk 9:0e838367ab6a 185 case 3:
JonaVonk 9:0e838367ab6a 186 playDemo();
JonaVonk 9:0e838367ab6a 187 break;
JonaVonk 9:0e838367ab6a 188 }
JonaVonk 9:0e838367ab6a 189 }
JonaVonk 9:0e838367ab6a 190
JonaVonk 9:0e838367ab6a 191 void EMGState()
JonaVonk 9:0e838367ab6a 192 {
JonaVonk 9:0e838367ab6a 193 pc.printf("EMG %d\n\r", stateStep);
JonaVonk 9:0e838367ab6a 194 switch(stateStep) {
JonaVonk 9:0e838367ab6a 195 case 0:
JonaVonk 9:0e838367ab6a 196 waiting();
JonaVonk 9:0e838367ab6a 197 break;
JonaVonk 9:0e838367ab6a 198 case 1:
JonaVonk 9:0e838367ab6a 199 calibrateMotors();
JonaVonk 9:0e838367ab6a 200 stateStep++;
JonaVonk 9:0e838367ab6a 201 break;
JonaVonk 9:0e838367ab6a 202 case 2:
JonaVonk 9:0e838367ab6a 203 waiting();
JonaVonk 9:0e838367ab6a 204 break;
JonaVonk 9:0e838367ab6a 205 case 3:
JonaVonk 9:0e838367ab6a 206 EMGcalibration();
JonaVonk 9:0e838367ab6a 207 pc.printf("hack");
JonaVonk 9:0e838367ab6a 208 stateStep++;
JonaVonk 9:0e838367ab6a 209 break;
JonaVonk 9:0e838367ab6a 210 case 4:
JonaVonk 9:0e838367ab6a 211 waiting();
JonaVonk 9:0e838367ab6a 212 break;
JonaVonk 9:0e838367ab6a 213 case 5:
JonaVonk 9:0e838367ab6a 214 moveToInitialPosition();
JonaVonk 9:0e838367ab6a 215 xCurrent = 0;
JonaVonk 9:0e838367ab6a 216 yCurrent = 20;
JonaVonk 9:0e838367ab6a 217 break;
JonaVonk 9:0e838367ab6a 218 case 6:
JonaVonk 9:0e838367ab6a 219 moveWithEMG();
JonaVonk 9:0e838367ab6a 220 break;
JonaVonk 9:0e838367ab6a 221 }
JonaVonk 9:0e838367ab6a 222 }
JonaVonk 9:0e838367ab6a 223
JonaVonk 9:0e838367ab6a 224 void waiting()
JonaVonk 9:0e838367ab6a 225 {
JonaVonk 9:0e838367ab6a 226 r_led = 0;
JonaVonk 9:0e838367ab6a 227 E1 =0;
JonaVonk 9:0e838367ab6a 228 E2 =0;
JonaVonk 9:0e838367ab6a 229 r_led = 1;
JonaVonk 9:0e838367ab6a 230 }
JonaVonk 9:0e838367ab6a 231
JonaVonk 9:0e838367ab6a 232 void calibrateMotors()
JonaVonk 9:0e838367ab6a 233 {
JonaVonk 9:0e838367ab6a 234 moveMotorToStop(&M1, &E1, &Enc1, -0.1);
JonaVonk 9:0e838367ab6a 235 moveMotorToStop(&M2, &E2, &Enc2, 0.2);
JonaVonk 9:0e838367ab6a 236 Enc2.reset();
JonaVonk 9:0e838367ab6a 237 moveMotorToStop(&M1, &E1, &Enc1, -0.1);
JonaVonk 9:0e838367ab6a 238 Enc1.reset();
JonaVonk 9:0e838367ab6a 239 pc.printf("%f", Enc1.getPulses());
JonaVonk 9:0e838367ab6a 240 }
JonaVonk 9:0e838367ab6a 241
JonaVonk 8:b40067b8a72d 242 void playDemo()
JonaVonk 8:b40067b8a72d 243 {
JonaVonk 7:80baf171503c 244 moveAlongPath(10, 30, -10, 30, 3);
JonaVonk 7:80baf171503c 245 moveAlongPath(-10, 30, -10, 20, 3);
JonaVonk 7:80baf171503c 246 moveAlongPath(-10, 20, 10, 20, 3);
JonaVonk 7:80baf171503c 247 moveAlongPath(10, 20, 10, 30, 3);
JonaVonk 2:96e093a48314 248 }
JonaVonk 2:96e093a48314 249
JonaVonk 9:0e838367ab6a 250 void EMGcalibration()
JonaVonk 9:0e838367ab6a 251 {
JonaVonk 9:0e838367ab6a 252 Timer tijd;
JonaVonk 9:0e838367ab6a 253 tijd.start();
JonaVonk 9:0e838367ab6a 254 ticker_calibration.attach(&sample, 0.002);
JonaVonk 9:0e838367ab6a 255 do {
JonaVonk 9:0e838367ab6a 256 //ticker_calibration.attach(&sample, 0.002);
JonaVonk 9:0e838367ab6a 257 if(emg1_cal < emg1_filtered) {
JonaVonk 9:0e838367ab6a 258 emg1_cal = emg1_filtered ;
JonaVonk 9:0e838367ab6a 259 pc.printf("EMG_cal : %g \n\r",emg1_cal);
JonaVonk 9:0e838367ab6a 260 }
JonaVonk 9:0e838367ab6a 261 if(emg2_cal < emg2_filtered) {
JonaVonk 9:0e838367ab6a 262 emg2_cal = emg2_filtered ;
JonaVonk 9:0e838367ab6a 263 }
JonaVonk 9:0e838367ab6a 264 pc.printf("emg1: %f\n\r", emg1_filtered);
JonaVonk 9:0e838367ab6a 265 } while(tijd.read()<10);
JonaVonk 9:0e838367ab6a 266 }
JonaVonk 2:96e093a48314 267
JonaVonk 9:0e838367ab6a 268
JonaVonk 9:0e838367ab6a 269 void moveToInitialPosition()
JonaVonk 9:0e838367ab6a 270 {
JonaVonk 9:0e838367ab6a 271 double xStart = 0;
JonaVonk 9:0e838367ab6a 272 double yStart = 20;
JonaVonk 9:0e838367ab6a 273
JonaVonk 9:0e838367ab6a 274 double rot1 = calcRot1(xStart, yStart);
JonaVonk 9:0e838367ab6a 275 double rot2 = calcRot2(xStart, yStart);
JonaVonk 9:0e838367ab6a 276
JonaVonk 9:0e838367ab6a 277 moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
JonaVonk 9:0e838367ab6a 278 moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
JonaVonk 9:0e838367ab6a 279 }
JonaVonk 9:0e838367ab6a 280
JonaVonk 9:0e838367ab6a 281 void moveWithEMG()
JonaVonk 9:0e838367ab6a 282 {
JonaVonk 9:0e838367ab6a 283 if(emg1_filtered >= 0.5*emg1_cal) {
JonaVonk 9:0e838367ab6a 284 speedy = 3;
JonaVonk 9:0e838367ab6a 285 pc.printf("emg1: %f", emg1_filtered);
JonaVonk 9:0e838367ab6a 286 } else {
JonaVonk 9:0e838367ab6a 287 speedy = 0;
JonaVonk 9:0e838367ab6a 288 }
JonaVonk 9:0e838367ab6a 289
JonaVonk 9:0e838367ab6a 290 if(emg2_filtered >= 0.5*emg2_cal) {
JonaVonk 9:0e838367ab6a 291 speedx = 3;
JonaVonk 9:0e838367ab6a 292 pc.printf("emg1: %f\n\r", emg2_filtered);
JonaVonk 9:0e838367ab6a 293 } else {
JonaVonk 9:0e838367ab6a 294 speedx = 0;
JonaVonk 9:0e838367ab6a 295 }
JonaVonk 9:0e838367ab6a 296 pc.printf("speedx: %f speedy: %f\r\n", speedx, speedy);
JonaVonk 9:0e838367ab6a 297 xCurrent = xCurrent + xDir*speedx*0.01;
JonaVonk 9:0e838367ab6a 298 yCurrent = yCurrent + yDir*speedy*0.01;
JonaVonk 9:0e838367ab6a 299 double rot2 = calcRot2(xCurrent, yCurrent);
JonaVonk 9:0e838367ab6a 300 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &tEMGMove, -rot2);
JonaVonk 9:0e838367ab6a 301 double rot1 = calcRot1(xCurrent, yCurrent);
JonaVonk 9:0e838367ab6a 302 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &tEMGMove, rot1);
JonaVonk 9:0e838367ab6a 303 wait(0.01);
JonaVonk 9:0e838367ab6a 304 }
JonaVonk 9:0e838367ab6a 305
JonaVonk 9:0e838367ab6a 306 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
JonaVonk 9:0e838367ab6a 307 {
JonaVonk 9:0e838367ab6a 308 Timer t;
JonaVonk 9:0e838367ab6a 309
JonaVonk 9:0e838367ab6a 310 double MotorPWM;
JonaVonk 9:0e838367ab6a 311
JonaVonk 9:0e838367ab6a 312 double posC;
JonaVonk 9:0e838367ab6a 313 double posP = Enc->getPulses()/(32*131.25);
JonaVonk 9:0e838367ab6a 314 double vel = 0;
JonaVonk 9:0e838367ab6a 315
JonaVonk 9:0e838367ab6a 316 int hasNotMoved = 0;
JonaVonk 9:0e838367ab6a 317
JonaVonk 9:0e838367ab6a 318 t.start();
JonaVonk 9:0e838367ab6a 319 do {
JonaVonk 9:0e838367ab6a 320 MotorPWM = speed - vel*0.7;
JonaVonk 9:0e838367ab6a 321 if(MotorPWM > 0) {
JonaVonk 9:0e838367ab6a 322 *M = 0;
JonaVonk 9:0e838367ab6a 323 *E = MotorPWM;
JonaVonk 9:0e838367ab6a 324 } else {
JonaVonk 9:0e838367ab6a 325 *M = 1;
JonaVonk 9:0e838367ab6a 326 *E = -MotorPWM;
JonaVonk 9:0e838367ab6a 327 }
JonaVonk 9:0e838367ab6a 328 wait(0.01);
JonaVonk 9:0e838367ab6a 329 posC = Enc->getPulses()/(32*131.25);
JonaVonk 9:0e838367ab6a 330 vel = (posC - posP)/t.read();
JonaVonk 9:0e838367ab6a 331 t.reset();
JonaVonk 9:0e838367ab6a 332 posP = posC;
JonaVonk 9:0e838367ab6a 333 //pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved);
JonaVonk 9:0e838367ab6a 334 if(abs(vel) > 0.001) {
JonaVonk 9:0e838367ab6a 335 hasNotMoved = 0;
JonaVonk 9:0e838367ab6a 336 } else {
JonaVonk 9:0e838367ab6a 337 hasNotMoved++;
JonaVonk 9:0e838367ab6a 338 }
JonaVonk 9:0e838367ab6a 339 } while(hasNotMoved < 6);
JonaVonk 9:0e838367ab6a 340 *E = 0;
JonaVonk 9:0e838367ab6a 341 }
JonaVonk 9:0e838367ab6a 342
JonaVonk 9:0e838367ab6a 343 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
JonaVonk 9:0e838367ab6a 344 {
JonaVonk 9:0e838367ab6a 345 double rot1;
JonaVonk 9:0e838367ab6a 346 double rot2;
JonaVonk 9:0e838367ab6a 347
JonaVonk 9:0e838367ab6a 348 Timer t;
JonaVonk 9:0e838367ab6a 349
JonaVonk 9:0e838367ab6a 350 vector<double> desiredLocation;
JonaVonk 9:0e838367ab6a 351
JonaVonk 9:0e838367ab6a 352 fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
JonaVonk 9:0e838367ab6a 353 fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
JonaVonk 9:0e838367ab6a 354
JonaVonk 9:0e838367ab6a 355 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
JonaVonk 9:0e838367ab6a 356
JonaVonk 9:0e838367ab6a 357 //Calculate the rotation of the motors at the start of the path
JonaVonk 9:0e838367ab6a 358 rot1 = calcRot1(xStart, yStart);
JonaVonk 9:0e838367ab6a 359 rot2 = calcRot2(xStart, yStart);
JonaVonk 9:0e838367ab6a 360 pc.printf("r1: %f r2: %f", rot1/6, rot2/6);
JonaVonk 9:0e838367ab6a 361
JonaVonk 9:0e838367ab6a 362 //Move arms to the start of the path
JonaVonk 9:0e838367ab6a 363 //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
JonaVonk 9:0e838367ab6a 364 //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
JonaVonk 9:0e838367ab6a 365
JonaVonk 9:0e838367ab6a 366 //start the timer
JonaVonk 9:0e838367ab6a 367 t.start();
JonaVonk 9:0e838367ab6a 368 while(pathLength > speed * t.read()) {
JonaVonk 9:0e838367ab6a 369 findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
JonaVonk 9:0e838367ab6a 370 rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
JonaVonk 9:0e838367ab6a 371 //pc.printf("\n\r Rot1: %f", rot1);
JonaVonk 9:0e838367ab6a 372 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
JonaVonk 9:0e838367ab6a 373 rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
JonaVonk 9:0e838367ab6a 374 //pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
JonaVonk 9:0e838367ab6a 375 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
JonaVonk 9:0e838367ab6a 376 //pc.printf("\n\r xCalc: %f yCalc: %f", calcX(pidInfo1.at(3), pidInfo2.at(3)), calcY(pidInfo1.at(3), pidInfo2.at(3)));
JonaVonk 9:0e838367ab6a 377 wait(0.01);
JonaVonk 9:0e838367ab6a 378 }
JonaVonk 9:0e838367ab6a 379
JonaVonk 9:0e838367ab6a 380 }
JonaVonk 9:0e838367ab6a 381
JonaVonk 9:0e838367ab6a 382 double calcRot1(double xDes, double yDes)
JonaVonk 9:0e838367ab6a 383 {
JonaVonk 9:0e838367ab6a 384 double alpha = atan(yDes/xDes);
JonaVonk 9:0e838367ab6a 385 if(alpha < 0) {
JonaVonk 9:0e838367ab6a 386 alpha = alpha+Pi;
JonaVonk 9:0e838367ab6a 387 }
JonaVonk 9:0e838367ab6a 388 //pc.printf("alpha: %f", alpha);
JonaVonk 9:0e838367ab6a 389 return gearRatio*((alpha - 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
JonaVonk 9:0e838367ab6a 390 }
JonaVonk 9:0e838367ab6a 391
JonaVonk 9:0e838367ab6a 392 double calcRot2(double xDes, double yDes)
JonaVonk 9:0e838367ab6a 393 {
JonaVonk 9:0e838367ab6a 394 double alpha = atan(yDes/xDes);
JonaVonk 9:0e838367ab6a 395 if(alpha < 0) {
JonaVonk 9:0e838367ab6a 396 alpha = alpha+Pi;
JonaVonk 9:0e838367ab6a 397 }
JonaVonk 9:0e838367ab6a 398 return gearRatio*((alpha + 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
JonaVonk 9:0e838367ab6a 399 }
JonaVonk 9:0e838367ab6a 400
JonaVonk 9:0e838367ab6a 401 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
JonaVonk 9:0e838367ab6a 402 {
JonaVonk 9:0e838367ab6a 403 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
JonaVonk 9:0e838367ab6a 404 double traveledDistance = speed * t->read();
JonaVonk 9:0e838367ab6a 405 double ratio = traveledDistance/pathLength;
JonaVonk 9:0e838367ab6a 406
JonaVonk 9:0e838367ab6a 407 desiredLocation->clear();
JonaVonk 9:0e838367ab6a 408 desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
JonaVonk 9:0e838367ab6a 409 desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);
JonaVonk 9:0e838367ab6a 410
JonaVonk 9:0e838367ab6a 411 }
JonaVonk 9:0e838367ab6a 412
JonaVonk 9:0e838367ab6a 413 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
JonaVonk 9:0e838367ab6a 414 {
JonaVonk 9:0e838367ab6a 415 double Kp = 61; //180 & 10 werkt zonder hulp
JonaVonk 9:0e838367ab6a 416 double Ki = 1;
JonaVonk 9:0e838367ab6a 417 double Kd = 7;
JonaVonk 9:0e838367ab6a 418
JonaVonk 9:0e838367ab6a 419 double rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 9:0e838367ab6a 420
JonaVonk 9:0e838367ab6a 421 double pErrorC = rotDes - rotC;
JonaVonk 9:0e838367ab6a 422
JonaVonk 9:0e838367ab6a 423 double tieme = t->read();
JonaVonk 9:0e838367ab6a 424 double dt = tieme - pidInfo->at(2);
JonaVonk 9:0e838367ab6a 425
JonaVonk 9:0e838367ab6a 426 double iError = pidInfo->at(1) + pErrorC*dt;
JonaVonk 9:0e838367ab6a 427 double dError = (pErrorC - pidInfo->at(0))/dt;
JonaVonk 9:0e838367ab6a 428
JonaVonk 9:0e838367ab6a 429 double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;
JonaVonk 9:0e838367ab6a 430
JonaVonk 9:0e838367ab6a 431 if(MotorPWM > 0) {
JonaVonk 9:0e838367ab6a 432 *M = 0;
JonaVonk 9:0e838367ab6a 433 *E = MotorPWM;
JonaVonk 9:0e838367ab6a 434 } else {
JonaVonk 9:0e838367ab6a 435 *M = 1;
JonaVonk 9:0e838367ab6a 436 *E = -MotorPWM;
JonaVonk 9:0e838367ab6a 437 }
JonaVonk 9:0e838367ab6a 438 pidInfo->clear();
JonaVonk 9:0e838367ab6a 439 pidInfo->push_back(pErrorC);
JonaVonk 9:0e838367ab6a 440 pidInfo->push_back(iError);
JonaVonk 9:0e838367ab6a 441 pidInfo->push_back(tieme);
JonaVonk 9:0e838367ab6a 442 pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
JonaVonk 9:0e838367ab6a 443 }
JonaVonk 9:0e838367ab6a 444
JonaVonk 9:0e838367ab6a 445 void sample()
JonaVonk 9:0e838367ab6a 446 {
JonaVonk 9:0e838367ab6a 447 emg1_filtered = FilterDesign(emg0.read());
JonaVonk 9:0e838367ab6a 448 emg2_filtered = FilterDesign(emg1.read());
JonaVonk 9:0e838367ab6a 449 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
JonaVonk 9:0e838367ab6a 450 scope.set(0, emg1_filtered ) ;
JonaVonk 9:0e838367ab6a 451 scope.set(1, emg2_filtered );
JonaVonk 9:0e838367ab6a 452 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
JonaVonk 9:0e838367ab6a 453 * Ensure that enough channels are available (HIDScope scope( 2 ))
JonaVonk 9:0e838367ab6a 454 * Finally, send all channels to the PC at once */
JonaVonk 9:0e838367ab6a 455 scope.send();
JonaVonk 9:0e838367ab6a 456 /* To indicate that the function is working, the LED is toggled */
JonaVonk 9:0e838367ab6a 457 //pc.printf("%f", emg1_filtered)
JonaVonk 9:0e838367ab6a 458 //led = !led;
JonaVonk 9:0e838367ab6a 459 }
JonaVonk 9:0e838367ab6a 460
JonaVonk 9:0e838367ab6a 461 void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed)
JonaVonk 9:0e838367ab6a 462 {
JonaVonk 9:0e838367ab6a 463 Timer t;
JonaVonk 9:0e838367ab6a 464
JonaVonk 9:0e838367ab6a 465 vector<double> pidInfo1 (4);
JonaVonk 9:0e838367ab6a 466 vector<double> pidInfo2 (4);
JonaVonk 9:0e838367ab6a 467
JonaVonk 9:0e838367ab6a 468 fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
JonaVonk 9:0e838367ab6a 469 fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
JonaVonk 9:0e838367ab6a 470
JonaVonk 9:0e838367ab6a 471 double xC = *xStart;
JonaVonk 9:0e838367ab6a 472 double yC = *yStart;
JonaVonk 9:0e838367ab6a 473
JonaVonk 9:0e838367ab6a 474 double rot1;
JonaVonk 9:0e838367ab6a 475 double rot2;
JonaVonk 9:0e838367ab6a 476
JonaVonk 9:0e838367ab6a 477 double tiemeP;
JonaVonk 9:0e838367ab6a 478 double tiemeC;
JonaVonk 9:0e838367ab6a 479 double dt;
JonaVonk 9:0e838367ab6a 480
JonaVonk 9:0e838367ab6a 481 t.start();
JonaVonk 9:0e838367ab6a 482
JonaVonk 9:0e838367ab6a 483 tiemeP = t.read();
JonaVonk 9:0e838367ab6a 484 while(t.read() < 0.1) {
JonaVonk 9:0e838367ab6a 485 tiemeC = t.read();
JonaVonk 9:0e838367ab6a 486 dt = tiemeC - tiemeP;
JonaVonk 9:0e838367ab6a 487 xC = xC+xSpeed*dt;
JonaVonk 9:0e838367ab6a 488 yC = yC+ySpeed*dt;
JonaVonk 9:0e838367ab6a 489 rot1 = calcRot1(xC, yC);
JonaVonk 9:0e838367ab6a 490 rot2 = calcRot2(xC, yC);
JonaVonk 9:0e838367ab6a 491 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
JonaVonk 9:0e838367ab6a 492 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
JonaVonk 9:0e838367ab6a 493 tiemeP = tiemeC;
JonaVonk 9:0e838367ab6a 494 wait(0.01);
JonaVonk 9:0e838367ab6a 495 }
JonaVonk 9:0e838367ab6a 496
JonaVonk 9:0e838367ab6a 497 *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3));
JonaVonk 9:0e838367ab6a 498 *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3));
JonaVonk 9:0e838367ab6a 499 }
JonaVonk 9:0e838367ab6a 500
JonaVonk 7:80baf171503c 501 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
JonaVonk 2:96e093a48314 502 {
JonaVonk 9:0e838367ab6a 503 double Kp = 2; //180 & 10 werkt zonder hulp
JonaVonk 7:80baf171503c 504 double Ki = 0;
JonaVonk 9:0e838367ab6a 505 double Kd = 0.1;
JonaVonk 7:80baf171503c 506
JonaVonk 5:5082d694e643 507 double pErrorC;
JonaVonk 5:5082d694e643 508 double pErrorP = 0;
JonaVonk 5:5082d694e643 509 double iError = 0;
JonaVonk 5:5082d694e643 510 double dError;
JonaVonk 2:96e093a48314 511
JonaVonk 5:5082d694e643 512 double U_p;
JonaVonk 5:5082d694e643 513 double U_i;
JonaVonk 5:5082d694e643 514 double U_d;
JonaVonk 2:96e093a48314 515
JonaVonk 5:5082d694e643 516 double rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 5:5082d694e643 517 double MotorPWM;
JonaVonk 2:96e093a48314 518
JonaVonk 2:96e093a48314 519 Timer t;
JonaVonk 5:5082d694e643 520 double tieme = 0;
JonaVonk 2:96e093a48314 521
JonaVonk 2:96e093a48314 522 t.start();
JonaVonk 2:96e093a48314 523 do {
JonaVonk 2:96e093a48314 524 pErrorP = pErrorC;
JonaVonk 5:5082d694e643 525 rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 2:96e093a48314 526 pErrorC = rotDes - rotC;
JonaVonk 5:5082d694e643 527 tieme = t.read();
JonaVonk 5:5082d694e643 528 t.reset();
JonaVonk 2:96e093a48314 529 iError = iError + pErrorC*tieme;
JonaVonk 2:96e093a48314 530 dError = (pErrorC - pErrorP)/tieme;
JonaVonk 7:80baf171503c 531
JonaVonk 5:5082d694e643 532 U_p = pErrorC*Kp;
JonaVonk 5:5082d694e643 533 U_i = iError*Ki;
JonaVonk 5:5082d694e643 534 U_d = dError*Kd;
JonaVonk 5:5082d694e643 535 MotorPWM = (U_p + U_i + U_d)*dir;
JonaVonk 2:96e093a48314 536
JonaVonk 2:96e093a48314 537 if(MotorPWM > 0) {
JonaVonk 2:96e093a48314 538 *M = 0;
JonaVonk 2:96e093a48314 539 *E = MotorPWM;
JonaVonk 2:96e093a48314 540 } else {
JonaVonk 2:96e093a48314 541 *M = 1;
JonaVonk 2:96e093a48314 542 *E = -MotorPWM;
JonaVonk 2:96e093a48314 543 }
JonaVonk 6:105b306350c6 544 wait(0.01);
JonaVonk 9:0e838367ab6a 545 //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 7:80baf171503c 546 } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
JonaVonk 5:5082d694e643 547 *E = 0;
JonaVonk 6:105b306350c6 548 //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 2:96e093a48314 549 t.stop();
JonaVonk 4:55e6707949dd 550 }
JonaVonk 2:96e093a48314 551
JonaVonk 9:0e838367ab6a 552 void flipx()
JonaVonk 7:80baf171503c 553 {
JonaVonk 9:0e838367ab6a 554 xDir = -xDir;
JonaVonk 7:80baf171503c 555
RobertoO 0:67c50348f842 556 }
JonaVonk 2:96e093a48314 557
JonaVonk 9:0e838367ab6a 558 void flipy()
JonaVonk 2:96e093a48314 559 {
JonaVonk 9:0e838367ab6a 560 yDir = -yDir;
JonaVonk 9:0e838367ab6a 561 }