Motor calibration
Dependencies: BioroboticsMotorControl mbed BioroboticsEMGFilter MODSERIAL
Diff: main.cpp
- Revision:
- 5:43aa136aecda
- Parent:
- 4:64d4fdf5437c
- Child:
- 6:656fb0834a1a
--- a/main.cpp Wed Oct 31 12:10:43 2018 +0000 +++ b/main.cpp Wed Oct 31 12:44:47 2018 +0000 @@ -12,7 +12,7 @@ const double Kp = 10.0; // PID proportional const double Ki = 0.1; // PID integral const double Kd = 0.5; // PID differential -const double motor_threshold = 0.0174533; // One degree +const double motor_threshold = 0.5*0.0174533; // One degree Ticker motor_calib1; // Ticker for motor1 calibration Ticker motor_calib2; // Ticker for motor2 calibration @@ -21,69 +21,64 @@ float motor_angle2 = 2; float time_passed_in_this_state1 = 0; // Time passed in the final state of motor 1 float time_passed_in_this_state2 = 0; // Time passed in the final state of motor 2 +float last_angle1 = -2; // Last angle +float last_angle2 = -2; void Motor1_Calibration() { - float current_motor1 = motor1.get_current_angle(); - if (current_motor1 - motor_threshold <= motor_angle1 && motor_angle1 <= current_motor1 + motor_threshold) // If the motor angle is within a margin of the current motor angle + float current_angle1 = motor1.get_current_angle(); + if (current_angle1 - motor_threshold <= last_angle1 && last_angle1 <= current_angle1 + motor_threshold) // If the motor angle is within a margin of the current motor angle { time_passed_in_this_state1++; - calibLED1 = 0; // LED turns blue - wait(0.1f); - calibLED1 = 1; + calibLED1 = !calibLED1; // LED turns blue } else { - motor_angle1 = motor1.get_current_angle(); - motor_angle1 = motor_angle1 - 5*motor_threshold; // Subtracting five degree angle from the current angle + motor_angle1 = current_angle1; + motor_angle1 = motor_angle1 - 30*motor_threshold; // Subtracting five degree angle from the current angle motor1.set_target_angle(motor_angle1); time_passed_in_this_state1 = 0; // Set time passed in this state back to zero - calibLED2 = 0; // LED turns red - wait(0.1f); - calibLED2 = 1; + calibLED2 = !calibLED2; // LED turns red } if (time_passed_in_this_state1 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib) { motor1.define_current_angle_as_x_radians(0.785398); // Defines beginstate motor 2 as -42 degrees motor_calib1.detach(); // Stop looping the function - calibLED3 = 0; // LED becomes red (purple if both motors are calibrated) - wait(0.1f); - calibLED3 = 1; + calibLED3 = !calibLED3; // LED becomes red (purple if both motors are calibrated) } + + last_angle1 = current_angle1; } void Motor2_Calibration() { - float current_motor2 = motor2.get_current_angle(); - if (current_motor2 - motor_threshold <= motor_angle2 && motor_angle2 <= current_motor2 + motor_threshold) // If the motor angle is within a margin of the current motor angle + float current_angle2 = motor2.get_current_angle(); + if (current_angle2 - motor_threshold <= last_angle2 && last_angle2 <= current_angle2 + motor_threshold) // If the motor angle is within a margin of the current motor angle { time_passed_in_this_state2++; - calibLED1 = 0; // LED becomes blue - wait(0.1f); - calibLED1 = 1; + calibLED1 = !calibLED1; // LED turns blue } else { - motor_angle2 = motor2.get_current_angle(); - motor_angle2 = motor_angle2 - 5*motor_threshold; // Subtracting five degree angle from the current angle + motor_angle2 = current_angle2; + motor_angle2 = motor_angle2 - 30*motor_threshold; // Subtracting five degree angle from the current angle motor2.set_target_angle(motor_angle2); time_passed_in_this_state2 = 0; // Set time passed in this state back to zero - calibLED2 = 0; - wait(0.1f); - calibLED2 = 1; + calibLED2 = !calibLED2; // LED turns red } if (time_passed_in_this_state2 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib) { motor2.define_current_angle_as_x_radians(-0.733038); // Defines beginstate motor 2 as -42 degrees motor_calib2.detach(); // Stop looping the function - calibLED3 = 0; // LED becomes red (purple if both motors are calibrated) - wait(0.1f); - calibLED3 = 1; + calibLED3 = !calibLED3; // LED becomes red (purple if both motors are calibrated) } + + last_angle2 = current_angle2; } + int main() { calibLED1 = 1; @@ -93,7 +88,7 @@ motor2.set_pid_k_values(Kp, Ki, Kd); motor1.start(pid_period); // Attach PID sample time motor2.start(pid_period); - motor_calib1.attach(Motor1_Calibration,0.5); // Ticker for motor calibration fucntion - motor_calib2.attach(Motor2_Calibration,0.5); + motor_calib1.attach(Motor1_Calibration,0.2); // Ticker for motor calibration fucntion + motor_calib2.attach(Motor2_Calibration,0.2); while (true) {} // Empty while loop to keep function from stopping } \ No newline at end of file