Tried using switch
Dependencies: FastPWM MODSERIAL QEI mbed
main_demo.cpp
- Committer:
- Mirjam
- Date:
- 2018-10-31
- Revision:
- 2:785737b1cd38
- Parent:
- 1:8cec72aa7728
File content as of revision 2:785737b1cd38:
#include "mbed.h" #include "FastPWM.h" #include "MODSERIAL.h" #include "QEI.h" #include "math.h" // -------------------------------------------------- // ----------------- SET UP ------------------------- QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, counts/rev) QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, counts/rev) DigitalOut directionM1(D4); DigitalOut directionM2(D7); FastPWM motor1_pwm(D5); FastPWM motor2_pwm(D6); MODSERIAL pc(USBTX, USBRX); // Tickers Ticker Demo; Ticker Write; // --------------------------------------------------- // ----------------- GLOBAL VARIABLES ---------------- volatile int counts1; volatile int counts2; volatile double theta1; volatile double theta2; const double pi = 3.14159265359; volatile double error1; volatile double error2; double point1x = 200.0; double point1y = 200.0; double point2x = 350.0; double point2y = 200.0; double point3x = 350.0; double point3y = 0.0; double point4x = 200.0; double point4y = 0.0; const double x0 = 80.0; //zero x position after homing const double y0 = 141.0; //zero y position after homing double setpointx = x0; double setpointy = y0; volatile double U1; volatile double U2; // Inverse Kinematics volatile double q1_diff; volatile double q2_diff; double sq = 2.0; //to square numbers const double L1 = 250.0; //length of the first link const double L3 = 350.0; //length of the second link // Reference angles of the starting position double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq)))); double q2_0_enc = q2_0 + q1_0; // -------------------------------------------------------------------- // ---------------Read out encoders------------------------------------ // -------------------------------------------------------------------- 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; } // ------------------------------------------------------------------------- // -------------- Determine Setpoints -------------------------------------- // ------------------------------------------------------------------------- void determinedemosetpoints( double &setpointx, double &setpointy) { int track = 1; if( fabs(setpointx - point1x) <= 0.2 && fabs(setpointy - point1y) <= 0.2){ track = 2; pc.printf("We gaan nu naar setpoint %i",track); } if( fabs(setpointx - point2x) <= 0.2 && fabs(setpointy - point2y) <= 0.2){ track = 3; pc.printf("We gaan nu naar setpoint %i", track); } if( fabs(setpointx - point3x) <= 0.2 && fabs(setpointy - point3y) <= 0.2){ track = 4; pc.printf("We gaan nu naar setpoint %i",track); } switch(track){ case 1: setpointx = setpointx + (point1x - x0)/150; setpointy = setpointy + (point1y - y0)/150; break; case 2: setpointx = setpointx + 0.2; setpointy = setpointy; break; case 3: setpointx = setpointx; setpointy = setpointy - 0.2; break; case 4: setpointx = setpointx - 0.2; setpointy = setpointy; break; } } // ----------------------------------------------------------------- // --------------------------- PI controllers ---------------------- // ----------------------------------------------------------------- double PI_controller1(double error1) { static double error_integral1 = 0; // Proportional part double Kp1 = 3.95; // Kp (proportionele controller, nu nog een random waarde) double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp) // Integral part double Ki1 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde) double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine) error_integral1 = error_integral1 + error1 * Ts1; double u_i1 = Ki1 * error_integral1; // Sum U1 = u_p1 + u_i1; // Return return U1; } double PI_controller2(double error2) { static double error_integral2 = 0; // Proportional part double Kp2 = 3.95; // Kp (proportionele controller, nu nog een random waarde) double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp) // Integral part double Ki2 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde) double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine) error_integral2 = error_integral2 + error2 * Ts2; double u_i2 = Ki2 * error_integral2; // Sum + U2 = u_p2 + u_i2; // Return return U2; } // ------------------------------------------------------------ // ------------ Inverse Kinematics ---------------------------- // ------------------------------------------------------------ double makeAngleq1(double x, double y){ double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration q1_diff = -2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint return q1_diff; } double makeAngleq2(double x, double y){ double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint return -q2_diff; } // ----------------------------------------------- // ------------ RUN MOTORS ----------------------- // ----------------------------------------------- void motoraansturing() { determinedemosetpoints(setpointx, setpointy); q1_diff = makeAngleq1(setpointx, setpointy); q2_diff = makeAngleq2(setpointx, setpointy); theta2 = counts2angle2(); error2 = q2_diff - theta2; theta1 = counts2angle1(); error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as. U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt. U2 = PI_controller2(error2); motor1_pwm.write(fabs(U1)); // Motor aansturen directionM1 = U1 > 0.0f; // Richting van de motor bepalen motor2_pwm.write(fabs(U2)); directionM2 = U2 > 0.0f; } void rundemo() { motoraansturing(); } int main() { pc.baud(115200); motor1_pwm.period_us(60); // Period is 60 microseconde motor2_pwm.period_us(60); Demo.attach(&rundemo, 0.005f); while (true) { } }