groep 16 / Mbed 2 deprecated Switches2

Switches 2.0

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@10:cbcb35182ef1, 2019-11-05 (annotated)

Committer:
sanou8
Date:
Tue Nov 05 13:23:20 2019 +0000
Revision:
10:cbcb35182ef1
Parent:
9:0e838367ab6a 
Met mijn commentaar erbij

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
sanou8 10:cbcb35182ef1 88 volatile double emg1_cal = 0.8; // Initial value of the calibrated first emg
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
sanou8 10:cbcb35182ef1 91 volatile double emg2_cal = 0.8; // Initial value of the calibrated second emg
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 {
sanou8 10:cbcb35182ef1 252 Timer tijd; // Timer for the duration of the calibration stage
sanou8 10:cbcb35182ef1 253 tijd.start(); // Start of the timer
sanou8 10:cbcb35182ef1 254 ticker_calibration.attach(&sample, 0.002); // Ticker for reading EMG-signals is attached to the function that filters the emg-signals
JonaVonk 9:0e838367ab6a 255 do {
sanou8 10:cbcb35182ef1 256 if(emg1_cal < emg1_filtered) { // Initial calibration value is compaired to the current EMG value.
sanou8 10:cbcb35182ef1 257 emg1_cal = emg1_filtered ; // When the current EMG value is higher than the calibration value, then the calibration
sanou8 10:cbcb35182ef1 258 pc.printf("EMG_cal : %g \n\r",emg1_cal); // value becomes the current EMG value.
JonaVonk 9:0e838367ab6a 259 }
sanou8 10:cbcb35182ef1 260 if(emg2_cal < emg2_filtered) { // The same is true for the second EMG signal
JonaVonk 9:0e838367ab6a 261 emg2_cal = emg2_filtered ;
JonaVonk 9:0e838367ab6a 262 }
JonaVonk 9:0e838367ab6a 263 pc.printf("emg1: %f\n\r", emg1_filtered);
sanou8 10:cbcb35182ef1 264 } while(tijd.read()<10); // After ten seconds the calibration of the EMG-signals stop
JonaVonk 9:0e838367ab6a 265 }
JonaVonk 2:96e093a48314 266
JonaVonk 9:0e838367ab6a 267
JonaVonk 9:0e838367ab6a 268 void moveToInitialPosition()
JonaVonk 9:0e838367ab6a 269 {
JonaVonk 9:0e838367ab6a 270 double xStart = 0;
JonaVonk 9:0e838367ab6a 271 double yStart = 20;
JonaVonk 9:0e838367ab6a 272
JonaVonk 9:0e838367ab6a 273 double rot1 = calcRot1(xStart, yStart);
JonaVonk 9:0e838367ab6a 274 double rot2 = calcRot2(xStart, yStart);
JonaVonk 9:0e838367ab6a 275
JonaVonk 9:0e838367ab6a 276 moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
JonaVonk 9:0e838367ab6a 277 moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
JonaVonk 9:0e838367ab6a 278 }
JonaVonk 9:0e838367ab6a 279
JonaVonk 9:0e838367ab6a 280 void moveWithEMG()
JonaVonk 9:0e838367ab6a 281 {
sanou8 10:cbcb35182ef1 282 if(emg1_filtered >= 0.5*emg1_cal) { // When the current value of EMG1 is higher than half of the calibration value,
sanou8 10:cbcb35182ef1 283 speedy = 3; // the speed in Y direction becomes 3. Otherwise the speed stays 0.
JonaVonk 9:0e838367ab6a 284 pc.printf("emg1: %f", emg1_filtered);
JonaVonk 9:0e838367ab6a 285 } else {
JonaVonk 9:0e838367ab6a 286 speedy = 0;
JonaVonk 9:0e838367ab6a 287 }
JonaVonk 9:0e838367ab6a 288
sanou8 10:cbcb35182ef1 289 if(emg2_filtered >= 0.5*emg2_cal) { // When the current value of EMG2 is higher than half of the calibration value,
sanou8 10:cbcb35182ef1 290 speedx = 3; // the speed in X direction becomes 3. Otherwise the speed stays 0.
JonaVonk 9:0e838367ab6a 291 pc.printf("emg1: %f\n\r", emg2_filtered);
JonaVonk 9:0e838367ab6a 292 } else {
JonaVonk 9:0e838367ab6a 293 speedx = 0;
JonaVonk 9:0e838367ab6a 294 }
JonaVonk 9:0e838367ab6a 295 pc.printf("speedx: %f speedy: %f\r\n", speedx, speedy);
JonaVonk 9:0e838367ab6a 296 xCurrent = xCurrent + xDir*speedx*0.01;
JonaVonk 9:0e838367ab6a 297 yCurrent = yCurrent + yDir*speedy*0.01;
JonaVonk 9:0e838367ab6a 298 double rot2 = calcRot2(xCurrent, yCurrent);
JonaVonk 9:0e838367ab6a 299 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &tEMGMove, -rot2);
JonaVonk 9:0e838367ab6a 300 double rot1 = calcRot1(xCurrent, yCurrent);
JonaVonk 9:0e838367ab6a 301 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &tEMGMove, rot1);
JonaVonk 9:0e838367ab6a 302 wait(0.01);
JonaVonk 9:0e838367ab6a 303 }
JonaVonk 9:0e838367ab6a 304
JonaVonk 9:0e838367ab6a 305 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
JonaVonk 9:0e838367ab6a 306 {
JonaVonk 9:0e838367ab6a 307 Timer t;
JonaVonk 9:0e838367ab6a 308
JonaVonk 9:0e838367ab6a 309 double MotorPWM;
JonaVonk 9:0e838367ab6a 310
JonaVonk 9:0e838367ab6a 311 double posC;
JonaVonk 9:0e838367ab6a 312 double posP = Enc->getPulses()/(32*131.25);
JonaVonk 9:0e838367ab6a 313 double vel = 0;
JonaVonk 9:0e838367ab6a 314
JonaVonk 9:0e838367ab6a 315 int hasNotMoved = 0;
JonaVonk 9:0e838367ab6a 316
JonaVonk 9:0e838367ab6a 317 t.start();
JonaVonk 9:0e838367ab6a 318 do {
JonaVonk 9:0e838367ab6a 319 MotorPWM = speed - vel*0.7;
JonaVonk 9:0e838367ab6a 320 if(MotorPWM > 0) {
JonaVonk 9:0e838367ab6a 321 *M = 0;
JonaVonk 9:0e838367ab6a 322 *E = MotorPWM;
JonaVonk 9:0e838367ab6a 323 } else {
JonaVonk 9:0e838367ab6a 324 *M = 1;
JonaVonk 9:0e838367ab6a 325 *E = -MotorPWM;
JonaVonk 9:0e838367ab6a 326 }
JonaVonk 9:0e838367ab6a 327 wait(0.01);
JonaVonk 9:0e838367ab6a 328 posC = Enc->getPulses()/(32*131.25);
JonaVonk 9:0e838367ab6a 329 vel = (posC - posP)/t.read();
JonaVonk 9:0e838367ab6a 330 t.reset();
JonaVonk 9:0e838367ab6a 331 posP = posC;
JonaVonk 9:0e838367ab6a 332 //pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved);
JonaVonk 9:0e838367ab6a 333 if(abs(vel) > 0.001) {
JonaVonk 9:0e838367ab6a 334 hasNotMoved = 0;
JonaVonk 9:0e838367ab6a 335 } else {
JonaVonk 9:0e838367ab6a 336 hasNotMoved++;
JonaVonk 9:0e838367ab6a 337 }
JonaVonk 9:0e838367ab6a 338 } while(hasNotMoved < 6);
JonaVonk 9:0e838367ab6a 339 *E = 0;
JonaVonk 9:0e838367ab6a 340 }
JonaVonk 9:0e838367ab6a 341
JonaVonk 9:0e838367ab6a 342 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
JonaVonk 9:0e838367ab6a 343 {
JonaVonk 9:0e838367ab6a 344 double rot1;
JonaVonk 9:0e838367ab6a 345 double rot2;
JonaVonk 9:0e838367ab6a 346
JonaVonk 9:0e838367ab6a 347 Timer t;
JonaVonk 9:0e838367ab6a 348
JonaVonk 9:0e838367ab6a 349 vector<double> desiredLocation;
JonaVonk 9:0e838367ab6a 350
JonaVonk 9:0e838367ab6a 351 fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
JonaVonk 9:0e838367ab6a 352 fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
JonaVonk 9:0e838367ab6a 353
JonaVonk 9:0e838367ab6a 354 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
JonaVonk 9:0e838367ab6a 355
JonaVonk 9:0e838367ab6a 356 //Calculate the rotation of the motors at the start of the path
JonaVonk 9:0e838367ab6a 357 rot1 = calcRot1(xStart, yStart);
JonaVonk 9:0e838367ab6a 358 rot2 = calcRot2(xStart, yStart);
JonaVonk 9:0e838367ab6a 359 pc.printf("r1: %f r2: %f", rot1/6, rot2/6);
JonaVonk 9:0e838367ab6a 360
JonaVonk 9:0e838367ab6a 361 //Move arms to the start of the path
JonaVonk 9:0e838367ab6a 362 //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
JonaVonk 9:0e838367ab6a 363 //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
JonaVonk 9:0e838367ab6a 364
JonaVonk 9:0e838367ab6a 365 //start the timer
JonaVonk 9:0e838367ab6a 366 t.start();
JonaVonk 9:0e838367ab6a 367 while(pathLength > speed * t.read()) {
JonaVonk 9:0e838367ab6a 368 findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
JonaVonk 9:0e838367ab6a 369 rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
JonaVonk 9:0e838367ab6a 370 //pc.printf("\n\r Rot1: %f", rot1);
JonaVonk 9:0e838367ab6a 371 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
JonaVonk 9:0e838367ab6a 372 rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
JonaVonk 9:0e838367ab6a 373 //pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
JonaVonk 9:0e838367ab6a 374 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
JonaVonk 9:0e838367ab6a 375 //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 376 wait(0.01);
JonaVonk 9:0e838367ab6a 377 }
JonaVonk 9:0e838367ab6a 378
JonaVonk 9:0e838367ab6a 379 }
JonaVonk 9:0e838367ab6a 380
JonaVonk 9:0e838367ab6a 381 double calcRot1(double xDes, double yDes)
JonaVonk 9:0e838367ab6a 382 {
JonaVonk 9:0e838367ab6a 383 double alpha = atan(yDes/xDes);
JonaVonk 9:0e838367ab6a 384 if(alpha < 0) {
JonaVonk 9:0e838367ab6a 385 alpha = alpha+Pi;
JonaVonk 9:0e838367ab6a 386 }
JonaVonk 9:0e838367ab6a 387 //pc.printf("alpha: %f", alpha);
JonaVonk 9:0e838367ab6a 388 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 389 }
JonaVonk 9:0e838367ab6a 390
JonaVonk 9:0e838367ab6a 391 double calcRot2(double xDes, double yDes)
JonaVonk 9:0e838367ab6a 392 {
JonaVonk 9:0e838367ab6a 393 double alpha = atan(yDes/xDes);
JonaVonk 9:0e838367ab6a 394 if(alpha < 0) {
JonaVonk 9:0e838367ab6a 395 alpha = alpha+Pi;
JonaVonk 9:0e838367ab6a 396 }
JonaVonk 9:0e838367ab6a 397 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 398 }
JonaVonk 9:0e838367ab6a 399
JonaVonk 9:0e838367ab6a 400 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
JonaVonk 9:0e838367ab6a 401 {
JonaVonk 9:0e838367ab6a 402 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
JonaVonk 9:0e838367ab6a 403 double traveledDistance = speed * t->read();
JonaVonk 9:0e838367ab6a 404 double ratio = traveledDistance/pathLength;
JonaVonk 9:0e838367ab6a 405
JonaVonk 9:0e838367ab6a 406 desiredLocation->clear();
JonaVonk 9:0e838367ab6a 407 desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
JonaVonk 9:0e838367ab6a 408 desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);
JonaVonk 9:0e838367ab6a 409
JonaVonk 9:0e838367ab6a 410 }
JonaVonk 9:0e838367ab6a 411
JonaVonk 9:0e838367ab6a 412 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
JonaVonk 9:0e838367ab6a 413 {
JonaVonk 9:0e838367ab6a 414 double Kp = 61; //180 & 10 werkt zonder hulp
JonaVonk 9:0e838367ab6a 415 double Ki = 1;
JonaVonk 9:0e838367ab6a 416 double Kd = 7;
JonaVonk 9:0e838367ab6a 417
JonaVonk 9:0e838367ab6a 418 double rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 9:0e838367ab6a 419
JonaVonk 9:0e838367ab6a 420 double pErrorC = rotDes - rotC;
JonaVonk 9:0e838367ab6a 421
JonaVonk 9:0e838367ab6a 422 double tieme = t->read();
JonaVonk 9:0e838367ab6a 423 double dt = tieme - pidInfo->at(2);
JonaVonk 9:0e838367ab6a 424
JonaVonk 9:0e838367ab6a 425 double iError = pidInfo->at(1) + pErrorC*dt;
JonaVonk 9:0e838367ab6a 426 double dError = (pErrorC - pidInfo->at(0))/dt;
JonaVonk 9:0e838367ab6a 427
JonaVonk 9:0e838367ab6a 428 double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;
JonaVonk 9:0e838367ab6a 429
JonaVonk 9:0e838367ab6a 430 if(MotorPWM > 0) {
JonaVonk 9:0e838367ab6a 431 *M = 0;
JonaVonk 9:0e838367ab6a 432 *E = MotorPWM;
JonaVonk 9:0e838367ab6a 433 } else {
JonaVonk 9:0e838367ab6a 434 *M = 1;
JonaVonk 9:0e838367ab6a 435 *E = -MotorPWM;
JonaVonk 9:0e838367ab6a 436 }
JonaVonk 9:0e838367ab6a 437 pidInfo->clear();
JonaVonk 9:0e838367ab6a 438 pidInfo->push_back(pErrorC);
JonaVonk 9:0e838367ab6a 439 pidInfo->push_back(iError);
JonaVonk 9:0e838367ab6a 440 pidInfo->push_back(tieme);
JonaVonk 9:0e838367ab6a 441 pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
JonaVonk 9:0e838367ab6a 442 }
JonaVonk 9:0e838367ab6a 443
JonaVonk 9:0e838367ab6a 444 void sample()
JonaVonk 9:0e838367ab6a 445 {
sanou8 10:cbcb35182ef1 446 emg1_filtered = FilterDesign(emg0.read()); // The first emg signal is put in the filter and the filtered values are saved
sanou8 10:cbcb35182ef1 447 emg2_filtered = FilterDesign(emg1.read()); // The second emg signal is put in the filter and the filtered values are saved
sanou8 10:cbcb35182ef1 448
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 );
sanou8 10:cbcb35182ef1 452
sanou8 10:cbcb35182ef1 453 scope.send(); // Finally, the values are send and can be used in other functions.
JonaVonk 9:0e838367ab6a 454 }
JonaVonk 9:0e838367ab6a 455
JonaVonk 9:0e838367ab6a 456 void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed)
JonaVonk 9:0e838367ab6a 457 {
JonaVonk 9:0e838367ab6a 458 Timer t;
JonaVonk 9:0e838367ab6a 459
JonaVonk 9:0e838367ab6a 460 vector<double> pidInfo1 (4);
JonaVonk 9:0e838367ab6a 461 vector<double> pidInfo2 (4);
JonaVonk 9:0e838367ab6a 462
JonaVonk 9:0e838367ab6a 463 fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
JonaVonk 9:0e838367ab6a 464 fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
JonaVonk 9:0e838367ab6a 465
JonaVonk 9:0e838367ab6a 466 double xC = *xStart;
JonaVonk 9:0e838367ab6a 467 double yC = *yStart;
JonaVonk 9:0e838367ab6a 468
JonaVonk 9:0e838367ab6a 469 double rot1;
JonaVonk 9:0e838367ab6a 470 double rot2;
JonaVonk 9:0e838367ab6a 471
JonaVonk 9:0e838367ab6a 472 double tiemeP;
JonaVonk 9:0e838367ab6a 473 double tiemeC;
JonaVonk 9:0e838367ab6a 474 double dt;
JonaVonk 9:0e838367ab6a 475
JonaVonk 9:0e838367ab6a 476 t.start();
JonaVonk 9:0e838367ab6a 477
JonaVonk 9:0e838367ab6a 478 tiemeP = t.read();
JonaVonk 9:0e838367ab6a 479 while(t.read() < 0.1) {
JonaVonk 9:0e838367ab6a 480 tiemeC = t.read();
JonaVonk 9:0e838367ab6a 481 dt = tiemeC - tiemeP;
JonaVonk 9:0e838367ab6a 482 xC = xC+xSpeed*dt;
JonaVonk 9:0e838367ab6a 483 yC = yC+ySpeed*dt;
JonaVonk 9:0e838367ab6a 484 rot1 = calcRot1(xC, yC);
JonaVonk 9:0e838367ab6a 485 rot2 = calcRot2(xC, yC);
JonaVonk 9:0e838367ab6a 486 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
JonaVonk 9:0e838367ab6a 487 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
JonaVonk 9:0e838367ab6a 488 tiemeP = tiemeC;
JonaVonk 9:0e838367ab6a 489 wait(0.01);
JonaVonk 9:0e838367ab6a 490 }
JonaVonk 9:0e838367ab6a 491
JonaVonk 9:0e838367ab6a 492 *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3));
JonaVonk 9:0e838367ab6a 493 *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3));
JonaVonk 9:0e838367ab6a 494 }
JonaVonk 9:0e838367ab6a 495
JonaVonk 7:80baf171503c 496 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
JonaVonk 2:96e093a48314 497 {
JonaVonk 9:0e838367ab6a 498 double Kp = 2; //180 & 10 werkt zonder hulp
JonaVonk 7:80baf171503c 499 double Ki = 0;
JonaVonk 9:0e838367ab6a 500 double Kd = 0.1;
JonaVonk 7:80baf171503c 501
JonaVonk 5:5082d694e643 502 double pErrorC;
JonaVonk 5:5082d694e643 503 double pErrorP = 0;
JonaVonk 5:5082d694e643 504 double iError = 0;
JonaVonk 5:5082d694e643 505 double dError;
JonaVonk 2:96e093a48314 506
JonaVonk 5:5082d694e643 507 double U_p;
JonaVonk 5:5082d694e643 508 double U_i;
JonaVonk 5:5082d694e643 509 double U_d;
JonaVonk 2:96e093a48314 510
JonaVonk 5:5082d694e643 511 double rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 5:5082d694e643 512 double MotorPWM;
JonaVonk 2:96e093a48314 513
JonaVonk 2:96e093a48314 514 Timer t;
JonaVonk 5:5082d694e643 515 double tieme = 0;
JonaVonk 2:96e093a48314 516
JonaVonk 2:96e093a48314 517 t.start();
JonaVonk 2:96e093a48314 518 do {
JonaVonk 2:96e093a48314 519 pErrorP = pErrorC;
JonaVonk 5:5082d694e643 520 rotC = Enc->getPulses()/(32*131.25) + initRot;
JonaVonk 2:96e093a48314 521 pErrorC = rotDes - rotC;
JonaVonk 5:5082d694e643 522 tieme = t.read();
JonaVonk 5:5082d694e643 523 t.reset();
JonaVonk 2:96e093a48314 524 iError = iError + pErrorC*tieme;
JonaVonk 2:96e093a48314 525 dError = (pErrorC - pErrorP)/tieme;
JonaVonk 7:80baf171503c 526
JonaVonk 5:5082d694e643 527 U_p = pErrorC*Kp;
JonaVonk 5:5082d694e643 528 U_i = iError*Ki;
JonaVonk 5:5082d694e643 529 U_d = dError*Kd;
JonaVonk 5:5082d694e643 530 MotorPWM = (U_p + U_i + U_d)*dir;
JonaVonk 2:96e093a48314 531
JonaVonk 2:96e093a48314 532 if(MotorPWM > 0) {
JonaVonk 2:96e093a48314 533 *M = 0;
JonaVonk 2:96e093a48314 534 *E = MotorPWM;
JonaVonk 2:96e093a48314 535 } else {
JonaVonk 2:96e093a48314 536 *M = 1;
JonaVonk 2:96e093a48314 537 *E = -MotorPWM;
JonaVonk 2:96e093a48314 538 }
JonaVonk 6:105b306350c6 539 wait(0.01);
JonaVonk 9:0e838367ab6a 540 //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 7:80baf171503c 541 } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
JonaVonk 5:5082d694e643 542 *E = 0;
JonaVonk 6:105b306350c6 543 //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
JonaVonk 2:96e093a48314 544 t.stop();
JonaVonk 4:55e6707949dd 545 }
JonaVonk 2:96e093a48314 546
JonaVonk 9:0e838367ab6a 547 void flipx()
JonaVonk 7:80baf171503c 548 {
JonaVonk 9:0e838367ab6a 549 xDir = -xDir;
JonaVonk 7:80baf171503c 550
RobertoO 0:67c50348f842 551 }
JonaVonk 2:96e093a48314 552
JonaVonk 9:0e838367ab6a 553 void flipy()
JonaVonk 2:96e093a48314 554 {
JonaVonk 9:0e838367ab6a 555 yDir = -yDir;
JonaVonk 9:0e838367ab6a 556 }