groep 16 / Mbed 2 deprecated Project_total

totale unit

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@23:d13db573a875, 2019-10-31 (annotated)

Committer:
sanou8
Date:
Thu Oct 31 20:36:14 2019 +0000
Revision:
23:d13db573a875
Parent:
22:08b3cd7bec7f 
Totale code

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:d13db573a875 7 #include "QEI.h"
sanou8 23:d13db573a875 8 #include <vector>
sanou8 23:d13db573a875 9
sanou8 23:d13db573a875 10 using std::vector;
sanou8 21:2c26b74a3e48 11
sanou8 21:2c26b74a3e48 12 Serial pc(USBTX,USBRX);
sanou8 23:d13db573a875 13 DigitalIn button_motor(SW2) ;
sanou8 23:d13db573a875 14 DigitalIn button_demo(D9) ;
sanou8 23:d13db573a875 15 DigitalIn button_emg(D8);
sanou8 23:d13db573a875 16 DigitalIn Fail_button(SW3);
sanou8 23:d13db573a875 17 InterruptIn sw2(SW2);
sanou8 23:d13db573a875 18 DigitalOut led_red(LED_RED);
sanou8 23:d13db573a875 19 DigitalOut led_green(LED_GREEN);
sanou8 23:d13db573a875 20 DigitalOut led_blue(LED_BLUE);
vsluiter 0:32bb76391d89 21
vsluiter 4:8b298dfada81 22 //Define objects
tomlankhorst 19:2bf824669684 23 AnalogIn emg0( A0 );
tomlankhorst 19:2bf824669684 24 AnalogIn emg1( A1 );
sanou8 23:d13db573a875 25 QEI Enc1(D12, D13, NC, 32);
sanou8 23:d13db573a875 26 QEI Enc2(D10, D11, NC, 32);
sanou8 23:d13db573a875 27 DigitalOut M1(D4);
sanou8 23:d13db573a875 28 DigitalOut M2(D7);
sanou8 23:d13db573a875 29 PwmOut E1(D5);
sanou8 23:d13db573a875 30 PwmOut E2(D6);
tomlankhorst 19:2bf824669684 31
sanou8 23:d13db573a875 32 Ticker StateMachine;
sanou8 21:2c26b74a3e48 33 Ticker ticker_calibration; // Ticker to send the EMG signals to screen
sanou8 21:2c26b74a3e48 34 Ticker sample_timer; // Ticker for reading out EMG
tomlankhorst 19:2bf824669684 35 HIDScope scope( 2 );
sanou8 23:d13db573a875 36 DigitalOut led(LED_RED);
sanou8 23:d13db573a875 37 DigitalOut controlLED(LED_GREEN);
sanou8 23:d13db573a875 38 Timer timer;
vsluiter 2:e314bb3b2d99 39
sanou8 21:2c26b74a3e48 40 volatile double emg1_filtered; //measured value of the first emg
sanou8 21:2c26b74a3e48 41 volatile double emg2_filtered; //measured value of the second emg
lucvandijk 22:08b3cd7bec7f 42 volatile double emg1_cal = 0.8;
sanou8 23:d13db573a875 43 double Pi = 3.14159265359;
sanou8 23:d13db573a875 44 double gearRatio = 40/9;
sanou8 23:d13db573a875 45 double time_in_state ;
sanou8 21:2c26b74a3e48 46
sanou8 23:d13db573a875 47 double initRot1 = 0;
sanou8 23:d13db573a875 48 double initRot2 = -gearRatio*(180 - 48.5)/360;
sanou8 23:d13db573a875 49 volatile double y_new = 20.0 ;
sanou8 23:d13db573a875 50 volatile double y_prev = 20.0 ;
sanou8 23:d13db573a875 51 double displacement = 1 ;
sanou8 23:d13db573a875 52 double speedy = 3 ;
sanou8 21:2c26b74a3e48 53
sanou8 23:d13db573a875 54 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, OPERATION, SHOOTING, MOVE_W_DEMO, FAILURE_MODE};
sanou8 23:d13db573a875 55 states currentState = WAITING; // Start in waiting state
sanou8 23:d13db573a875 56 bool stateChanged = true;
sanou8 21:2c26b74a3e48 57
sanou8 21:2c26b74a3e48 58 void sample() ;
sanou8 23:d13db573a875 59 void EMGcalibration() ;
sanou8 23:d13db573a875 60 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
sanou8 23:d13db573a875 61 double calcRot1(double xDes, double yDes);
sanou8 23:d13db573a875 62 double calcRot2(double xDes, double yDes);
sanou8 23:d13db573a875 63 void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
sanou8 23:d13db573a875 64 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed);
sanou8 23:d13db573a875 65 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
sanou8 23:d13db573a875 66 double calcX(double rot1, double rot2);
sanou8 23:d13db573a875 67 double calcY(double rot1, double rot2);
sanou8 23:d13db573a875 68 void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
sanou8 23:d13db573a875 69 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
sanou8 23:d13db573a875 70 void DEMO_motor() ;
sanou8 23:d13db573a875 71 void EMG_move();
sanou8 23:d13db573a875 72 void state_machine(void);
sanou8 23:d13db573a875 73 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed);
sanou8 23:d13db573a875 74 void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2);
sanou8 21:2c26b74a3e48 75
sanou8 23:d13db573a875 76 void changespeed()
sanou8 23:d13db573a875 77 {
sanou8 23:d13db573a875 78 speedy = speedy*-1;
sanou8 23:d13db573a875 79 }
sanou8 21:2c26b74a3e48 80
vsluiter 0:32bb76391d89 81
vsluiter 0:32bb76391d89 82 int main()
sanou8 23:d13db573a875 83 {
sanou8 23:d13db573a875 84 led_red = 1;
sanou8 23:d13db573a875 85 led_green = 1;
sanou8 23:d13db573a875 86 led_blue = 1;
sanou8 23:d13db573a875 87
sanou8 21:2c26b74a3e48 88 pc.baud(115200);
sanou8 23:d13db573a875 89
sanou8 23:d13db573a875 90
sanou8 23:d13db573a875 91 StateMachine.attach(&state_machine, 0.005f);
sanou8 21:2c26b74a3e48 92 while(true) {
sanou8 23:d13db573a875 93
sanou8 23:d13db573a875 94
lucvandijk 22:08b3cd7bec7f 95 }
sanou8 21:2c26b74a3e48 96 }
sanou8 21:2c26b74a3e48 97
sanou8 21:2c26b74a3e48 98
sanou8 21:2c26b74a3e48 99
sanou8 21:2c26b74a3e48 100
sanou8 21:2c26b74a3e48 101
sanou8 21:2c26b74a3e48 102
sanou8 23:d13db573a875 103 void sample()
sanou8 21:2c26b74a3e48 104 {
sanou8 21:2c26b74a3e48 105 emg1_filtered = FilterDesign(emg0.read());
sanou8 21:2c26b74a3e48 106 emg2_filtered = FilterDesign(emg1.read());
sanou8 21:2c26b74a3e48 107 /* 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 108 scope.set(0, emg1_filtered ) ;
sanou8 21:2c26b74a3e48 109 scope.set(1, emg2_filtered );
sanou8 23:d13db573a875 110 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
sanou8 21:2c26b74a3e48 111 * Ensure that enough channels are available (HIDScope scope( 2 ))
sanou8 21:2c26b74a3e48 112 * Finally, send all channels to the PC at once */
sanou8 21:2c26b74a3e48 113 scope.send();
sanou8 21:2c26b74a3e48 114 /* To indicate that the function is working, the LED is toggled */
sanou8 21:2c26b74a3e48 115 //pc.printf("%f", emg1_filtered)
sanou8 21:2c26b74a3e48 116 //led = !led;
sanou8 21:2c26b74a3e48 117 }
sanou8 21:2c26b74a3e48 118
sanou8 23:d13db573a875 119 void EMGcalibration()
sanou8 23:d13db573a875 120 {
sanou8 23:d13db573a875 121
sanou8 23:d13db573a875 122 Timer tijd;
sanou8 23:d13db573a875 123 tijd.start();
sanou8 23:d13db573a875 124 do {
sanou8 23:d13db573a875 125 ticker_calibration.attach(&sample, 0.002);
sanou8 23:d13db573a875 126 if(emg1_cal < emg1_filtered) {
sanou8 23:d13db573a875 127 emg1_cal = emg1_filtered ;
sanou8 23:d13db573a875 128 pc.printf("EMG_cal : %g \n\r",emg1_cal);
sanou8 23:d13db573a875 129 }
sanou8 23:d13db573a875 130 } while(tijd<10);
sanou8 23:d13db573a875 131 }
sanou8 23:d13db573a875 132
sanou8 23:d13db573a875 133
sanou8 23:d13db573a875 134 //function to mave a motor to a certain number of rotations, counted from the start of the program.
sanou8 23:d13db573a875 135 //parameters:
sanou8 23:d13db573a875 136 //DigitalOut *M = pointer to direction cpntol pin of motor
sanou8 23:d13db573a875 137 //PwmOut *E = pointer to speed contorl pin of motor
sanou8 23:d13db573a875 138 //QEI *Enc = pointer to encoder of motor
sanou8 23:d13db573a875 139 //double rotDes = desired rotation
sanou8 23:d13db573a875 140 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
sanou8 23:d13db573a875 141 {
sanou8 23:d13db573a875 142 double Kp = 30; //180 & 10 werkt zonder hulp
sanou8 23:d13db573a875 143 double Ki = 0;
sanou8 23:d13db573a875 144 double Kd = 2;
sanou8 23:d13db573a875 145
sanou8 23:d13db573a875 146 double pErrorC;
sanou8 23:d13db573a875 147 double pErrorP = 0;
sanou8 23:d13db573a875 148 double iError = 0;
sanou8 23:d13db573a875 149 double dError;
sanou8 23:d13db573a875 150
sanou8 23:d13db573a875 151 double U_p;
sanou8 23:d13db573a875 152 double U_i;
sanou8 23:d13db573a875 153 double U_d;
sanou8 23:d13db573a875 154
sanou8 23:d13db573a875 155 double rotC = Enc->getPulses()/(32*131.25) + initRot;
sanou8 23:d13db573a875 156 double MotorPWM;
sanou8 23:d13db573a875 157
sanou8 23:d13db573a875 158 Timer t;
sanou8 23:d13db573a875 159 double tieme = 0;
sanou8 23:d13db573a875 160
sanou8 23:d13db573a875 161 t.start();
sanou8 23:d13db573a875 162 do {
sanou8 23:d13db573a875 163 pErrorP = pErrorC;
sanou8 23:d13db573a875 164 rotC = Enc->getPulses()/(32*131.25) + initRot;
sanou8 23:d13db573a875 165 pErrorC = rotDes - rotC;
sanou8 23:d13db573a875 166 tieme = t.read();
sanou8 23:d13db573a875 167 t.reset();
sanou8 23:d13db573a875 168 iError = iError + pErrorC*tieme;
sanou8 23:d13db573a875 169 dError = (pErrorC - pErrorP)/tieme;
sanou8 23:d13db573a875 170
sanou8 23:d13db573a875 171 U_p = pErrorC*Kp;
sanou8 23:d13db573a875 172 U_i = iError*Ki;
sanou8 23:d13db573a875 173 U_d = dError*Kd;
sanou8 23:d13db573a875 174 MotorPWM = (U_p + U_i + U_d)*dir;
sanou8 23:d13db573a875 175
sanou8 23:d13db573a875 176 if(MotorPWM > 0) {
sanou8 23:d13db573a875 177 *M = 0;
sanou8 23:d13db573a875 178 *E = MotorPWM;
sanou8 23:d13db573a875 179 } else {
sanou8 23:d13db573a875 180 *M = 1;
sanou8 23:d13db573a875 181 *E = -MotorPWM;
sanou8 23:d13db573a875 182 }
sanou8 23:d13db573a875 183 wait(0.01);
sanou8 23:d13db573a875 184 //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
sanou8 23:d13db573a875 185 } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
sanou8 23:d13db573a875 186 *E = 0;
sanou8 23:d13db573a875 187 //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
sanou8 23:d13db573a875 188 t.stop();
sanou8 23:d13db573a875 189 }
sanou8 23:d13db573a875 190
sanou8 23:d13db573a875 191
sanou8 23:d13db573a875 192 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
sanou8 23:d13db573a875 193 {
sanou8 23:d13db573a875 194 double Kp = 61; //180 & 10 werkt zonder hulp
sanou8 23:d13db573a875 195 double Ki = 1;
sanou8 23:d13db573a875 196 double Kd = 7;
sanou8 23:d13db573a875 197
sanou8 23:d13db573a875 198 double rotC = Enc->getPulses()/(32*131.25) + initRot;
sanou8 23:d13db573a875 199
sanou8 23:d13db573a875 200 double pErrorC = rotDes - rotC;
sanou8 23:d13db573a875 201
sanou8 23:d13db573a875 202 double tieme = t->read();
sanou8 23:d13db573a875 203 double dt = tieme - pidInfo->at(2);
sanou8 23:d13db573a875 204
sanou8 23:d13db573a875 205 double iError = pidInfo->at(1) + pErrorC*dt;
sanou8 23:d13db573a875 206 double dError = (pErrorC - pidInfo->at(0))/dt;
sanou8 23:d13db573a875 207
sanou8 23:d13db573a875 208 double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;
sanou8 23:d13db573a875 209
sanou8 23:d13db573a875 210 if(MotorPWM > 0) {
sanou8 23:d13db573a875 211 *M = 0;
sanou8 23:d13db573a875 212 *E = MotorPWM;
sanou8 23:d13db573a875 213 } else {
sanou8 23:d13db573a875 214 *M = 1;
sanou8 23:d13db573a875 215 *E = -MotorPWM;
sanou8 23:d13db573a875 216 }
sanou8 23:d13db573a875 217 pidInfo->clear();
sanou8 23:d13db573a875 218 pidInfo->push_back(pErrorC);
sanou8 23:d13db573a875 219 pidInfo->push_back(iError);
sanou8 23:d13db573a875 220 pidInfo->push_back(tieme);
sanou8 23:d13db573a875 221 pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
sanou8 23:d13db573a875 222 }
sanou8 23:d13db573a875 223
sanou8 23:d13db573a875 224
sanou8 23:d13db573a875 225 //double that calculates the rotation of one arm.
sanou8 23:d13db573a875 226 //parameters:
sanou8 23:d13db573a875 227 //double xDes = ofset of the arm's shaft in cm in the x direction
sanou8 23:d13db573a875 228 //double yDes = ofset of the arm's shaft in cm in the y direction
sanou8 23:d13db573a875 229 double calcRot1(double xDes, double yDes)
sanou8 23:d13db573a875 230 {
sanou8 23:d13db573a875 231 double alpha = atan(yDes/xDes);
sanou8 23:d13db573a875 232 if(alpha < 0) {
sanou8 23:d13db573a875 233 alpha = alpha+Pi;
sanou8 23:d13db573a875 234 }
sanou8 23:d13db573a875 235 //pc.printf("alpha: %f", alpha);
sanou8 23:d13db573a875 236 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:d13db573a875 237 }
sanou8 23:d13db573a875 238
sanou8 23:d13db573a875 239 double calcRot2(double xDes, double yDes)
sanou8 23:d13db573a875 240 {
sanou8 23:d13db573a875 241 double alpha = atan(yDes/xDes);
sanou8 23:d13db573a875 242 if(alpha < 0) {
sanou8 23:d13db573a875 243 alpha = alpha+Pi;
sanou8 23:d13db573a875 244 }
sanou8 23:d13db573a875 245 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:d13db573a875 246 }
sanou8 23:d13db573a875 247
sanou8 23:d13db573a875 248 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
sanou8 23:d13db573a875 249 {
sanou8 23:d13db573a875 250 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
sanou8 23:d13db573a875 251 double traveledDistance = speed * t->read();
sanou8 23:d13db573a875 252 double ratio = traveledDistance/pathLength;
sanou8 23:d13db573a875 253
sanou8 23:d13db573a875 254 desiredLocation->clear();
sanou8 23:d13db573a875 255 desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
sanou8 23:d13db573a875 256 desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);
sanou8 23:d13db573a875 257
sanou8 23:d13db573a875 258 }
sanou8 23:d13db573a875 259
sanou8 23:d13db573a875 260 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
sanou8 23:d13db573a875 261 {
sanou8 23:d13db573a875 262 double rot1;
sanou8 23:d13db573a875 263 double rot2;
sanou8 23:d13db573a875 264
sanou8 23:d13db573a875 265 Timer t;
sanou8 23:d13db573a875 266
sanou8 23:d13db573a875 267 vector<double> desiredLocation;
sanou8 23:d13db573a875 268
sanou8 23:d13db573a875 269 vector<double> pidInfo1 (3);
sanou8 23:d13db573a875 270 vector<double> pidInfo2 (3);
sanou8 23:d13db573a875 271
sanou8 23:d13db573a875 272 fill(pidInfo1.begin(), pidInfo1.begin()+3, 0);
sanou8 23:d13db573a875 273 fill(pidInfo2.begin(), pidInfo2.begin()+3, 0);
sanou8 23:d13db573a875 274
sanou8 23:d13db573a875 275 double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
sanou8 23:d13db573a875 276
sanou8 23:d13db573a875 277 //Calculate the rotation of the motors at the start of the path
sanou8 23:d13db573a875 278 rot1 = calcRot1(xStart, yStart);
sanou8 23:d13db573a875 279 rot2 = calcRot2(xStart, yStart);
sanou8 23:d13db573a875 280 pc.printf("r1: %f r2: %f", rot1/6, rot2/6);
sanou8 23:d13db573a875 281
sanou8 23:d13db573a875 282 //Move arms to the start of the path
sanou8 23:d13db573a875 283 //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
sanou8 23:d13db573a875 284 //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
sanou8 23:d13db573a875 285
sanou8 23:d13db573a875 286 //start the timer
sanou8 23:d13db573a875 287 t.start();
sanou8 23:d13db573a875 288 while(pathLength > speed * t.read()) {
sanou8 23:d13db573a875 289 findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
sanou8 23:d13db573a875 290 rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
sanou8 23:d13db573a875 291 //pc.printf("\n\r Rot1: %f", rot1);
sanou8 23:d13db573a875 292 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
sanou8 23:d13db573a875 293 rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
sanou8 23:d13db573a875 294 pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
sanou8 23:d13db573a875 295 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
sanou8 23:d13db573a875 296 wait(0.01);
sanou8 23:d13db573a875 297 }
sanou8 23:d13db573a875 298
sanou8 23:d13db573a875 299 }
sanou8 23:d13db573a875 300
sanou8 23:d13db573a875 301
sanou8 23:d13db573a875 302
sanou8 23:d13db573a875 303 double calcX(double rot1, double rot2)
sanou8 23:d13db573a875 304 {
sanou8 23:d13db573a875 305 return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi);
sanou8 23:d13db573a875 306 }
sanou8 23:d13db573a875 307
sanou8 23:d13db573a875 308 double calcY(double rot1, double rot2)
sanou8 23:d13db573a875 309 {
sanou8 23:d13db573a875 310 return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi);
sanou8 23:d13db573a875 311 }
sanou8 23:d13db573a875 312
sanou8 23:d13db573a875 313
sanou8 23:d13db573a875 314 void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed)
sanou8 21:2c26b74a3e48 315 {
sanou8 23:d13db573a875 316 Timer t;
sanou8 23:d13db573a875 317
sanou8 23:d13db573a875 318 vector<double> pidInfo1 (4);
sanou8 23:d13db573a875 319 vector<double> pidInfo2 (4);
sanou8 23:d13db573a875 320
sanou8 23:d13db573a875 321 fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
sanou8 23:d13db573a875 322 fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
sanou8 23:d13db573a875 323
sanou8 23:d13db573a875 324 double xC = *xStart;
sanou8 23:d13db573a875 325 double yC = *yStart;
sanou8 23:d13db573a875 326
sanou8 23:d13db573a875 327 double rot1;
sanou8 23:d13db573a875 328 double rot2;
sanou8 23:d13db573a875 329
sanou8 23:d13db573a875 330 double tiemeP;
sanou8 23:d13db573a875 331 double tiemeC;
sanou8 23:d13db573a875 332 double dt;
sanou8 23:d13db573a875 333
sanou8 23:d13db573a875 334 t.start();
sanou8 23:d13db573a875 335
sanou8 23:d13db573a875 336 tiemeP = t.read();
sanou8 23:d13db573a875 337 while(t.read() < 0.1) {
sanou8 23:d13db573a875 338 tiemeC = t.read();
sanou8 23:d13db573a875 339 dt = tiemeC - tiemeP;
sanou8 23:d13db573a875 340 xC = xC+xSpeed*dt;
sanou8 23:d13db573a875 341 yC = yC+ySpeed*dt;
sanou8 23:d13db573a875 342 rot1 = calcRot1(xC, yC);
sanou8 23:d13db573a875 343 rot2 = calcRot2(xC, yC);
sanou8 23:d13db573a875 344 moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
sanou8 23:d13db573a875 345 moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
sanou8 23:d13db573a875 346 tiemeP = tiemeC;
sanou8 23:d13db573a875 347 wait(0.01);
sanou8 23:d13db573a875 348 }
sanou8 23:d13db573a875 349
sanou8 23:d13db573a875 350 *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3));
sanou8 23:d13db573a875 351 *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3));
sanou8 23:d13db573a875 352 }
sanou8 23:d13db573a875 353
sanou8 23:d13db573a875 354 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
sanou8 23:d13db573a875 355 {
sanou8 23:d13db573a875 356 Timer t;
sanou8 23:d13db573a875 357
sanou8 23:d13db573a875 358 double MotorPWM;
sanou8 23:d13db573a875 359
sanou8 23:d13db573a875 360 double posC;
sanou8 23:d13db573a875 361 double posP = Enc->getPulses()/(32*131.25);
sanou8 23:d13db573a875 362 double vel = 0;
sanou8 23:d13db573a875 363
sanou8 23:d13db573a875 364 int hasNotMoved = 0;
sanou8 23:d13db573a875 365
sanou8 23:d13db573a875 366
sanou8 23:d13db573a875 367 t.start();
sanou8 23:d13db573a875 368 do {
sanou8 23:d13db573a875 369 MotorPWM = speed - vel*0.7;
sanou8 23:d13db573a875 370 if(MotorPWM > 0) {
sanou8 23:d13db573a875 371 *M = 0;
sanou8 23:d13db573a875 372 *E = MotorPWM;
sanou8 23:d13db573a875 373 } else {
sanou8 23:d13db573a875 374 *M = 1;
sanou8 23:d13db573a875 375 *E = -MotorPWM;
sanou8 23:d13db573a875 376 }
sanou8 23:d13db573a875 377 wait(0.01);
sanou8 23:d13db573a875 378 posC = Enc->getPulses()/(32*131.25);
sanou8 23:d13db573a875 379 vel = (posC - posP)/t.read();
sanou8 23:d13db573a875 380 t.reset();
sanou8 23:d13db573a875 381 posP = posC;
sanou8 23:d13db573a875 382 pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved);
sanou8 23:d13db573a875 383 if(abs(vel) > 0.001) {
sanou8 23:d13db573a875 384 hasNotMoved = 0;
sanou8 23:d13db573a875 385 } else {
sanou8 23:d13db573a875 386 hasNotMoved++;
sanou8 23:d13db573a875 387 }
sanou8 23:d13db573a875 388 } while(hasNotMoved < 6);
sanou8 23:d13db573a875 389 *E = 0;
sanou8 23:d13db573a875 390 }
sanou8 23:d13db573a875 391
sanou8 23:d13db573a875 392 void calibrateMotor()
sanou8 23:d13db573a875 393 {
sanou8 23:d13db573a875 394 moveMotorToStop(&M1, &E1, &Enc1, -0.1);
sanou8 23:d13db573a875 395 moveMotorToStop(&M2, &E2, &Enc2, 0.2);
sanou8 23:d13db573a875 396 Enc2.reset();
sanou8 23:d13db573a875 397 moveMotorToStop(&M1, &E1, &Enc1, -0.1);
sanou8 23:d13db573a875 398 Enc1.reset();
sanou8 23:d13db573a875 399 pc.printf("%f", Enc1.getPulses());
sanou8 23:d13db573a875 400 }
sanou8 23:d13db573a875 401
sanou8 23:d13db573a875 402 void EMG_move()
sanou8 23:d13db573a875 403 {
sanou8 23:d13db573a875 404
sanou8 23:d13db573a875 405 double xStart = 0;
sanou8 23:d13db573a875 406 double yStart = 20;
sanou8 23:d13db573a875 407
sanou8 23:d13db573a875 408 double rot1 = calcRot1(xStart, yStart);
sanou8 23:d13db573a875 409 double rot2 = calcRot2(xStart, yStart);
sanou8 23:d13db573a875 410
sanou8 23:d13db573a875 411 moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
sanou8 23:d13db573a875 412 moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
sanou8 23:d13db573a875 413 sw2.rise(changespeed);
sanou8 23:d13db573a875 414 while(true) {
sanou8 23:d13db573a875 415
sanou8 23:d13db573a875 416
sanou8 23:d13db573a875 417 while(emg1_filtered >= 0.4*emg1_cal) {
sanou8 23:d13db573a875 418 //controlLED = !controlLED ;
sanou8 23:d13db573a875 419 moveWithSpeed(&xStart, &yStart, 0, speedy);
sanou8 23:d13db573a875 420 pc.printf("ystart: %g \n\r",yStart);
sanou8 21:2c26b74a3e48 421 }
sanou8 23:d13db573a875 422
sanou8 23:d13db573a875 423 rot1 = calcRot1(xStart, yStart);
sanou8 23:d13db573a875 424 rot2 = calcRot2(xStart, yStart);
sanou8 23:d13db573a875 425 moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
sanou8 23:d13db573a875 426 moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
sanou8 23:d13db573a875 427 }
sanou8 23:d13db573a875 428 }
sanou8 23:d13db573a875 429
sanou8 23:d13db573a875 430 void DEMO_motor()
sanou8 23:d13db573a875 431 {
sanou8 23:d13db573a875 432 while(1){
sanou8 23:d13db573a875 433 moveAlongPath(10, 30, -10, 30, 1);
sanou8 23:d13db573a875 434 moveAlongPath(-10, 30, -10, 20, 1);
sanou8 23:d13db573a875 435 moveAlongPath(-10, 20, 10, 20, 1);
sanou8 23:d13db573a875 436 moveAlongPath(10, 20, 10, 30, 1);
sanou8 23:d13db573a875 437 }
sanou8 23:d13db573a875 438 }
sanou8 23:d13db573a875 439
sanou8 23:d13db573a875 440
sanou8 23:d13db573a875 441
sanou8 23:d13db573a875 442
sanou8 23:d13db573a875 443
sanou8 23:d13db573a875 444 void state_machine(void)
sanou8 23:d13db573a875 445 {
sanou8 23:d13db573a875 446
sanou8 23:d13db573a875 447 switch(currentState) {
sanou8 23:d13db573a875 448
sanou8 23:d13db573a875 449 case WAITING:
sanou8 23:d13db573a875 450 // Description:
sanou8 23:d13db573a875 451 // In this state we do nothing, and wait for a command
sanou8 23:d13db573a875 452
sanou8 23:d13db573a875 453 // Actions
sanou8 23:d13db573a875 454 led_red = 0;
sanou8 23:d13db573a875 455 led_green = 0;
sanou8 23:d13db573a875 456 led_blue = 0; // Colouring the led WHITE
sanou8 23:d13db573a875 457
sanou8 23:d13db573a875 458 // State transition logic
sanou8 23:d13db573a875 459 if (button_motor < 1){
sanou8 23:d13db573a875 460 currentState = MOTOR_ANGLE_CLBRT;
sanou8 23:d13db573a875 461 stateChanged = true;
sanou8 23:d13db573a875 462 pc.printf("Starting Calibration\n\r");
sanou8 23:d13db573a875 463 } else if (Fail_button < 1){
sanou8 23:d13db573a875 464 currentState = FAILURE_MODE;
sanou8 23:d13db573a875 465 stateChanged = true;
sanou8 23:d13db573a875 466 }
sanou8 23:d13db573a875 467 break;
sanou8 23:d13db573a875 468
sanou8 23:d13db573a875 469 case MOTOR_ANGLE_CLBRT:
sanou8 23:d13db573a875 470 // Description:
sanou8 23:d13db573a875 471 // In this state the robot moves to a mechanical limit of motion,
sanou8 23:d13db573a875 472 // in order to calibrate the motors.
sanou8 23:d13db573a875 473 led_red = 1;
sanou8 23:d13db573a875 474 led_green = 0;
sanou8 23:d13db573a875 475 led_blue = 0;
sanou8 23:d13db573a875 476 timer.start();
sanou8 23:d13db573a875 477 if (stateChanged) {
sanou8 23:d13db573a875 478 calibrateMotor(); // Actuate motor 1 and 2.
sanou8 23:d13db573a875 479 stateChanged = true; // Keep this loop going, until the transition conditions are satisfied.
sanou8 23:d13db573a875 480 }
sanou8 23:d13db573a875 481 time_in_state = timer.read();
sanou8 23:d13db573a875 482 if(time_in_state>5) {
sanou8 23:d13db573a875 483 currentState = EMG_CLBRT ;
sanou8 23:d13db573a875 484 stateChanged = true;
sanou8 23:d13db573a875 485 pc.printf("Starting Calibration\n\r");
sanou8 23:d13db573a875 486 }
sanou8 23:d13db573a875 487
sanou8 23:d13db573a875 488 break;
sanou8 23:d13db573a875 489
sanou8 23:d13db573a875 490 case EMG_CLBRT:
sanou8 23:d13db573a875 491 led_red = 1;
sanou8 23:d13db573a875 492 led_green = 1;
sanou8 23:d13db573a875 493 led_blue = 0;
sanou8 23:d13db573a875 494 timer.start() ;
sanou8 23:d13db573a875 495 if (stateChanged) {
sanou8 23:d13db573a875 496 EMGcalibration();
sanou8 23:d13db573a875 497 stateChanged = true;
sanou8 23:d13db573a875 498 }
sanou8 23:d13db573a875 499 time_in_state = timer.read();
sanou8 23:d13db573a875 500 if (time_in_state >=5) {
sanou8 23:d13db573a875 501 currentState = WAITING4SIGNAL ;
sanou8 23:d13db573a875 502 stateChanged = true;
sanou8 23:d13db573a875 503 pc.printf("Waiting for signal");
sanou8 23:d13db573a875 504 break;
sanou8 23:d13db573a875 505
sanou8 23:d13db573a875 506 case WAITING4SIGNAL:
sanou8 23:d13db573a875 507 led_red = 1;
sanou8 23:d13db573a875 508 led_green = 0;
sanou8 23:d13db573a875 509 led_blue = 1;
sanou8 23:d13db573a875 510 if (button_motor < 1) {
sanou8 23:d13db573a875 511 currentState = MOTOR_ANGLE_CLBRT;
sanou8 23:d13db573a875 512 stateChanged = true;
sanou8 23:d13db573a875 513 pc.printf("Starting Calibration \n\r");
sanou8 23:d13db573a875 514 } else if (button_demo < 1) {
sanou8 23:d13db573a875 515 currentState = MOVE_W_DEMO;
sanou8 23:d13db573a875 516 stateChanged = true;
sanou8 23:d13db573a875 517 pc.printf("DEMO mode \r\n");
sanou8 23:d13db573a875 518 wait(1.0f);
sanou8 23:d13db573a875 519 } else if (button_emg <1) {
sanou8 23:d13db573a875 520 currentState = MOVE_W_EMG;
sanou8 23:d13db573a875 521 stateChanged = true;
sanou8 23:d13db573a875 522 pc.printf("EMG mode\r\n");
sanou8 23:d13db573a875 523 wait(1.0f);
sanou8 23:d13db573a875 524 } else if (Fail_button == 0) {
sanou8 23:d13db573a875 525 currentState = FAILURE_MODE;
sanou8 23:d13db573a875 526 stateChanged = true;
sanou8 23:d13db573a875 527 }
sanou8 23:d13db573a875 528
sanou8 23:d13db573a875 529 break;
sanou8 23:d13db573a875 530
sanou8 23:d13db573a875 531 case MOVE_W_EMG:
sanou8 23:d13db573a875 532 if (stateChanged) {
sanou8 23:d13db573a875 533 void EMG_move();
sanou8 23:d13db573a875 534 stateChanged = true;
sanou8 23:d13db573a875 535 }
sanou8 23:d13db573a875 536 if(button_demo < 1) {
sanou8 23:d13db573a875 537 currentState = WAITING4SIGNAL ;
sanou8 23:d13db573a875 538 stateChanged = true;
sanou8 23:d13db573a875 539 }
sanou8 23:d13db573a875 540
sanou8 23:d13db573a875 541 break;
sanou8 23:d13db573a875 542
sanou8 23:d13db573a875 543 case MOVE_W_DEMO:
sanou8 23:d13db573a875 544 if (stateChanged) {
sanou8 23:d13db573a875 545 DEMO_motor() ;
sanou8 23:d13db573a875 546 stateChanged = true;
sanou8 23:d13db573a875 547 }
sanou8 23:d13db573a875 548 if(button_demo < 1) {
sanou8 23:d13db573a875 549 currentState = WAITING4SIGNAL ;
sanou8 23:d13db573a875 550 stateChanged = true;
sanou8 23:d13db573a875 551 }
sanou8 23:d13db573a875 552
sanou8 23:d13db573a875 553 break;
sanou8 23:d13db573a875 554
sanou8 23:d13db573a875 555 case FAILURE_MODE:
sanou8 23:d13db573a875 556 if (stateChanged) {
sanou8 23:d13db573a875 557 led_red = 0;
sanou8 23:d13db573a875 558 led_green = 1;
sanou8 23:d13db573a875 559 led_blue = 1;
sanou8 23:d13db573a875 560 stateChanged = true;
sanou8 23:d13db573a875 561 }
sanou8 23:d13db573a875 562 break;
sanou8 23:d13db573a875 563
sanou8 23:d13db573a875 564
sanou8 23:d13db573a875 565
sanou8 23:d13db573a875 566
sanou8 23:d13db573a875 567 }
sanou8 23:d13db573a875 568 }
sanou8 23:d13db573a875 569 }