Knopjes kunnen gebruikt worden om de robot the bedienen
Dependencies: FastPWM MODSERIAL QEI mbed
main.cpp
- Committer:
- Duif
- Date:
- 2018-11-01
- Revision:
- 0:f81ca7708f95
- Child:
- 1:f628ab39b9f4
File content as of revision 0:f81ca7708f95:
#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); DigitalOut ledr(LED_RED); DigitalOut ledg(LED_GREEN); DigitalOut ledb(LED_BLUE); DigitalIn directionx(SW3); //x direction switch button DigitalIn directiony(SW2); //y direction switch button DigitalIn buttonx(D2); //x EMG replacement DigitalIn buttony(D3); //y EMG replacement // 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; //volatile double error1_final=10.0; //volatile double error2_final=10.0; //volatile double q1_diff_final; //volatile double q2_diff_final; //double point1x = 200.0; //double point1y = 200.0; //double point2x = 200.0; //double point2y = 100.0; //double point3x = 350.0; //double point3y = 0.0; //double point4x = 200.0; //double point4y = 0.0; //volatile int track = 1; const double x0 = 80.0; //zero x position after homing const double y0 = 141.0; //zero y position after homing volatile double setpointx = x0; volatile double setpointy = y0; volatile double U1; volatile double U2; volatile double xfinal; volatile double yfinal; volatile int sx;//value of the button and store as switch volatile int sy;//value of the button and store as switch double dirx = 1.0; //determine the direction of the setpoint placement double diry = 1.0; //determine the direction of the setpoint placement const double v=0.1; //moving speed of setpoint // Inverse Kinematics volatile double q1_diff; volatile double q2_diff; const 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 = -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 = (pi-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 determinedemoset(){ setpointx = setpointx + dirx*sx*v; setpointy = setpointy + diry*sy*v; } //function to change the moving direction of the setpoint void ChangeDirectionX(){ dirx = -1*dirx; } void ChangeDirectionY(){ diry = -1*diry; } // ----------------------------------------------------------------- // --------------------------- 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() { determinedemoset(); q1_diff = makeAngleq1(setpointx, setpointy); q2_diff = makeAngleq2(setpointx, setpointy); //q1_diff_final = makeAngleq1(xfinal, yfinal); //q2_diff_final = makeAngleq2(xfinal, yfinal); theta2 = counts2angle2(); error2 = q2_diff - theta2; theta1 = counts2angle1(); error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as. //errors die de setpoints bepalen //error1_final = q1_diff_final - theta1; //error2_final = q2_diff_final - theta2; 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); InterruptIn directionx(SW3); directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction InterruptIn directiony(SW2); directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n"); while (true) { sx = !buttonx.read(); //this has to be replaced with the input from the EMG, this then functions like a button sy = !buttony.read(); //this has to be replaced with the input from the EMG, this then functions like a button pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2); //if (track == 1) { //pc.printf("Gaat naar positie 1\r\n"); //} //else if (track == 12) { //pc.printf("Gaat naar positie 2\r\n"); //} wait(0.5f); } }