groep 16
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