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?
User | Revision | Line number | New 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 | } |