De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 68:2879967ebb25
- Parent:
- 63:d17f45d88c7a
- Child:
- 69:bfefdfb04c29
diff -r d17f45d88c7a -r 2879967ebb25 main.cpp --- a/main.cpp Fri Nov 01 00:58:58 2019 +0000 +++ b/main.cpp Tue Nov 05 10:01:56 2019 +0000 @@ -40,7 +40,7 @@ AnalogIn potmeter1 (A4); AnalogIn potmeter2 (A5); -// Encoder inputs +// Motor Encoder inputs DigitalIn encA1(D9); DigitalIn encB1(D8); DigitalIn encA2(D13); @@ -114,8 +114,8 @@ bool led_color_changed = true; // Enable LED entry functions // Global constants -const double Fs = 500.0; -const double Ts = 1/Fs; +const double Fs = 500.0; //Sampling frequency +const double Ts = 1/Fs; /* ------------------------------ LED COLOR FUNCTIONS ------------------------------ @@ -197,8 +197,6 @@ /* ------------------------------ HELPER FUNCTIONS ------------------------------ */ -// Empty placeholder function, needs to be deleted at end of project -void doStuff() {} // Return max value of vector double getMax(const vector<double> &vect) @@ -590,15 +588,15 @@ QEI encoder2(D12, D13, NC, encoder_res, QEI::X4_ENCODING); //Encoder of motor 2 // Initialize variables for encoder reading -volatile int counts1; -volatile int counts1af; -int counts1offset; +volatile int counts1; //Counts after compensation with offset +volatile int counts1af; //Counts measured by encoder +int counts1offset; //Offset due to calibration volatile int countsPrev1 = 0; volatile int deltaCounts1; -volatile int counts2; -volatile int counts2af; -int counts2offset; +volatile int counts2; // Counts after compensation with offset +volatile int counts2af; //Counts measured by encoder +int counts2offset; //Offset due to calibration volatile int countsPrev2 = 0; volatile int deltaCounts2; @@ -619,59 +617,61 @@ const float l1 = 26.0; // Distance base-joint2 [cm] const float l2 = 62.0; // Distance join2-endpoint [cm] -float q1 = -10.0f * deg2rad; // Angle of first joint [rad] (starts off in reference position) +float q1 = -10.0f * deg2rad; // Angle of first joint [rad] (in calibration position) float q1dot; // Velocity of first joint [rad/s] -float q2 = -140.0f * deg2rad; -float q2dot; +float q2 = -140.0f * deg2rad; // Angle of second joint [rad] (in calibration position +float q2dot; // Velocity of second joint [rad/s] -float Vx = 0.0; // Desired linear velocity x direction -float Vy = 0.0; // Desired linear velocity y direction +float Vx = 0.0; // Desired linear velocity x direction [cm/s] +float Vy = 0.0; // Desired linear velocity y direction [cm/s] float xe; // Endpoint x position [cm] float ye; // Endpoint y position [cm] // Motor angles in starting position const float motor1_init = ( q1 + q2 ) * gearratio1; // Measured angle motor 1 in initial (starting) position -float motor1_ref = motor1_init; // Expected motor angle +float motor1_ref = motor1_init; // Expected motor angle (starting from initial position) float motor1_angle = motor1_init; // Actual motor angle const float motor2_init = q1 * gearratio2; // Measured angle motor 2 in initial (starting) position -float motor2_ref = motor2_init; -float motor2_angle = motor2_init; +float motor2_ref = motor2_init; // Motor 2 reference position (starting from initial position) +float motor2_angle = motor2_init; // Actual motor angle // Initialize variables for motor control float motor1offset; // Offset during calibration -float omega1; //velocity (rad/s) +float omega1; //angular velocity of motor [rad/s] bool motordir1; // Toggle of motor direction -double controlsignal1; // ?? -float motor1_error; // Error between encoder and reference +double controlsignal1; // Result of the PID controller +float motor1_error; // Error between desired and actual angle of the motor -float motor2offset; -float omega2; -bool motordir2; -double controlsignal2; -float motor2_error; +float motor2offset; // Offset during calibration +float omega2; //Angular velocity of motor [rad/s] +bool motordir2; // Toggle of motor direction +double controlsignal2; // Result of the PID controller +float motor2_error; // Error between desired and actual angle of the motor // Initialize variables for PID controller float Kp = 0.27; // Proportional gain float Ki = 0.35; // Integral gain -float Kd = 0.1; // Derivative gain -float Ka = 1.0; +float Kd = 0.1; // Derivative gain +float Ka = 1.0; //Windup gain -float u_p1; // -float u_i1; // -float ux1; // -float up1; // Proportional contribution -float ek1; // +float u_p1; // Proportional contribution +float u_i1; // Integral contribution +float u_d1; // Derivative contribution +float ux1; // Windup error (before multiplication with Ka) +float up1; // Summed contributions +float ek1; // Windup error float ei1 = 0.0; // Integral error (starts at 0) -float u_p2; -float u_i2; -float ux2; -float up2; -float ek2; -float ei2 = 0.0; +float u_p2; // Proportional contribution +float u_i2; // Integral contribution +float u_d2; // Derivative contribution +float ux2; // Windup error (before multiplication with Ka) +float up2; // Summed contributions +float ek2; // Windup error +float ei2 = 0.0; // Integral error (starts at 0) /* ------------------------------ MOTOR FUNCTIONS ------------------------------ @@ -680,14 +680,14 @@ { // Motor 1 static float error_integral1 = 0.0; - static float e_prev1 = motor1_error; + static float e_prev1 = motor1_error; // Saving the previous error, needed for derivative computation //Proportional part u_p1 = Kp * motor1_error; //Integral part - if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) { - error_integral1 = error_integral1 + ei1 * Ts; + if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) { // Only active during operational states to prevent windup + error_integral1 = error_integral1 + ei1 * Ts; u_i1 = Ki * error_integral1; } @@ -697,9 +697,9 @@ e_prev1 = motor1_error; // Sum and limit - up1 = u_p1 + u_i1 + u_d1; - if ( up1 > 1.0f ) { - controlsignal1 = 1.0f; + up1 = u_p1 + u_i1 + u_d1; // Sum the contributions of P, I and D + if ( up1 > 1.0f ) { // Saturated limit of motor, contintue with value between -1 and 1 + controlsignal1 = 1.0f; } else if ( up1 < -1.0f ) { controlsignal1 = -1.0f; } else { @@ -707,19 +707,19 @@ } // To prevent windup - ux1 = up1 - controlsignal1; + ux1 = up1 - controlsignal1; // Computation of the overflow ek1 = Ka * ux1; - ei1 = motor1_error - ek1; + ei1 = motor1_error - ek1; // Integral error with windup subtracted // Motor 2 static float error_integral2 = 0.0; - static float e_prev2 = motor2_error; + static float e_prev2 = motor2_error; // Saving the previous error, needed for derivative computation //Proportional part: u_p2 = Kp * motor2_error; //Integral part - if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) { + if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) { // Only active during operational states to prevent windup error_integral2 = error_integral2 + ei2 * Ts; u_i2 = Ki * error_integral2; } @@ -730,8 +730,8 @@ e_prev2 = motor2_error; // Sum and limit - up2 = u_p2 + u_i2 + u_d2; - if ( up2 > 1.0f ) { + up2 = u_p2 + u_i2 + u_d2; // Sum the contributions of P, I and D + if ( up2 > 1.0f ) { // Saturated limit of motor, contintue with value between -1 and 1 controlsignal2 = 1.0f; } else if ( up2 < -1.0f ) { controlsignal2 = -1.0f; @@ -740,78 +740,78 @@ } // To prevent windup - ux2 = up2 - controlsignal2; + ux2 = up2 - controlsignal2; // Computation of the overflow ek2 = Ka * ux2; - ei2 = motor2_error - ek2; + ei2 = motor2_error - ek2; // Integral error with windup subtracted } void RKI() { // Derived function for angular velocity of joint angles - q1dot = (l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); - q2dot = ((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); - q1 = q1 + q1dot * Ts; - q2 = q2 + q2dot * Ts; + q1dot = (l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); // Computation of the desired angular velocities with given linear velocities Vx and Vy + q2dot = ((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); // Computation of the desired angular velocities with given linear velocities Vx and Vy + q1 = q1 + q1dot * Ts; // Calculating desired joint angle by integrating the desired angular velocity + q2 = q2 + q2dot * Ts; // Calculating desired joint angle by integrating the desired angular velocity - xe = l1 * cos(q1) + l2 * cos(q1+q2); - ye = l1 * sin(q1) + l2 * sin(q1+q2); + xe = l1 * cos(q1) + l2 * cos(q1+q2); // Calculation of the current endpoint position + ye = l1 * sin(q1) + l2 * sin(q1+q2); // Calculation of the current endpoint position - if ( q1 < -5.0f * deg2rad) { + if ( q1 < -5.0f * deg2rad) { // Setting limits to the joint angles q1 = -5.0f * deg2rad; - } else if ( q1 > 65.0f * deg2rad ) { + } else if ( q1 > 65.0f * deg2rad ) { // Setting limits to the joint angles q1 = 65.0f * deg2rad; } else { q1 = q1; } - if ( q2 > -50.0f * deg2rad ) { + if ( q2 > -50.0f * deg2rad ) { // Setting limits to the joint angles q2 = -50.0f * deg2rad; - } else if ( q2 < -138.0f * deg2rad ) { + } else if ( q2 < -138.0f * deg2rad ) { // Setting limits to the joint angles q2 = -138.0f * deg2rad; } else { q2 = q2; } - motor1_ref = 5.5f * q1 + 5.5f * q2; - motor2_ref = 2.75f * q1; + motor1_ref = 5.5f * q1 + 5.5f * q2; // Calculating the desired motor angle to attain the correct joint angles + motor2_ref = 2.75f * q1; // Calculating the desired motor angle to attain the correct joint angles } void motorControl() { - counts1 = counts1af - counts1offset; - motor1_angle = (counts1 * encoder_factorin / gearbox_ratio) + motor1_init; // Angle of motor shaft in rad + correctie voor q1 en q2 - omega1 = deltaCounts1 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s - motor1_error = motor1_ref - motor1_angle; - if ( controlsignal1 < 0.0f ) { + counts1 = counts1af - counts1offset; // Counts measured by encoder compensated with the offset due to calibration + motor1_angle = (counts1 * encoder_factorin / gearbox_ratio) + motor1_init; // Angle of motor shaft [rad] + correction of initial value + omega1 = deltaCounts1 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft [rad/s] + motor1_error = motor1_ref - motor1_angle; // Difference between desired and measured angle of motor + if ( controlsignal1 < 0.0f ) { // Determine the direction motordir1 = 0; } else { motordir1 = 1; } - motor1Power.write(abs(controlsignal1)); - motor1Direction = motordir1; + motor1Power.write(abs(controlsignal1)); // Assigning the desired power to the motor + motor1Direction = motordir1; // Assigning the desired direction - counts2 = counts2af - counts2offset; - motor2_angle = (counts2 * encoder_factorin / gearbox_ratio) + motor2_init; // Angle of motor shaft in rad + correctie voor q1 - omega2 = deltaCounts2 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s - motor2_error = motor2_ref-motor2_angle; - if ( controlsignal2 < 0.0f ) { + counts2 = counts2af - counts2offset; // Counts measured by encoder compensated with the offset due to calibration + motor2_angle = (counts2 * encoder_factorin / gearbox_ratio) + motor2_init; // Angle of motor shaft [rad] + correction of initial value + omega2 = deltaCounts2 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft [rad/s] + motor2_error = motor2_ref-motor2_angle; // Difference between desired and measured angle of motor + if ( controlsignal2 < 0.0f ) { // Determine the direction motordir2 = 0; } else { motordir2 = 1; } if (motor_encoder_cal_done == true) { - motor2Power.write(abs(controlsignal2)); + motor2Power.write(abs(controlsignal2)); // Assinging the desired power to the motor } - motor2Direction = motordir2; + motor2Direction = motordir2; // Assigning the desired direction to the motor } void motorKillPower() { - motor1Power.write(0.0f); - motor2Power.write(0.0f); - Vx=0.0f; - Vy=0.0f; + motor1Power.write(0.0f); // Setting motor power to 0 to stop motion + motor2Power.write(0.0f); // Setting motor power to 0 to stop motion + Vx=0.0f; // Setting desired linear motion to 0 to prevent RKI from adjusting the motorpower + Vy=0.0f; // Setting desired linear motion to 0 to prevent RKI from adjusting the motorpower } /* @@ -824,10 +824,10 @@ motor_state_changed = false; // More functions } - motor1Power.write(0.0f); + motor1Power.write(0.0f); // Giving the motor no power to be able to adjust the robot configuration motor2Power.write(0.0f); - counts1offset = counts1af ; - counts2offset = counts2af; + counts1offset = counts1af ; // Storing the offset of the encoder + counts2offset = counts2af; // Storing the offset of the encoder // State transition guard if ( button2_pressed ) { @@ -849,7 +849,7 @@ // Do stuff until end condition is true PID_controller(); - motorControl(); + motorControl(); //These three functions cause the robot to move within the link limits RKI(); // State transition guard @@ -1018,8 +1018,8 @@ timerStateChange.start(); } - Vx = 5.0; - Vy = 5.0; + Vx = 5.0; // Move in the positive x direction + Vy = 5.0; // Move in the positive y direction PID_controller(); motorControl(); @@ -1027,8 +1027,6 @@ scope.set(0, motor2_ref * rad2deg ); scope.set(1, motor2_angle * rad2deg ); - //scope.set(2, motor2_ref ); - //scope.set(3, motor2_angle ); scope.send(); // State transition guard @@ -1049,7 +1047,7 @@ // Do stuff until end condition is met static float t = 0.0; - if ( t >= 0.0f && t < 3.0f ) { + if ( t >= 0.0f && t < 3.0f ) { // With this code the endpoint will make a square every 12 seconds Vx = 5.0; Vy = 0.0; } else if ( t >= 3.0f && t < 6.0f ) { @@ -1387,7 +1385,7 @@ { sampleSignals(); global_state_machine(); - changeservo(); + changeservo(); // Servo angle can be adjusted during every state } /* @@ -1439,10 +1437,6 @@ while(true) { pc.printf("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_curr_state); pc.printf("Potmeter 1: %f Potmeter 2: %f\r\n", potmeter1.read(), potmeter2.read()); - //pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir); - //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg); - //pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref*rad2deg/5.5f,motor1_angle*rad2deg/5.5f); - //pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref*rad2deg/2.75f,motor2_angle*rad2deg/2.75f); //pc.printf("Xe: %f Ye: %f\r\n",xe,ye); wait(0.5f); }