Motor calibration
Dependencies: BioroboticsMotorControl mbed BioroboticsEMGFilter MODSERIAL
Diff: main.cpp
- Revision:
- 2:9d23d93d097f
- Parent:
- 1:9c75e4cca419
- Child:
- 3:5b8a12611a1e
diff -r 9c75e4cca419 -r 9d23d93d097f main.cpp --- a/main.cpp Wed Oct 31 09:30:19 2018 +0000 +++ b/main.cpp Wed Oct 31 10:16:56 2018 +0000 @@ -1,4 +1,6 @@ #include "mbed.h" +#include "MODSERIAL.h" +#include "motor.h" Motor motor1(D6, D7, D13, D12); // Defining motor pins (PWM, direction, encoder) Motor motor2(D5, D4, D10, D11); // Defining motor pins (PWM, direction, encoder) @@ -12,31 +14,51 @@ Ticker motor_calib; // Ticker for motor calibration -void Motor_Calibration() +float motor_angle1 = 2; // Set motor angle to arbitrary value for first loop +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 + +void Motor1_Calibration() { - if (motor_angle1 = motor1.get_current_angle) + if (0.9 * motor1.get_current_angle() <= motor_angle1 <= 1.1 * motor1.get_current_angle()) // If the motor angle is within a margin of the current motor anlge + { + time_passed_in_this_state1++; + } + else + { + motor_angle1 = motor1.get_current_angle(); + motor_angle1 = motor_angle1 - 0.0174533; // Subtracting one 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 + } + + if (time_passed_in_this_state1 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib) { motor1.set_current_angle_as_zero(); - calibLED1 = 0; - break + calibLED1 = 0; // LED becomes blue (purple if both motors are calibrated) + } +} + +void Motor2_Calibration() +{ + if (0.9 * motor2.get_current_angle() <= motor_angle2 <= 1.1 * motor2.get_current_angle()) // If the motor angle is within a margin of the current motor anlge + { + time_passed_in_this_state2++; + } + else + { + motor_angle2 = motor2.get_current_angle(); + motor_angle2 = motor_angle2 - 0.0174533; // Subtracting one 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 } - if (motor_angle2 = motor2.get_current_angle) + if (time_passed_in_this_state2 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib) { motor2.set_current_angle_as_zero(); - calibLED2 = 0; - break + calibLED2 = 0; // LED becomes red (purple if both motors are calibrated) } - - float motor_angle1 = motor1.get_current_angle; - motor_angle1--; - motor1.set_target_angle(motor_angle1); - - float motor_angle2 = motor2.get_current_angle; - motor_angle2--; - motor2.set_target_angle(motor_angle2); - - } int main() @@ -45,8 +67,8 @@ calibLED2 = 1; motor1.set_pid_k_values(Kp, Ki, Kd); // Attach PID-controller values motor2.set_pid_k_values(Kp, Ki, Kd); - motor1.start(pid_period) // Attach PID sample time + motor1.start(pid_period); // Attach PID sample time motor2.start(pid_period); - motor_calib.attach(Motor_Calibration,0.5); // Ticker for motor calibration fucntion + motor_calib.attach(Motor1_Calibration,0.5); // Ticker for motor calibration fucntion while (true) {} // Empty while loop to keep function from stopping } \ No newline at end of file