totale unit
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 23:d13db573a875
- Parent:
- 22:08b3cd7bec7f
--- a/main.cpp Tue Oct 29 12:11:23 2019 +0000 +++ b/main.cpp Thu Oct 31 20:36:14 2019 +0000 @@ -4,68 +4,94 @@ #include "BiQuad.h" #include "BiQuad4.h" #include "MODSERIAL.h" +#include "QEI.h" +#include <vector> + +using std::vector; Serial pc(USBTX,USBRX); -DigitalIn button(SW3) ; - +DigitalIn button_motor(SW2) ; +DigitalIn button_demo(D9) ; +DigitalIn button_emg(D8); +DigitalIn Fail_button(SW3); +InterruptIn sw2(SW2); +DigitalOut led_red(LED_RED); +DigitalOut led_green(LED_GREEN); +DigitalOut led_blue(LED_BLUE); //Define objects AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); -AnalogIn potmeter1(PTC11); // Input of two potmeters -AnalogIn potmeter2(PTC10); +QEI Enc1(D12, D13, NC, 32); +QEI Enc2(D10, D11, NC, 32); +DigitalOut M1(D4); +DigitalOut M2(D7); +PwmOut E1(D5); +PwmOut E2(D6); - +Ticker StateMachine; Ticker ticker_calibration; // Ticker to send the EMG signals to screen Ticker sample_timer; // Ticker for reading out EMG HIDScope scope( 2 ); -DigitalOut led(LED1); +DigitalOut led(LED_RED); +DigitalOut controlLED(LED_GREEN); +Timer timer; volatile double emg1_filtered; //measured value of the first emg volatile double emg2_filtered; //measured value of the second emg -volatile double emg1_max ; // calibrated value of first emg -volatile double emg2_max ; volatile double emg1_cal = 0.8; - - // Read EMG -//void EMGread() -//{ -// emg1_filtered = FilterDesign(emg0.read()); -// emg2_filtered = FilterDesign(emg1.read()); - //pc.printf("emg1_cal = %f, emg2_cal = %f \n\r", emg1_filtered, emg2_filtered); -//} +double Pi = 3.14159265359; +double gearRatio = 40/9; +double time_in_state ; +double initRot1 = 0; +double initRot2 = -gearRatio*(180 - 48.5)/360; +volatile double y_new = 20.0 ; +volatile double y_prev = 20.0 ; +double displacement = 1 ; +double speedy = 3 ; - +enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, OPERATION, SHOOTING, MOVE_W_DEMO, FAILURE_MODE}; +states currentState = WAITING; // Start in waiting state +bool stateChanged = true; void sample() ; -void EMGcalibration () ; +void EMGcalibration() ; +void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes); +double calcRot1(double xDes, double yDes); +double calcRot2(double xDes, double yDes); +void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]); +void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed); +void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes); +double calcX(double rot1, double rot2); +double calcY(double rot1, double rot2); +void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed); +void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation); +void DEMO_motor() ; +void EMG_move(); +void state_machine(void); +void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed); +void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2); - +void changespeed() +{ + speedy = speedy*-1; +} int main() -{ - /**Attach the 'sample' function to the timer 'sample_timer'. - * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz - */ - +{ + led_red = 1; + led_green = 1; + led_blue = 1; + pc.baud(115200); - //EMGcalibration(); - sample_timer.attach(&sample, 0.002); - - - /*empty loop, sample() is executed periodically*/ + + +StateMachine.attach(&state_machine, 0.005f); while(true) { - if(SW3==0){ - EMGcalibration(); - } - - led = 1; - - while(emg1_filtered >= 0.9*emg1_cal){ - led = 0; - } + + } } @@ -74,14 +100,14 @@ -void sample() +void sample() { emg1_filtered = FilterDesign(emg0.read()); emg2_filtered = FilterDesign(emg1.read()); /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ scope.set(0, emg1_filtered ) ; scope.set(1, emg2_filtered ); - /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) + /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) * Ensure that enough channels are available (HIDScope scope( 2 )) * Finally, send all channels to the PC at once */ scope.send(); @@ -90,15 +116,454 @@ //led = !led; } -void EMGcalibration () +void EMGcalibration() +{ + + Timer tijd; + tijd.start(); + do { + ticker_calibration.attach(&sample, 0.002); + if(emg1_cal < emg1_filtered) { + emg1_cal = emg1_filtered ; + pc.printf("EMG_cal : %g \n\r",emg1_cal); + } + } while(tijd<10); +} + + +//function to mave a motor to a certain number of rotations, counted from the start of the program. +//parameters: +//DigitalOut *M = pointer to direction cpntol pin of motor +//PwmOut *E = pointer to speed contorl pin of motor +//QEI *Enc = pointer to encoder of motor +//double rotDes = desired rotation +void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes) +{ + double Kp = 30; //180 & 10 werkt zonder hulp + double Ki = 0; + double Kd = 2; + + double pErrorC; + double pErrorP = 0; + double iError = 0; + double dError; + + double U_p; + double U_i; + double U_d; + + double rotC = Enc->getPulses()/(32*131.25) + initRot; + double MotorPWM; + + Timer t; + double tieme = 0; + + t.start(); + do { + pErrorP = pErrorC; + rotC = Enc->getPulses()/(32*131.25) + initRot; + pErrorC = rotDes - rotC; + tieme = t.read(); + t.reset(); + iError = iError + pErrorC*tieme; + dError = (pErrorC - pErrorP)/tieme; + + U_p = pErrorC*Kp; + U_i = iError*Ki; + U_d = dError*Kd; + MotorPWM = (U_p + U_i + U_d)*dir; + + if(MotorPWM > 0) { + *M = 0; + *E = MotorPWM; + } else { + *M = 1; + *E = -MotorPWM; + } + wait(0.01); + //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); + } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); + *E = 0; + //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); + t.stop(); +} + + +void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes) +{ + double Kp = 61; //180 & 10 werkt zonder hulp + double Ki = 1; + double Kd = 7; + + double rotC = Enc->getPulses()/(32*131.25) + initRot; + + double pErrorC = rotDes - rotC; + + double tieme = t->read(); + double dt = tieme - pidInfo->at(2); + + double iError = pidInfo->at(1) + pErrorC*dt; + double dError = (pErrorC - pidInfo->at(0))/dt; + + double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir; + + if(MotorPWM > 0) { + *M = 0; + *E = MotorPWM; + } else { + *M = 1; + *E = -MotorPWM; + } + pidInfo->clear(); + pidInfo->push_back(pErrorC); + pidInfo->push_back(iError); + pidInfo->push_back(tieme); + pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot); +} + + +//double that calculates the rotation of one arm. +//parameters: +//double xDes = ofset of the arm's shaft in cm in the x direction +//double yDes = ofset of the arm's shaft in cm in the y direction +double calcRot1(double xDes, double yDes) +{ + double alpha = atan(yDes/xDes); + if(alpha < 0) { + alpha = alpha+Pi; + } + //pc.printf("alpha: %f", alpha); + 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)); +} + +double calcRot2(double xDes, double yDes) +{ + double alpha = atan(yDes/xDes); + if(alpha < 0) { + alpha = alpha+Pi; + } + 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)); +} + +void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation) +{ + double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0)); + double traveledDistance = speed * t->read(); + double ratio = traveledDistance/pathLength; + + desiredLocation->clear(); + desiredLocation->push_back(xStart + (xEnd - xStart)*ratio); + desiredLocation->push_back(yStart + (yEnd - yStart)*ratio); + +} + +void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed) +{ + double rot1; + double rot2; + + Timer t; + + vector<double> desiredLocation; + + vector<double> pidInfo1 (3); + vector<double> pidInfo2 (3); + + fill(pidInfo1.begin(), pidInfo1.begin()+3, 0); + fill(pidInfo2.begin(), pidInfo2.begin()+3, 0); + + double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0)); + + //Calculate the rotation of the motors at the start of the path + rot1 = calcRot1(xStart, yStart); + rot2 = calcRot2(xStart, yStart); + pc.printf("r1: %f r2: %f", rot1/6, rot2/6); + + //Move arms to the start of the path + //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2); + //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1); + + //start the timer + t.start(); + while(pathLength > speed * t.read()) { + findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation); + rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1)); + //pc.printf("\n\r Rot1: %f", rot1); + moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2); + rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1)); + pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2); + moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1); + wait(0.01); + } + +} + + + +double calcX(double rot1, double rot2) +{ + return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi); +} + +double calcY(double rot1, double rot2) +{ + return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi); +} + + +void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed) { - -Timer t; -t.start(); - do { - ticker_calibration.attach(&sample, 0.002); - if(emg1_cal < emg1_filtered){ - emg1_cal = emg1_filtered ; + Timer t; + + vector<double> pidInfo1 (4); + vector<double> pidInfo2 (4); + + fill(pidInfo1.begin(), pidInfo1.begin()+4, 0); + fill(pidInfo2.begin(), pidInfo2.begin()+4, 0); + + double xC = *xStart; + double yC = *yStart; + + double rot1; + double rot2; + + double tiemeP; + double tiemeC; + double dt; + + t.start(); + + tiemeP = t.read(); + while(t.read() < 0.1) { + tiemeC = t.read(); + dt = tiemeC - tiemeP; + xC = xC+xSpeed*dt; + yC = yC+ySpeed*dt; + rot1 = calcRot1(xC, yC); + rot2 = calcRot2(xC, yC); + moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2); + moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1); + tiemeP = tiemeC; + wait(0.01); + } + + *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3)); + *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3)); +} + +void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed) +{ + Timer t; + + double MotorPWM; + + double posC; + double posP = Enc->getPulses()/(32*131.25); + double vel = 0; + + int hasNotMoved = 0; + + + t.start(); + do { + MotorPWM = speed - vel*0.7; + if(MotorPWM > 0) { + *M = 0; + *E = MotorPWM; + } else { + *M = 1; + *E = -MotorPWM; + } + wait(0.01); + posC = Enc->getPulses()/(32*131.25); + vel = (posC - posP)/t.read(); + t.reset(); + posP = posC; + pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved); + if(abs(vel) > 0.001) { + hasNotMoved = 0; + } else { + hasNotMoved++; + } + } while(hasNotMoved < 6); + *E = 0; +} + +void calibrateMotor() +{ + moveMotorToStop(&M1, &E1, &Enc1, -0.1); + moveMotorToStop(&M2, &E2, &Enc2, 0.2); + Enc2.reset(); + moveMotorToStop(&M1, &E1, &Enc1, -0.1); + Enc1.reset(); + pc.printf("%f", Enc1.getPulses()); +} + +void EMG_move() +{ + + double xStart = 0; + double yStart = 20; + + double rot1 = calcRot1(xStart, yStart); + double rot2 = calcRot2(xStart, yStart); + + moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2); + moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1); + sw2.rise(changespeed); + while(true) { + + + while(emg1_filtered >= 0.4*emg1_cal) { + //controlLED = !controlLED ; + moveWithSpeed(&xStart, &yStart, 0, speedy); + pc.printf("ystart: %g \n\r",yStart); } - }while(t<10); -} \ No newline at end of file + + rot1 = calcRot1(xStart, yStart); + rot2 = calcRot2(xStart, yStart); + moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2); + moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1); + } +} + +void DEMO_motor() +{ + while(1){ + moveAlongPath(10, 30, -10, 30, 1); + moveAlongPath(-10, 30, -10, 20, 1); + moveAlongPath(-10, 20, 10, 20, 1); + moveAlongPath(10, 20, 10, 30, 1); + } +} + + + + + +void state_machine(void) +{ + + switch(currentState) { + + case WAITING: + // Description: + // In this state we do nothing, and wait for a command + + // Actions + led_red = 0; + led_green = 0; + led_blue = 0; // Colouring the led WHITE + + // State transition logic + if (button_motor < 1){ + currentState = MOTOR_ANGLE_CLBRT; + stateChanged = true; + pc.printf("Starting Calibration\n\r"); + } else if (Fail_button < 1){ + currentState = FAILURE_MODE; + stateChanged = true; + } + break; + + case MOTOR_ANGLE_CLBRT: + // Description: + // In this state the robot moves to a mechanical limit of motion, + // in order to calibrate the motors. + led_red = 1; + led_green = 0; + led_blue = 0; + timer.start(); + if (stateChanged) { + calibrateMotor(); // Actuate motor 1 and 2. + stateChanged = true; // Keep this loop going, until the transition conditions are satisfied. + } + time_in_state = timer.read(); + if(time_in_state>5) { + currentState = EMG_CLBRT ; + stateChanged = true; + pc.printf("Starting Calibration\n\r"); + } + + break; + + case EMG_CLBRT: + led_red = 1; + led_green = 1; + led_blue = 0; + timer.start() ; + if (stateChanged) { + EMGcalibration(); + stateChanged = true; + } + time_in_state = timer.read(); + if (time_in_state >=5) { + currentState = WAITING4SIGNAL ; + stateChanged = true; + pc.printf("Waiting for signal"); + break; + + case WAITING4SIGNAL: + led_red = 1; + led_green = 0; + led_blue = 1; + if (button_motor < 1) { + currentState = MOTOR_ANGLE_CLBRT; + stateChanged = true; + pc.printf("Starting Calibration \n\r"); + } else if (button_demo < 1) { + currentState = MOVE_W_DEMO; + stateChanged = true; + pc.printf("DEMO mode \r\n"); + wait(1.0f); + } else if (button_emg <1) { + currentState = MOVE_W_EMG; + stateChanged = true; + pc.printf("EMG mode\r\n"); + wait(1.0f); + } else if (Fail_button == 0) { + currentState = FAILURE_MODE; + stateChanged = true; + } + + break; + + case MOVE_W_EMG: + if (stateChanged) { + void EMG_move(); + stateChanged = true; + } + if(button_demo < 1) { + currentState = WAITING4SIGNAL ; + stateChanged = true; + } + + break; + + case MOVE_W_DEMO: + if (stateChanged) { + DEMO_motor() ; + stateChanged = true; + } + if(button_demo < 1) { + currentState = WAITING4SIGNAL ; + stateChanged = true; + } + + break; + + case FAILURE_MODE: + if (stateChanged) { + led_red = 0; + led_green = 1; + led_blue = 1; + stateChanged = true; + } + break; + + + + +} +} +}