Biorobotics 7 / Mbed 2 deprecated Motor_calibration

Motor calibration

Dependencies:   BioroboticsMotorControl mbed BioroboticsEMGFilter MODSERIAL

main.cpp@3:5b8a12611a1e, 2018-10-31 (annotated)

Committer:
MAHCSnijders
Date:
Wed Oct 31 10:38:04 2018 +0000
Revision:
3:5b8a12611a1e
Parent:
2:9d23d93d097f
Child:
4:64d4fdf5437c
With new definitions of home state (correct angles for IK and FK)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MAHCSnijders 0:61f4586742be 1 #include "mbed.h"
MAHCSnijders 2:9d23d93d097f 2 #include "MODSERIAL.h"
MAHCSnijders 2:9d23d93d097f 3 #include "motor.h"
MAHCSnijders 0:61f4586742be 4
MAHCSnijders 0:61f4586742be 5 Motor motor1(D6, D7, D13, D12); // Defining motor pins (PWM, direction, encoder)
MAHCSnijders 0:61f4586742be 6 Motor motor2(D5, D4, D10, D11); // Defining motor pins (PWM, direction, encoder)
MAHCSnijders 1:9c75e4cca419 7 DigitalOut calibLED1(LED_BLUE); // LED to check if calibration motor 1 is done
MAHCSnijders 1:9c75e4cca419 8 DigitalOut calibLED2(LED_RED); // LED to check if calibration motor 2 is done
MAHCSnijders 0:61f4586742be 9
MAHCSnijders 1:9c75e4cca419 10 const float pid_period = 0.001; // PID sample period in seconds.
MAHCSnijders 1:9c75e4cca419 11 const double Kp = 10.0; // PID proportional
MAHCSnijders 1:9c75e4cca419 12 const double Ki = 0.1; // PID integral
MAHCSnijders 1:9c75e4cca419 13 const double Kd = 0.5; // PID differential
MAHCSnijders 0:61f4586742be 14
MAHCSnijders 1:9c75e4cca419 15 Ticker motor_calib; // Ticker for motor calibration
MAHCSnijders 0:61f4586742be 16
MAHCSnijders 2:9d23d93d097f 17 float motor_angle1 = 2; // Set motor angle to arbitrary value for first loop
MAHCSnijders 2:9d23d93d097f 18 float motor_angle2 = 2;
MAHCSnijders 2:9d23d93d097f 19 float time_passed_in_this_state1 = 0; // Time passed in the final state of motor 1
MAHCSnijders 2:9d23d93d097f 20 float time_passed_in_this_state2 = 0; // Time passed in the final state of motor 2
MAHCSnijders 2:9d23d93d097f 21
MAHCSnijders 2:9d23d93d097f 22 void Motor1_Calibration()
MAHCSnijders 1:9c75e4cca419 23 {
MAHCSnijders 2:9d23d93d097f 24 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
MAHCSnijders 2:9d23d93d097f 25 {
MAHCSnijders 2:9d23d93d097f 26 time_passed_in_this_state1++;
MAHCSnijders 2:9d23d93d097f 27 }
MAHCSnijders 2:9d23d93d097f 28 else
MAHCSnijders 2:9d23d93d097f 29 {
MAHCSnijders 2:9d23d93d097f 30 motor_angle1 = motor1.get_current_angle();
MAHCSnijders 2:9d23d93d097f 31 motor_angle1 = motor_angle1 - 0.0174533; // Subtracting one degree angle from the current angle
MAHCSnijders 2:9d23d93d097f 32 motor1.set_target_angle(motor_angle1);
MAHCSnijders 2:9d23d93d097f 33 time_passed_in_this_state1 == 0; // Set time passed in this state back to zero
MAHCSnijders 2:9d23d93d097f 34 }
MAHCSnijders 2:9d23d93d097f 35
MAHCSnijders 2:9d23d93d097f 36 if (time_passed_in_this_state1 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib)
MAHCSnijders 1:9c75e4cca419 37 {
MAHCSnijders 3:5b8a12611a1e 38 motor1.define_current_angle_as_x_radians(0.785398); // Defines beginstate motor 2 as -42 degrees
MAHCSnijders 2:9d23d93d097f 39 calibLED1 = 0; // LED becomes blue (purple if both motors are calibrated)
MAHCSnijders 2:9d23d93d097f 40 }
MAHCSnijders 2:9d23d93d097f 41 }
MAHCSnijders 2:9d23d93d097f 42
MAHCSnijders 2:9d23d93d097f 43 void Motor2_Calibration()
MAHCSnijders 2:9d23d93d097f 44 {
MAHCSnijders 2:9d23d93d097f 45 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
MAHCSnijders 2:9d23d93d097f 46 {
MAHCSnijders 2:9d23d93d097f 47 time_passed_in_this_state2++;
MAHCSnijders 2:9d23d93d097f 48 }
MAHCSnijders 2:9d23d93d097f 49 else
MAHCSnijders 2:9d23d93d097f 50 {
MAHCSnijders 2:9d23d93d097f 51 motor_angle2 = motor2.get_current_angle();
MAHCSnijders 2:9d23d93d097f 52 motor_angle2 = motor_angle2 - 0.0174533; // Subtracting one degree angle from the current angle
MAHCSnijders 2:9d23d93d097f 53 motor2.set_target_angle(motor_angle2);
MAHCSnijders 2:9d23d93d097f 54 time_passed_in_this_state2 == 0; // Set time passed in this state back to zero
MAHCSnijders 1:9c75e4cca419 55 }
MAHCSnijders 1:9c75e4cca419 56
MAHCSnijders 2:9d23d93d097f 57 if (time_passed_in_this_state2 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib)
MAHCSnijders 1:9c75e4cca419 58 {
MAHCSnijders 3:5b8a12611a1e 59 motor2.define_current_angle_as_x_radians(-0.733038); // Defines beginstate motor 2 as -42 degrees
MAHCSnijders 2:9d23d93d097f 60 calibLED2 = 0; // LED becomes red (purple if both motors are calibrated)
MAHCSnijders 1:9c75e4cca419 61 }
MAHCSnijders 0:61f4586742be 62 }
MAHCSnijders 0:61f4586742be 63
MAHCSnijders 0:61f4586742be 64 int main()
MAHCSnijders 0:61f4586742be 65 {
MAHCSnijders 1:9c75e4cca419 66 calibLED1 = 1;
MAHCSnijders 1:9c75e4cca419 67 calibLED2 = 1;
MAHCSnijders 1:9c75e4cca419 68 motor1.set_pid_k_values(Kp, Ki, Kd); // Attach PID-controller values
MAHCSnijders 0:61f4586742be 69 motor2.set_pid_k_values(Kp, Ki, Kd);
MAHCSnijders 2:9d23d93d097f 70 motor1.start(pid_period); // Attach PID sample time
MAHCSnijders 0:61f4586742be 71 motor2.start(pid_period);
MAHCSnijders 2:9d23d93d097f 72 motor_calib.attach(Motor1_Calibration,0.5); // Ticker for motor calibration fucntion
MAHCSnijders 0:61f4586742be 73 while (true) {} // Empty while loop to keep function from stopping
MAHCSnijders 0:61f4586742be 74 }