groep 16 / Mbed 2 deprecated EMG_mode_sander

emg mode van sander

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@24:830e4dfa112a, 2019-10-31 (annotated)

Committer:
sanou8
Date:
Thu Oct 31 21:15:56 2019 +0000
Revision:
24:830e4dfa112a
Parent:
23:b8fa74336d2a 
emg mode met werkende x en y aansturing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
sanou8 21:2c26b74a3e48 3 #include "FilterDesign.h"
sanou8 21:2c26b74a3e48 4 #include "BiQuad.h"
sanou8 21:2c26b74a3e48 5 #include "BiQuad4.h"
sanou8 21:2c26b74a3e48 6 #include "MODSERIAL.h"
sanou8 23:b8fa74336d2a 7 #include "QEI.h"
sanou8 23:b8fa74336d2a 8 #include <vector>
sanou8 23:b8fa74336d2a 9
sanou8 23:b8fa74336d2a 10 using std::vector;
sanou8 21:2c26b74a3e48 11
sanou8 21:2c26b74a3e48 12 Serial pc(USBTX,USBRX);
sanou8 21:2c26b74a3e48 13 DigitalIn button(SW3) ;
sanou8 23:b8fa74336d2a 14 InterruptIn sw2(SW2);
sanou8 24:830e4dfa112a 15 InterruptIn sw3(SW3);
vsluiter 0:32bb76391d89 16
vsluiter 4:8b298dfada81 17 //Define objects
tomlankhorst 19:2bf824669684 18 AnalogIn emg0( A0 );
tomlankhorst 19:2bf824669684 19 AnalogIn emg1( A1 );
sanou8 23:b8fa74336d2a 20 QEI Enc1(D12, D13, NC, 32);
sanou8 23:b8fa74336d2a 21 QEI Enc2(D10, D11, NC, 32);
sanou8 23:b8fa74336d2a 22 DigitalOut M1(D4);
sanou8 23:b8fa74336d2a 23 DigitalOut M2(D7);
sanou8 23:b8fa74336d2a 24 PwmOut E1(D5);
sanou8 23:b8fa74336d2a 25 PwmOut E2(D6);
tomlankhorst 19:2bf824669684 26
sanou8 21:2c26b74a3e48 27
sanou8 21:2c26b74a3e48 28 Ticker ticker_calibration; // Ticker to send the EMG signals to screen
sanou8 21:2c26b74a3e48 29 Ticker sample_timer; // Ticker for reading out EMG
tomlankhorst 19:2bf824669684 30 HIDScope scope( 2 );
sanou8 23:b8fa74336d2a 31 DigitalOut led(LED_RED);
sanou8 23:b8fa74336d2a 32 DigitalOut controlLED(LED_GREEN);
sanou8 23:b8fa74336d2a 33 Timer timer;
vsluiter 2:e314bb3b2d99 34
sanou8 21:2c26b74a3e48 35 volatile double emg1_filtered; //measured value of the first emg
sanou8 21:2c26b74a3e48 36 volatile double emg2_filtered; //measured value of the second emg
sanou8 24:830e4dfa112a 37 volatile double emg1_cal = 0.8; // calibrated value of first emg
sanou8 24:830e4dfa112a 38 volatile double emg2_cal = 0.8 ; // calibrated value of second emg
sanou8 23:b8fa74336d2a 39 double Pi = 3.14159265359;
sanou8 23:b8fa74336d2a 40 double gearRatio = 40/9;
sanou8 21:2c26b74a3e48 41
sanou8 23:b8fa74336d2a 42 double initRot1 = 0;
sanou8 23:b8fa74336d2a 43 double initRot2 = -gearRatio*(180 - 48.5)/360;
sanou8 24:830e4dfa112a 44
sanou8 23:b8fa74336d2a 45 double speedy = 3 ;
sanou8 24:830e4dfa112a 46 double speedx = 3 ;
sanou8 21:2c26b74a3e48 47
sanou8 21:2c26b74a3e48 48
sanou8 21:2c26b74a3e48 49
sanou8 23:b8fa74336d2a 50 void sample() ;
sanou8 23:b8fa74336d2a 51 void EMGcalibration() ;
sanou8 23:b8fa74336d2a 52 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
sanou8 23:b8fa74336d2a 53 double calcRot1(double xDes, double yDes);
sanou8 23:b8fa74336d2a 54 double calcRot2(double xDes, double yDes);
sanou8 23:b8fa74336d2a 55 void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
sanou8 23:b8fa74336d2a 56 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed);
sanou8 23:b8fa74336d2a 57 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
sanou8 23:b8fa74336d2a 58 double calcX(double rot1, double rot2);
sanou8 23:b8fa74336d2a 59 double calcY(double rot1, double rot2);
sanou8 23:b8fa74336d2a 60 void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
sanou8 23:b8fa74336d2a 61 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
sanou8 23:b8fa74336d2a 62 void EMG_move();
sanou8 23:b8fa74336d2a 63
sanou8 24:830e4dfa112a 64 void changespeedy(){
sanou8 23:b8fa74336d2a 65 speedy = speedy*-1;
sanou8 23:b8fa74336d2a 66 }
sanou8 24:830e4dfa112a 67 void changespeedx(){
sanou8 24:830e4dfa112a 68 speedx = speedx*-1;
sanou8 24:830e4dfa112a 69 }
vsluiter 0:32bb76391d89 70
vsluiter 0:32bb76391d89 71 int main()
sanou8 23:b8fa74336d2a 72 {
sanou8 23:b8fa74336d2a 73
sanou8 23:b8fa74336d2a 74
sanou8 21:2c26b74a3e48 75 pc.baud(115200);
sanou8 23:b8fa74336d2a 76 sw2.mode(PullUp);
tomlankhorst 19:2bf824669684 77 sample_timer.attach(&sample, 0.002);
sanou8 23:b8fa74336d2a 78
sanou8 23:b8fa74336d2a 79
sanou8 23:b8fa74336d2a 80 controlLED = 1;
sanou8 23:b8fa74336d2a 81 while(button >= 1) {
sanou8 23:b8fa74336d2a 82 led = 1 ;
sanou8 23:b8fa74336d2a 83 }
sanou8 23:b8fa74336d2a 84 led = 0 ;
sanou8 23:b8fa74336d2a 85 EMGcalibration();
sanou8 23:b8fa74336d2a 86 led = 1 ;
sanou8 23:b8fa74336d2a 87 double xStart = 0;
sanou8 23:b8fa74336d2a 88 double yStart = 20;
sanou8 23:b8fa74336d2a 89
sanou8 23:b8fa74336d2a 90 double rot1 = calcRot1(xStart, yStart);
sanou8 23:b8fa74336d2a 91 double rot2 = calcRot2(xStart, yStart);
sanou8 23:b8fa74336d2a 92
sanou8 23:b8fa74336d2a 93 moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
sanou8 23:b8fa74336d2a 94 moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
sanou8 24:830e4dfa112a 95 sw2.rise(changespeedy);
sanou8 24:830e4dfa112a 96 sw3.rise(changespeedx);
sanou8 23:b8fa74336d2a 97 while(true) {
sanou8 23:b8fa74336d2a 98
sanou8 23:b8fa74336d2a 99
sanou8 24:830e4dfa112a 100 while(emg1_filtered >= 0.5*emg1_cal) {
sanou8 23:b8fa74336d2a 101 //controlLED = !controlLED ;
sanou8 23:b8fa74336d2a 102 moveWithSpeed(&xStart, &yStart, 0, speedy);
sanou8 23:b8fa74336d2a 103 pc.printf("ystart: %g \n\r",yStart);
sanou8 23:b8fa74336d2a 104 }
sanou8 24:830e4dfa112a 105 while(emg2_filtered >= 0.5*emg2_cal) {
sanou8 24:830e4dfa112a 106 moveWithSpeed(&xStart, &yStart, speedx, 0);
sanou8 24:830e4dfa112a 107 }
sanou8 23:b8fa74336d2a 108
sanou8 23:b8fa74336d2a 109 rot1 = calcRot1(xStart, yStart);
sanou8 23:b8fa74336d2a 110 rot2 = calcRot2(xStart, yStart);
sanou8 23:b8fa74336d2a 111 moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
sanou8 23:b8fa74336d2a 112 moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
sanou8 23:b8fa74336d2a 113 }
sanou8 23:b8fa74336d2a 114 }
sanou8 23:b8fa74336d2a 115
sanou8 23:b8fa74336d2a 116
sanou8 21:2c26b74a3e48 117
sanou8 21:2c26b74a3e48 118
sanou8 21:2c26b74a3e48 119
sanou8 23:b8fa74336d2a 120 void sample()
sanou8 21:2c26b74a3e48 121 {
sanou8 21:2c26b74a3e48 122 emg1_filtered = FilterDesign(emg0.read());
sanou8 21:2c26b74a3e48 123 emg2_filtered = FilterDesign(emg1.read());
sanou8 21:2c26b74a3e48 124 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
sanou8 21:2c26b74a3e48 125 scope.set(0, emg1_filtered ) ;
sanou8 21:2c26b74a3e48 126 scope.set(1, emg2_filtered );
sanou8 23:b8fa74336d2a 127 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
sanou8 21:2c26b74a3e48 128 * Ensure that enough channels are available (HIDScope scope( 2 ))
sanou8 21:2c26b74a3e48 129 * Finally, send all channels to the PC at once */
sanou8 21:2c26b74a3e48 130 scope.send();
sanou8 21:2c26b74a3e48 131 /* To indicate that the function is working, the LED is toggled */
sanou8 21:2c26b74a3e48 132 //pc.printf("%f", emg1_filtered)
sanou8 21:2c26b74a3e48 133 //led = !led;
sanou8 21:2c26b74a3e48 134 }
sanou8 21:2c26b74a3e48 135
sanou8 23:b8fa74336d2a 136 void EMGcalibration()
sanou8 23:b8fa74336d2a 137 {
sanou8 23:b8fa74336d2a 138
sanou8 23:b8fa74336d2a 139 Timer tijd;
sanou8 23:b8fa74336d2a 140 tijd.start();
sanou8 23:b8fa74336d2a 141 do {
sanou8 23:b8fa74336d2a 142 ticker_calibration.attach(&sample, 0.002);
sanou8 23:b8fa74336d2a 143 if(emg1_cal < emg1_filtered) {
sanou8 23:b8fa74336d2a 144 emg1_cal = emg1_filtered ;
sanou8 23:b8fa74336d2a 145 pc.printf("EMG_cal : %g \n\r",emg1_cal);
sanou8 23:b8fa74336d2a 146 }
sanou8 24:830e4dfa112a 147 if(emg2_cal < emg2_filtered) {
sanou8 24:830e4dfa112a 148 emg2_cal = emg2_filtered ;
sanou8 24:830e4dfa112a 149 }
sanou8 23:b8fa74336d2a 150 } while(tijd<10);
sanou8 23:b8fa74336d2a 151 }
sanou8 23:b8fa74336d2a 152
sanou8 23:b8fa74336d2a 153
sanou8 23:b8fa74336d2a 154 //function to mave a motor to a certain number of rotations, counted from the start of the program.
sanou8 23:b8fa74336d2a 155 //parameters:
sanou8 23:b8fa74336d2a 156 //DigitalOut *M = pointer to direction cpntol pin of motor
sanou8 23:b8fa74336d2a 157 //PwmOut *E = pointer to speed contorl pin of motor
sanou8 23:b8fa74336d2a 158 //QEI *Enc = pointer to encoder of motor
sanou8 23:b8fa74336d2a 159 //double rotDes = desired rotation
sanou8 23:b8fa74336d2a 160 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
sanou8 23:b8fa74336d2a 161 {
sanou8 23:b8fa74336d2a 162 double Kp = 30; //180 & 10 werkt zonder hulp
sanou8 23:b8fa74336d2a 163 double Ki = 0;
sanou8 23:b8fa74336d2a 164 double Kd = 2;
sanou8 23:b8fa74336d2a 165
sanou8 23:b8fa74336d2a 166 double pErrorC;
sanou8 23:b8fa74336d2a 167 double pErrorP = 0;
sanou8 23:b8fa74336d2a 168 double iError = 0;
sanou8 23:b8fa74336d2a 169 double dError;
sanou8 23:b8fa74336d2a 170
sanou8 23:b8fa74336d2a 171 double U_p;
sanou8 23:b8fa74336d2a 172 double U_i;
sanou8 23:b8fa74336d2a 173 double U_d;
sanou8 23:b8fa74336d2a 174
sanou8 23:b8fa74336d2a 175 double rotC = Enc->getPulses()/(32*131.25) + initRot;
sanou8 23:b8fa74336d2a 176 double MotorPWM;
sanou8 23:b8fa74336d2a 177
sanou8 23:b8fa74336d2a 178 Timer t;
sanou8 23:b8fa74336d2a 179 double tieme = 0;
sanou8 23:b8fa74336d2a 180
sanou8 23:b8fa74336d2a 181 t.start();
sanou8 23:b8fa74336d2a 182 do {
sanou8 23:b8fa74336d2a 183 pErrorP = pErrorC;
sanou8 23:b8fa74336d2a 184 rotC = Enc->getPulses()/(32*131.25) + initRot;
sanou8 23:b8fa74336d2a 185 pErrorC = rotDes - rotC;
sanou8 23:b8fa74336d2a 186 tieme = t.read();
sanou8 23:b8fa74336d2a 187 t.reset();
sanou8 23:b8fa74336d2a 188 iError = iError + pErrorC*tieme;
sanou8 23:b8fa74336d2a 189 dError = (pErrorC - pErrorP)/tieme;
sanou8 23:b8fa74336d2a 190
sanou8 23:b8fa74336d2a 191 U_p = pErrorC*Kp;
sanou8 23:b8fa74336d2a 192 U_i = iError*Ki;
sanou8 23:b8fa74336d2a 193 U_d = dError*Kd;
sanou8 23:b8fa74336d2a 194 MotorPWM = (U_p + U_i + U_d)*dir;
sanou8 23:b8fa74336d2a 195
sanou8 23:b8fa74336d2a 196 if(MotorPWM > 0) {
sanou8 23:b8fa74336d2a 197 *M = 0;
sanou8 23:b8fa74336d2a 198 *E = MotorPWM;
sanou8 23:b8fa74336d2a 199 } else {
sanou8 23:b8fa74336d2a 200 *M = 1;
sanou8 23:b8fa74336d2a 201 *E = -MotorPWM;
sanou8 23:b8fa74336d2a 202 }
sanou8 23:b8fa74336d2a 203 wait(0.01);
sanou8 23:b8fa74336d2a 204 //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
sanou8 23:b8fa74336d2a 205 } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
sanou8 23:b8fa74336d2a 206 *E = 0;
sanou8 23:b8fa74336d2a 207 //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
sanou8 23:b8fa74336d2a 208 t.stop();
sanou8 23:b8fa74336d2a 209 }
sanou8 23:b8fa74336d2a 210
sanou8 23:b8fa74336d2a 211
sanou8 23:b8fa74336d2a 212 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
sanou8 23:b8fa74336d2a 213 {
sanou8 23:b8fa74336d2a 214 double Kp = 61; //180 & 10 werkt zonder hulp
sanou8 23:b8fa74336d2a 215 double Ki = 1;
sanou8 23:b8fa74336d2a 216 double Kd = 7;
sanou8 23:b8fa74336d2a 217
sanou8 23:b8fa74336d2a 218 double rotC = Enc->getPulses()/(32*131.25) + initRot;
sanou8 23:b8fa74336d2a 219
sanou8 23:b8fa74336d2a 220 double pErrorC = rotDes - rotC;
sanou8 23:b8fa74336d2a 221
sanou8 23:b8fa74336d2a 222 double tieme = t->read();
sanou8 23:b8fa74336d2a 223 double dt = tieme - pidInfo->at(2);
sanou8 23:b8fa74336d2a 224
sanou8 23:b8fa74336d2a 225 double iError = pidInfo->at(1) + pErrorC*dt;
sanou8 23:b8fa74336d2a 226 double dError = (pErrorC - pidInfo->at(0))/dt;
sanou8 23:b8fa74336d2a 227
sanou8 23:b8fa74336d2a 228 double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;
sanou8 23:b8fa74336d2a 229
sanou8 23:b8fa74336d2a 230 if(MotorPWM > 0) {
sanou8 23:b8fa74336d2a 231 *M = 0;
sanou8 23:b8fa74336d2a 232 *E = MotorPWM;
sanou8 23:b8fa74336d2a 233 } else {
sanou8 23:b8fa74336d2a 234 *M = 1;
sanou8 23:b8fa74336d2a 235 *E = -MotorPWM;
sanou8 23:b8fa74336d2a 236 }
sanou8 23:b8fa74336d2a 237 pidInfo->clear();
sanou8 23:b8fa74336d2a 238 pidInfo->push_back(pErrorC);
sanou8 23:b8fa74336d2a 239 pidInfo->push_back(iError);
sanou8 23:b8fa74336d2a 240 pidInfo->push_back(tieme);
sanou8 23:b8fa74336d2a 241 pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
sanou8 23:b8fa74336d2a 242 }
sanou8 23:b8fa74336d2a 243
sanou8 23:b8fa74336d2a 244
sanou8 23:b8fa74336d2a 245 //double that calculates the rotation of one arm.
sanou8 23:b8fa74336d2a 246 //parameters:
sanou8 23:b8fa74336d2a 247 //double xDes = ofset of the arm's shaft in cm in the x direction
sanou8 23:b8fa74336d2a 248 //double yDes = ofset of the arm's shaft in cm in the y direction
sanou8 23:b8fa74336d2a 249 double calcRot1(double xDes, double yDes)
sanou8 21:2c26b74a3e48 250 {
sanou8 23:b8fa74336d2a 251 double alpha = atan(yDes/xDes);
sanou8 23:b8fa74336d2a 252 if(alpha < 0) {
sanou8 23:b8fa74336d2a 253 alpha = alpha+Pi;
sanou8 23:b8fa74336d2a 254 }
sanou8 23:b8fa74336d2a 255 //pc.printf("alpha: %f", alpha);
sanou8 23:b8fa74336d2a 256 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));
sanou8 23:b8fa74336d2a 257 }
sanou8 23:b8fa74336d2a 258
sanou8 23:b8fa74336d2a 259 double calcRot2(double xDes, double yDes)
sanou8 23:b8fa74336d2a 260 {
sanou8 23:b8fa74336d2a 261 double alpha = atan(yDes/xDes);
sanou8 23:b8fa74336d2a 262 if(alpha < 0) {
sanou8 23:b8fa74336d2a 263 alpha = alpha+Pi;
sanou8 23:b8fa74336d2a 264 }
sanou8 23:b8fa74336d2a 265 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));
sanou8 23:b8fa74336d2a 266 }
sanou8 23:b8fa74336d2a 267
sanou8 23:b8fa74336d2a 268 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
sanou8 23:b8fa74336d2a 269 {
sanou8 23:b8fa74336d2a 270 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
sanou8 23:b8fa74336d2a 271 double traveledDistance = speed * t->read();
sanou8 23:b8fa74336d2a 272 double ratio = traveledDistance/pathLength;
sanou8 23:b8fa74336d2a 273
sanou8 23:b8fa74336d2a 274 desiredLocation->clear();
sanou8 23:b8fa74336d2a 275 desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
sanou8 23:b8fa74336d2a 276 desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);
sanou8 23:b8fa74336d2a 277
sanou8 23:b8fa74336d2a 278 }
sanou8 23:b8fa74336d2a 279
sanou8 23:b8fa74336d2a 280 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
sanou8 23:b8fa74336d2a 281 {
sanou8 23:b8fa74336d2a 282 double rot1;
sanou8 23:b8fa74336d2a 283 double rot2;
sanou8 23:b8fa74336d2a 284
sanou8 23:b8fa74336d2a 285 Timer t;
sanou8 23:b8fa74336d2a 286
sanou8 23:b8fa74336d2a 287 vector<double> desiredLocation;
sanou8 23:b8fa74336d2a 288
sanou8 23:b8fa74336d2a 289 vector<double> pidInfo1 (3);
sanou8 23:b8fa74336d2a 290 vector<double> pidInfo2 (3);
sanou8 23:b8fa74336d2a 291
sanou8 23:b8fa74336d2a 292 fill(pidInfo1.begin(), pidInfo1.begin()+3, 0);
sanou8 23:b8fa74336d2a 293 fill(pidInfo2.begin(), pidInfo2.begin()+3, 0);
sanou8 23:b8fa74336d2a 294
sanou8 23:b8fa74336d2a 295 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
sanou8 23:b8fa74336d2a 296
sanou8 23:b8fa74336d2a 297 //Calculate the rotation of the motors at the start of the path
sanou8 23:b8fa74336d2a 298 rot1 = calcRot1(xStart, yStart);
sanou8 23:b8fa74336d2a 299 rot2 = calcRot2(xStart, yStart);
sanou8 23:b8fa74336d2a 300 //pc.printf("r1: %f r2: %f", rot1/6, rot2/6);
sanou8 23:b8fa74336d2a 301
sanou8 23:b8fa74336d2a 302 //Move arms to the start of the path
sanou8 23:b8fa74336d2a 303 //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
sanou8 23:b8fa74336d2a 304 //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
sanou8 23:b8fa74336d2a 305
sanou8 23:b8fa74336d2a 306 //start the timer
sanou8 23:b8fa74336d2a 307 t.start();
sanou8 23:b8fa74336d2a 308 while(pathLength > speed * t.read()) {
sanou8 23:b8fa74336d2a 309 findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
sanou8 23:b8fa74336d2a 310 rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
sanou8 23:b8fa74336d2a 311 //pc.printf("\n\r Rot1: %f", rot1);
sanou8 23:b8fa74336d2a 312 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
sanou8 23:b8fa74336d2a 313 rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
sanou8 23:b8fa74336d2a 314 //pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
sanou8 23:b8fa74336d2a 315 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
sanou8 23:b8fa74336d2a 316 wait(0.01);
sanou8 23:b8fa74336d2a 317 }
sanou8 23:b8fa74336d2a 318
sanou8 23:b8fa74336d2a 319 }
sanou8 23:b8fa74336d2a 320
sanou8 23:b8fa74336d2a 321
sanou8 23:b8fa74336d2a 322
sanou8 23:b8fa74336d2a 323 double calcX(double rot1, double rot2)
sanou8 23:b8fa74336d2a 324 {
sanou8 23:b8fa74336d2a 325 return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi);
sanou8 23:b8fa74336d2a 326 }
sanou8 23:b8fa74336d2a 327
sanou8 23:b8fa74336d2a 328 double calcY(double rot1, double rot2)
sanou8 23:b8fa74336d2a 329 {
sanou8 23:b8fa74336d2a 330 return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi);
sanou8 23:b8fa74336d2a 331 }
sanou8 23:b8fa74336d2a 332
sanou8 23:b8fa74336d2a 333
sanou8 23:b8fa74336d2a 334 void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed)
sanou8 23:b8fa74336d2a 335 {
sanou8 23:b8fa74336d2a 336 Timer t;
sanou8 23:b8fa74336d2a 337
sanou8 23:b8fa74336d2a 338 vector<double> pidInfo1 (4);
sanou8 23:b8fa74336d2a 339 vector<double> pidInfo2 (4);
sanou8 23:b8fa74336d2a 340
sanou8 23:b8fa74336d2a 341 fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
sanou8 23:b8fa74336d2a 342 fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
sanou8 23:b8fa74336d2a 343
sanou8 23:b8fa74336d2a 344 double xC = *xStart;
sanou8 23:b8fa74336d2a 345 double yC = *yStart;
sanou8 23:b8fa74336d2a 346
sanou8 23:b8fa74336d2a 347 double rot1;
sanou8 23:b8fa74336d2a 348 double rot2;
sanou8 23:b8fa74336d2a 349
sanou8 23:b8fa74336d2a 350 double tiemeP;
sanou8 23:b8fa74336d2a 351 double tiemeC;
sanou8 23:b8fa74336d2a 352 double dt;
sanou8 23:b8fa74336d2a 353
sanou8 23:b8fa74336d2a 354 t.start();
sanou8 23:b8fa74336d2a 355
sanou8 23:b8fa74336d2a 356 tiemeP = t.read();
sanou8 23:b8fa74336d2a 357 while(t.read() < 0.1) {
sanou8 23:b8fa74336d2a 358 tiemeC = t.read();
sanou8 23:b8fa74336d2a 359 dt = tiemeC - tiemeP;
sanou8 23:b8fa74336d2a 360 xC = xC+xSpeed*dt;
sanou8 23:b8fa74336d2a 361 yC = yC+ySpeed*dt;
sanou8 23:b8fa74336d2a 362 rot1 = calcRot1(xC, yC);
sanou8 23:b8fa74336d2a 363 rot2 = calcRot2(xC, yC);
sanou8 23:b8fa74336d2a 364 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
sanou8 23:b8fa74336d2a 365 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
sanou8 23:b8fa74336d2a 366 tiemeP = tiemeC;
sanou8 23:b8fa74336d2a 367 wait(0.01);
sanou8 23:b8fa74336d2a 368 }
sanou8 23:b8fa74336d2a 369
sanou8 23:b8fa74336d2a 370 *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3));
sanou8 23:b8fa74336d2a 371 *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3));
sanou8 23:b8fa74336d2a 372 }
sanou8 23:b8fa74336d2a 373
sanou8 23:b8fa74336d2a 374