Fully finished code
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
Revision 11:cdef02091b40, committed 2019-11-05
- Comitter:
- JonaVonk
- Date:
- Tue Nov 05 16:03:38 2019 +0000
- Parent:
- 10:cbcb35182ef1
- Commit message:
- Nu met allerlei comments enzo
Changed in this revision
Filter/FilterDesign.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r cbcb35182ef1 -r cdef02091b40 Filter/FilterDesign.cpp --- a/Filter/FilterDesign.cpp Tue Nov 05 13:23:20 2019 +0000 +++ b/Filter/FilterDesign.cpp Tue Nov 05 16:03:38 2019 +0000 @@ -30,14 +30,13 @@ // Multiplication with the gain double gain = 10.00000; - -BiQuad notch50(nb0, nb1, nb2, na1, na2); + BiQuad4 highpass(hpb0, hpb1, hpb2, hpb3, hpb4, hpa1, hpa2, hpa3, hpa4); BiQuad4 lowpass(lpb0, lpb1, lpb2, lpb3, lpb4, lpa1, lpa2, lpa3, lpa4); double FilterDesign(double u) { - double y_hp = highpass.step(y_n); // First the highpassfilter + double y_hp = highpass.step(u); // First the highpassfilter double y_abs = abs(y_hp); // Make the signal values absolute double y_lp = lowpass.step(y_abs); // Then a lowpass filter double y_gain = y_lp*gain; // Multiply by a gain
diff -r cbcb35182ef1 -r cdef02091b40 main.cpp --- a/main.cpp Tue Nov 05 13:23:20 2019 +0000 +++ b/main.cpp Tue Nov 05 16:03:38 2019 +0000 @@ -11,8 +11,6 @@ MODSERIAL pc(USBTX, USBRX); -// main() runs in its own thread in the OS - //State machine InterruptIn stateSwitch(SW2); InterruptIn stepSwitch(SW3); @@ -111,6 +109,7 @@ int main() { + //set up some functions of Intterrupts stateSwitch.rise(switchState); stepSwitch.rise(switchStep); @@ -121,6 +120,7 @@ ySwitch.fall(flipy); pc.baud(115200); + //Once everything is set, this while loop runs over and over again. The stateMachine funcion finds the function that has to be executed and executes it. while(1) { stateMachine(); } @@ -129,21 +129,30 @@ void switchState() { + //this switches between the two states state++; state = state%2; + + //the number of steps has to start at 0 everytime one switches between states stateStep = 0; + + //the switch set everything up to start the different modes switch(state) { //demo mode case 0: + //stop the timer used for moving with EMG tEMGMove.stop(); - pc.printf("demo\n\r"); + //sets the number of steps in this mode noStateSteps = 4; break; + //EMG mode case 1: - pc.printf("EMG\n\r"); + //sets the number of steps in this mode noStateSteps = 7; + //empties the vectors with info needed for PID control fill(pidInfo1.begin(), pidInfo1.begin()+4, 0); fill(pidInfo2.begin(), pidInfo2.begin()+4, 0); + //resets and starts the timer used for moving with EMG tEMGMove.reset(); tEMGMove.start(); break; @@ -152,12 +161,14 @@ void switchStep() { + //keeps track of the step the machine is at within it's mode stateStep++; stateStep = stateStep%noStateSteps; } void stateMachine() { + //switch between the modes switch (state) { case 0: demoState(); @@ -170,19 +181,23 @@ void demoState() { - pc.printf("demo %d\n\r", stateStep); + //switches between the steps within the demo mode switch (stateStep) { case 0: + //waits waiting(); break; case 1: + //calibrates and moves on to the next step calibrateMotors(); stateStep++; break; case 2: + //waits waiting(); break; case 3: + //plays the demo playDemo(); break; } @@ -190,32 +205,39 @@ void EMGState() { + //switches between the steps within the EMG mode pc.printf("EMG %d\n\r", stateStep); switch(stateStep) { case 0: + //waits waiting(); break; case 1: + //calibrates the motors and moves on to the next step calibrateMotors(); stateStep++; break; case 2: + //waits waiting(); break; case 3: + //calibrates the EMG and moves on to the next step EMGcalibration(); - pc.printf("hack"); stateStep++; break; case 4: + //waits waiting(); break; case 5: + //moves to the starting position for EMG moveToInitialPosition(); xCurrent = 0; yCurrent = 20; break; case 6: + //uses EMG signals to determin the speed of the end effector moveWithEMG(); break; } @@ -223,6 +245,7 @@ void waiting() { + //turns on an LED and makes sure that the motors stop moving r_led = 0; E1 =0; E2 =0; @@ -231,16 +254,17 @@ void calibrateMotors() { + //Calibrates the motors by moving them until they can't go further and then resets the encoders moveMotorToStop(&M1, &E1, &Enc1, -0.1); moveMotorToStop(&M2, &E2, &Enc2, 0.2); + moveMotorToStop(&M1, &E1, &Enc1, -0.1); Enc2.reset(); - moveMotorToStop(&M1, &E1, &Enc1, -0.1); Enc1.reset(); - pc.printf("%f", Enc1.getPulses()); } void playDemo() { + //moves the end effector along the lines of a rectangle moveAlongPath(10, 30, -10, 30, 3); moveAlongPath(-10, 30, -10, 20, 3); moveAlongPath(-10, 20, 10, 20, 3); @@ -254,19 +278,19 @@ ticker_calibration.attach(&sample, 0.002); // Ticker for reading EMG-signals is attached to the function that filters the emg-signals do { if(emg1_cal < emg1_filtered) { // Initial calibration value is compaired to the current EMG value. - emg1_cal = emg1_filtered ; // When the current EMG value is higher than the calibration value, then the calibration - pc.printf("EMG_cal : %g \n\r",emg1_cal); // value becomes the current EMG value. + emg1_cal = emg1_filtered ; // When the current EMG value is higher than the calibration value, then the calibration value becomes the current EMG value. } if(emg2_cal < emg2_filtered) { // The same is true for the second EMG signal emg2_cal = emg2_filtered ; } - pc.printf("emg1: %f\n\r", emg1_filtered); } while(tijd.read()<10); // After ten seconds the calibration of the EMG-signals stop } void moveToInitialPosition() { + //Moves the endeffector to it's initial position of (0, 20) + double xStart = 0; double yStart = 20; @@ -281,42 +305,56 @@ { if(emg1_filtered >= 0.5*emg1_cal) { // When the current value of EMG1 is higher than half of the calibration value, speedy = 3; // the speed in Y direction becomes 3. Otherwise the speed stays 0. - pc.printf("emg1: %f", emg1_filtered); } else { speedy = 0; } if(emg2_filtered >= 0.5*emg2_cal) { // When the current value of EMG2 is higher than half of the calibration value, speedx = 3; // the speed in X direction becomes 3. Otherwise the speed stays 0. - pc.printf("emg1: %f\n\r", emg2_filtered); } else { speedx = 0; } - pc.printf("speedx: %f speedy: %f\r\n", speedx, speedy); + + + //caluculates the position at wich the en effector should be given a certain speed xCurrent = xCurrent + xDir*speedx*0.01; yCurrent = yCurrent + yDir*speedy*0.01; + + //calculates the desired rotation of the motor and moves it there. double rot2 = calcRot2(xCurrent, yCurrent); + double rot1 = calcRot1(xCurrent, yCurrent); moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &tEMGMove, -rot2); - double rot1 = calcRot1(xCurrent, yCurrent); moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &tEMGMove, rot1); wait(0.01); } +/* +takes as an input: +*M = a pointer to the pin controling the direction of the rotation of the motor +*E = a pointer to the pin controling the speed of the motor +*Enc = a pointer to the encoder of the motor +speed = a double to set the speed at which the motor has to rotate +*/ void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed) { Timer t; double MotorPWM; + // defines (and sets) the variables for the cuurent position of the motor, it's previous position and it's velocity double posC; double posP = Enc->getPulses()/(32*131.25); double vel = 0; + //defines a cunter to keep track of when the motor is standing still int hasNotMoved = 0; t.start(); do { + //sets the duty cycle for the motor and prevents the motor of movig too fast due to the weight attached to the and effector. MotorPWM = speed - vel*0.7; + + //makes sure the motor moves in he right direction with the right speed if(MotorPWM > 0) { *M = 0; *E = MotorPWM; @@ -325,108 +363,166 @@ *E = -MotorPWM; } wait(0.01); + + //reads a new value for the position of the motor posC = Enc->getPulses()/(32*131.25); + //calculates the speed of the motor vel = (posC - posP)/t.read(); t.reset(); posP = posC; - //pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved); + //keeps track of the consecutive number of times the motor did not move if(abs(vel) > 0.001) { hasNotMoved = 0; } else { hasNotMoved++; } } while(hasNotMoved < 6); + //stops the motors from moving *E = 0; } +/* +takes as input +xStart = a double to define the x-coordinate of the end effector at the start of the path +yStart = a double to define the y-coordinate of the end effector at the start of the path +xEnd = a double to define the x-coordinate of the end effector at the end of the path +yEnd = a double to define the y-coordinate of the end effector at the end of the path +speed = a double to set the speed at which the end effector should move along the path +*/ void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed) { + //define variables for motor rotations, a timer and a vector for the desired x and y coordinate of the end effector double rot1; double rot2; Timer t; vector<double> desiredLocation; - + + //fill the pid Info vectors with zeros fill(pidInfo1.begin(), pidInfo1.begin()+4, 0); fill(pidInfo2.begin(), pidInfo2.begin()+4, 0); + //calculate the length of the path along which the end effectr has to move 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(); + //move along the path for as long asit should take to do so idealy while(pathLength > speed * t.read()) { + //find the desired location of the end effector findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation); + + //find the desired rotation of the motors given the desired location of the end effector rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1)); - //pc.printf("\n\r Rot1: %f", rot1); + rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1)); + //Give the motors a certain speed given the rotation they should end up in 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); - //pc.printf("\n\r xCalc: %f yCalc: %f", calcX(pidInfo1.at(3), pidInfo2.at(3)), calcY(pidInfo1.at(3), pidInfo2.at(3))); wait(0.01); } } + +/* +takes as input: +xDes = the x-coordinate desired loaction of the end effector +yDes = the y-coordinate desired loaction of the end effector +*/ double calcRot1(double xDes, double yDes) { double alpha = atan(yDes/xDes); + //makes sure the right solution for alpha was given (there are two solutions and only the positive one is usefull) if(alpha < 0) { alpha = alpha+Pi; } - //pc.printf("alpha: %f", alpha); + //returns the desired rotation of motor 1 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)); } +/* +takes as input: +xDes = the x-coordinate desired loaction of the end effector +yDes = the y-coordinate desired loaction of the end effector +*/ double calcRot2(double xDes, double yDes) { double alpha = atan(yDes/xDes); + //makes sure the right solution for alpha was given (there are two solutions and only the positive one is usefull) if(alpha < 0) { alpha = alpha+Pi; } + //returns the desired rotation of motor 2 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)); } +/* +takes as input: +xStart = a double to define the x-coordinate of the end effector at the start of the path +yStart = a double to define the y-coordinate of the end effector at the start of the path +xEnd = a double to define the x-coordinate of the end effector at the end of the path +yEnd = a double to define the y-coordinate of the end effector at the end of the path +speed = a double to set the speed at which the end effector should move along the path +*t = a pointer to a time to keep track of the time that passed since the operation is started +*desiredLocation = a pointer to a vector of doubles in which the desired x and y coordinates can be stored +*/ void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation) { + //calculate the length of the path double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0)); + //calculate how far the end effector should have moved double traveledDistance = speed * t->read(); + + //calculate the desired location of the end effector and store it in the vector double ratio = traveledDistance/pathLength; - desiredLocation->clear(); desiredLocation->push_back(xStart + (xEnd - xStart)*ratio); desiredLocation->push_back(yStart + (yEnd - yStart)*ratio); } +/* +takes as input: +*M = a pointer to the pin controling the direction of the rotation of the motor +*E = a pointer to the pin controling the speed of the motor +*Enc = a pointer to the encoder of the motor +initRot = the rotation of the motor after it was calibrated +dir = the direction in which the motor has to move in order to make the error smaller +*pidInfo = a pointer to a vector in which values are stored that are needed for the next time this function runs +*t = a pointer to a time to keep track of the time that passed since the operation is started +rotDes = the desired rotation of the motor +*/ 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 + //defining and setting a variable to store the K values of the PID controler + double Kp = 61; double Ki = 1; double Kd = 7; + //defining and setting a variable to store the current rotation of the motor double rotC = Enc->getPulses()/(32*131.25) + initRot; + //defining and setting a variable to store the current current error double pErrorC = rotDes - rotC; + //defining and setting a variable to store the time that has passed since the start of the opperation double tieme = t->read(); + //defining and setting a variable to store delta t, using the passed time that was stored in the pidInfo vector double dt = tieme - pidInfo->at(2); + //defining and setting a variable to store the integration of the error using it's previous value that was stored in the pidInfo vector double iError = pidInfo->at(1) + pErrorC*dt; + //defining and setting a variable to store the derivative of the error using the previous value of the error that was stored in the pidInfo vector double dError = (pErrorC - pidInfo->at(0))/dt; + //defining and setting a variable to store duty cycle of the motor and making sure it moves in the right direction double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir; - + //set the PWM signal that pin E and the signal that pin M send to the motor shield if(MotorPWM > 0) { *M = 0; *E = MotorPWM; @@ -434,6 +530,7 @@ *M = 1; *E = -MotorPWM; } + //clear the pidInfo vector and fill it with te new values pidInfo->clear(); pidInfo->push_back(pErrorC); pidInfo->push_back(iError); @@ -453,82 +550,123 @@ scope.send(); // Finally, the values are send and can be used in other functions. } +/* +takes as input: +*xStart = a pointer to a double that keeps track of the x-cordinate at which the end effector should start +*yStart = a pointer to a double that keeps track of the y-cordinate at which the end effector should start +xSpeed = the desired speeed in the x direction +ySpeed = the desired seed in the y direction +*/ void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed) { + //define a timer Timer t; + //define and fill the vectors with info for the PID controler vector<double> pidInfo1 (4); vector<double> pidInfo2 (4); - fill(pidInfo1.begin(), pidInfo1.begin()+4, 0); fill(pidInfo2.begin(), pidInfo2.begin()+4, 0); + //define and set variables to store the current x and y coordinates in double xC = *xStart; double yC = *yStart; - + + //define variables to store the desired rotations of the motors in double rot1; double rot2; + //define variables to store previously passed time, currently passed time and the delta t in double tiemeP; double tiemeC; double dt; + //start the timer t.start(); + //set the previously passed time tiemeP = t.read(); + + //move the robot with a certain speed for in a certain direction for 1/10 of a second while(t.read() < 0.1) { + //read the passes time tiemeC = t.read(); + //determine delta t dt = tiemeC - tiemeP; + //determine the desired loaction of the end effector given a certain initial position and a speed xC = xC+xSpeed*dt; yC = yC+ySpeed*dt; + //determine the desired rotaion of the motors rot1 = calcRot1(xC, yC); rot2 = calcRot2(xC, yC); + //give the motors a certain speed to move with to reach their desired rotation moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2); moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1); + //set the previously passed time tiemeP = tiemeC; + //give the motors some time to move wait(0.01); } - - *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3)); - *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3)); + //set the the current location of the motors as the desired location of the motors + *xStart = xC; + *yStart = yC; } +/* +takes as input: +*M = a pointer to the pin controling the direction of the rotation of the motor +*E = a pointer to the pin controling the speed of the motor +*Enc = a pointer to the encoder of the motor +initRot = the rotation of the motor after it was calibrated +dir = the direction in which the motor has to move in order to make the error smaller +rotDes = the desired rotation of the motor +*/ void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes) { - double Kp = 2; //180 & 10 werkt zonder hulp + //define and set variable to store K values + double Kp = 2; double Ki = 0; double Kd = 0.1; + //define (and set) variable to store errors double pErrorC; double pErrorP = 0; double iError = 0; double dError; - double U_p; - double U_i; - double U_d; - + //define and set a variable to store the the current ration of the motor double rotC = Enc->getPulses()/(32*131.25) + initRot; + + //define a variable to store the dury cycle of the E pin double MotorPWM; + //define a timer Timer t; + + //define and set a variable to store the time that has passed double tieme = 0; + //start the timer t.start(); do { + //set the previous error pErrorP = pErrorC; + //read the current roation of the motor rotC = Enc->getPulses()/(32*131.25) + initRot; + //determine the current error pErrorC = rotDes - rotC; + //read the time that has passed and reset it tieme = t.read(); t.reset(); + //determine the integration of the error iError = iError + pErrorC*tieme; + //determine the derivitaive of the error dError = (pErrorC - pErrorP)/tieme; - - U_p = pErrorC*Kp; - U_i = iError*Ki; - U_d = dError*Kd; - MotorPWM = (U_p + U_i + U_d)*dir; - + + //determine the dutycycle of the E pin + MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir; + + //Move the motor in the right direction with the right speed if(MotorPWM > 0) { *M = 0; *E = MotorPWM; @@ -536,21 +674,25 @@ *M = 1; *E = -MotorPWM; } + //give the motor some time to move wait(0.01); - //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); + //keep doing this until the error is very small and the motor is hardly moving } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01); + //stop the movement of the arm *E = 0; - //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM); + //stop the timer t.stop(); } void flipx() { + //flip the x direction of the end effector in EMG mode xDir = -xDir; } void flipy() { + //flip the y direction of the end effector in EMG mode yDir = -yDir; } \ No newline at end of file