Combination of working libraries useful for the BioRobotics course.
Dependencies: mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM
Revision 3:695daa59763d, committed 2019-10-24
- Comitter:
- sanou8
- Date:
- Thu Oct 24 12:12:58 2019 +0000
- Parent:
- 2:3feeeb434275
- Commit message:
- DEMO apparaat
Changed in this revision
BiQuad4th_order.lib | 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 3feeeb434275 -r 695daa59763d BiQuad4th_order.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BiQuad4th_order.lib Thu Oct 24 12:12:58 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Mirjam/code/BiQuad4th_order/#ac4bef72d6af
diff -r 3feeeb434275 -r 695daa59763d main.cpp --- a/main.cpp Mon Oct 14 09:48:55 2019 +0000 +++ b/main.cpp Thu Oct 24 12:12:58 2019 +0000 @@ -1,33 +1,324 @@ #include "mbed.h" -//#include "HIDScope.h" +#include "MODSERIAL.h" #include "QEI.h" -#include "MODSERIAL.h" -//#include "BiQuad.h" -//#include "FastPWM.h" +//#include "FilterDesign.h" +#include "BiQuad.h" +#include "BiQuad4.h" +#include "HIDScope.h" + + +QEI Encoder1(D12,D13,NC,32); +QEI Encoder2(D10,D11,NC,32); + +DigitalOut M1(D4); +DigitalOut M2(D7); +DigitalIn button(SW3); + +PwmOut E1(D5); +PwmOut E2(D6); + + +// GLOBAL VALUES +float pi = 3.14159265359; +int counts1; // Counts of encoder 1 +int counts2; // Counts of encoder 2 +float theta1; // Angle of motor 1 (rad) +float theta2; // Angle of motor 2 (rad) +float vel_1; // Velocity of motor 1 +float vel_2; // Velocity of motor 2 +float theta1_prev = 0.0; // Previous angles set to zero +float theta2_prev = 0.0; +double dirx = 1.0; //determine the direction of the setpoint placement +double diry = 1.0; //determine the direction of the setpoint placement +volatile double U1; // Motor voltage for motor 1 +volatile double U2; // Motor voltage for motor 2 +float tijd = 0.005; -MODSERIAL pc(USBTX, USBRX); -InterruptIn button(SW3); +// Inverse kinematics +const double La = 200.0; // length of the first link +const double Lb = 200.0; // length of the second link +volatile double q1_diff; +volatile double q2_diff; +const double sq = 2.0; // to square numbers + + +// DEMO +double point1x = 0; // Coordinates of first prescribed point +double point1y = 0; +double point2x = 0; // Coordinates of second prescribed point +double point2y = 80; +double point3x = -50; // Coordinates of third prescribed point +double point3y = 80; +double point4x = -50; // Coordinates of fourth prescribed point +double point4y = 30; +double point5x = 0; // Coordinates of fifth prescribed point +double point5y = 30; +volatile int track = 1; // Start with the track from zero to first prescribed point -Ticker motor; -volatile int counts; -void readencoder() -{ -QEI Encoder(D12,D13,NC,32); -counts = Encoder.getPulses(); -} -void print() -{ - pc.printf("%i",counts); - } +const double stepsize1 = 0.15; +const double stepsize2 = 0.25; +const double setpoint_error = 0.3; +double setpointx = 0 ; +double setpointy = 0 ; + +void ReadEncoder1() ; +void ReadEncoder2() ; +double counts2angle1() ; +double counts2angle2() ; +void ChangeDirectionX(); +void ChangeDirectionY() ; +void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes); +double determinedemosetx(double setpointx, double setpointy) ; +double determinedemosety(double setpointx, double setpointy) ; +double calcRot1(double xDes, double yDes) ; +double calcRot2(double xDes, double yDes) ; + + + + +// ---------------------------------------------- +// ------- MAIN FUNCTION ------------------------ +// ---------------------------------------------- int main() { - button.mode(PullUp); - pc.baud(115200); - button.rise(print); - motor.attach(readencoder, 0.002); + while (true) { + moveMotorTo(&M1,&E1,&Encoder1,calcRot1(setpointx,setpointy)) ; + moveMotorTo(&M2,&E2,&Encoder2,calcRot2(setpointx,setpointy)) ; } -} \ No newline at end of file +} + + +// Encoders +void ReadEncoder1() // Read Encoder of motor 1. +{ + counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1 + theta1 = (float(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1 + vel_1 = (theta1 - theta1_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs + theta1_prev = theta1; // Define theta_prev +} + +void ReadEncoder2() // Read encoder of motor 2. +{ + counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2 + theta2 = (float(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2 + vel_2 = (theta2 - theta2_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs + theta2_prev = theta2; // Define theta_prev +} +double counts2angle1() +{ + counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1 + theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1 + return theta1; +} + +double counts2angle2() +{ + counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2 + theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2 + return theta2; +} + + + + +//function to change the moving direction of the setpoint +void ChangeDirectionX(){ + dirx = -1*dirx; + } + +void ChangeDirectionY(){ + diry = -1*diry; + } + +// DEMO set + +double determinedemosetx(double setpointx, double setpointy) +{ + + if (setpointx < point1x && track == 1){ + setpointx = setpointx + stepsize1; + } + + // From point 1 to 2 + if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 51)) { + track = 12; + } + + if (setpointx < point2x && track == 12){ + setpointx = point2x; + } + + // From point 2 to 3 + if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12){ + track = 23; + } + + if (setpointy > point3y && track == 23){ + setpointx = setpointx + stepsize1; + // Stay on the same y value from point 2 to 3 + } + + // From point 3 to 4 + if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)) { + setpointy = point4y; + track = 34; + } + + if (setpointx > point4x && track == 34){ + setpointx = setpointx - stepsize2; + } + + // From point 4 to 5 + if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){ + setpointx = point4x; + track = 45; + } + + + if (setpointy < point5y && track == 45){ + setpointx = point5x; + // From point 4 to 5, stay on the same x value + } + + // From point 5 to 1 + if ((fabs(setpointx - point5x) <= setpoint_error) && (fabs(setpointy - point5y) <= setpoint_error) && (track == 45)){ + setpointx = point5x; + track = 51; + } + + + if (setpointy < point1y && track == 51){ + setpointx = point1x; + // From point 4 to 5, stay on the same x value + } + return setpointx; +} + +double determinedemosety(double setpointx, double setpointy) +{ + // From reference to point 1 + if(setpointy < point1y && track == 1){ + setpointy = setpointy + (stepsize2); + } + + // From point 1 to 2 + if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 51)){ + setpointy = point2y; + // Stay on the same y from point 1 to 2. + track = 12; + } + if (setpointx < point2x && track == 12){ + setpointy = point2y; + } + + // From point 2 to 3 + if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){ + setpointx = point3x; + track = 23; + } + if ((setpointy > point3y) && (track == 23)){ + setpointy = setpointy + (-stepsize2); + track = 23; + } + + // From point 3 to 4 + if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)){ + setpointy = setpointy; + track = 34; + } + if (setpointx > point4x && track == 34){ + setpointy = setpointy; + } + + // From point 4 to 5 + if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){ + track = 45; + } + + if (setpointy > point5y && track == 45){ + setpointy = setpointy - (stepsize2); + // From point 4 to 5, stay on the same x value. + } + // From point 5 to 1 + if ((fabs(setpointx - point5x) <= setpoint_error) && (fabs(setpointy - point5y) <= setpoint_error) && (track == 45)){ + track = 51; + } + + if (setpointy < point1y && track == 51){ + setpointy = setpointy + (stepsize2); + // From point 4 to 5, stay on the same x value. + } + return setpointy; + +} + +//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 +//float rotDes = desired rotation +void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes) +{ + float pErrorC; + float pErrorP = 0; + float iError = 0; + float dError; + + float Kp = 5; + float Ki = 0.01; + float Kd = 0.7; + + float rotC = Enc->getPulses()/(32*131.25); + float rotP = 0; + float MotorPWM; + + Timer t; + float tieme = 0; + + t.start(); + do { + pErrorP = pErrorC; + pErrorC = rotDes - rotC; + iError = iError + pErrorC*tieme; + dError = (pErrorC - pErrorP)/tieme; + + MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd; + + if(MotorPWM > 0) { + *M = 0; + *E = MotorPWM; + } else { + *M = 1; + *E = -MotorPWM; + } + + rotP = rotC; + rotC = Enc->getPulses()/(32*131.25); + tieme = t.read(); + t.reset(); + } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01); + t.stop(); +} + +//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) +{ + return 6*((atan(yDes/xDes) - 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 that calculates the rotation of the other 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 calcRot2(double xDes, double yDes) +{ + return 6*((atan(yDes/xDes) + 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)); +}; \ No newline at end of file