Motor calibration
Dependencies: BioroboticsMotorControl mbed BioroboticsEMGFilter MODSERIAL
main.cpp
- Committer:
- MAHCSnijders
- Date:
- 2018-10-31
- Revision:
- 3:5b8a12611a1e
- Parent:
- 2:9d23d93d097f
- Child:
- 4:64d4fdf5437c
File content as of revision 3:5b8a12611a1e:
#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) DigitalOut calibLED1(LED_BLUE); // LED to check if calibration motor 1 is done DigitalOut calibLED2(LED_RED); // LED to check if calibration motor 2 is done const float pid_period = 0.001; // PID sample period in seconds. const double Kp = 10.0; // PID proportional const double Ki = 0.1; // PID integral const double Kd = 0.5; // PID differential Ticker motor_calib; // Ticker for 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 (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.define_current_angle_as_x_radians(0.785398); // Defines beginstate motor 2 as -42 degrees 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 (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 calibLED2 = 0; // LED becomes red (purple if both motors are calibrated) } } int main() { calibLED1 = 1; 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 motor2.start(pid_period); motor_calib.attach(Motor1_Calibration,0.5); // Ticker for motor calibration fucntion while (true) {} // Empty while loop to keep function from stopping }