Motor calibration
Dependencies: BioroboticsMotorControl mbed BioroboticsEMGFilter MODSERIAL
Diff: main.cpp
- Revision:
- 1:9c75e4cca419
- Parent:
- 0:61f4586742be
- Child:
- 2:9d23d93d097f
--- a/main.cpp Wed Oct 31 09:12:19 2018 +0000 +++ b/main.cpp Wed Oct 31 09:30:19 2018 +0000 @@ -2,15 +2,32 @@ 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 double Kp = 10.0; -const double Ki = 0.1; -const double Kd = 0.5; +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_cali; +Ticker motor_calib; // Ticker for motor calibration void Motor_Calibration() -{ +{ + if (motor_angle1 = motor1.get_current_angle) + { + motor1.set_current_angle_as_zero(); + calibLED1 = 0; + break + } + + if (motor_angle2 = motor2.get_current_angle) + { + motor2.set_current_angle_as_zero(); + calibLED2 = 0; + break + } + float motor_angle1 = motor1.get_current_angle; motor_angle1--; motor1.set_target_angle(motor_angle1); @@ -18,14 +35,18 @@ float motor_angle2 = motor2.get_current_angle; motor_angle2--; motor2.set_target_angle(motor_angle2); + + } int main() { - motor1.set_pid_k_values(Kp, Ki, Kd); // Attach PID-controller + 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 time + motor1.start(pid_period) // Attach PID sample time motor2.start(pid_period); - motor.attach(Motor_Calibration,0.5); // Ticker for motor calibration fucntion + motor_calib.attach(Motor_Calibration,0.5); // Ticker for motor calibration fucntion while (true) {} // Empty while loop to keep function from stopping } \ No newline at end of file