Motor calibration
Dependencies: BioroboticsMotorControl mbed BioroboticsEMGFilter MODSERIAL
main.cpp@4:64d4fdf5437c, 2018-10-31 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Wed Oct 31 12:10:43 2018 +0000
- Revision:
- 4:64d4fdf5437c
- Parent:
- 3:5b8a12611a1e
- Child:
- 5:43aa136aecda
Fixed stuff, should work, not tested, :(
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:64d4fdf5437c | 7 | DigitalOut calibLED1(LED_BLUE); |
MAHCSnijders | 4:64d4fdf5437c | 8 | DigitalOut calibLED2(LED_RED); |
MAHCSnijders | 4:64d4fdf5437c | 9 | DigitalOut calibLED3(LED_GREEN); |
MAHCSnijders | 0:61f4586742be | 10 | |
MAHCSnijders | 1:9c75e4cca419 | 11 | const float pid_period = 0.001; // PID sample period in seconds. |
MAHCSnijders | 1:9c75e4cca419 | 12 | const double Kp = 10.0; // PID proportional |
MAHCSnijders | 1:9c75e4cca419 | 13 | const double Ki = 0.1; // PID integral |
MAHCSnijders | 1:9c75e4cca419 | 14 | const double Kd = 0.5; // PID differential |
MAHCSnijders | 4:64d4fdf5437c | 15 | const double motor_threshold = 0.0174533; // One degree |
MAHCSnijders | 0:61f4586742be | 16 | |
MAHCSnijders | 4:64d4fdf5437c | 17 | Ticker motor_calib1; // Ticker for motor1 calibration |
MAHCSnijders | 4:64d4fdf5437c | 18 | Ticker motor_calib2; // Ticker for motor2 calibration |
MAHCSnijders | 0:61f4586742be | 19 | |
MAHCSnijders | 2:9d23d93d097f | 20 | float motor_angle1 = 2; // Set motor angle to arbitrary value for first loop |
MAHCSnijders | 2:9d23d93d097f | 21 | float motor_angle2 = 2; |
MAHCSnijders | 2:9d23d93d097f | 22 | float time_passed_in_this_state1 = 0; // Time passed in the final state of motor 1 |
MAHCSnijders | 2:9d23d93d097f | 23 | float time_passed_in_this_state2 = 0; // Time passed in the final state of motor 2 |
MAHCSnijders | 2:9d23d93d097f | 24 | |
MAHCSnijders | 2:9d23d93d097f | 25 | void Motor1_Calibration() |
MAHCSnijders | 1:9c75e4cca419 | 26 | { |
MAHCSnijders | 4:64d4fdf5437c | 27 | float current_motor1 = motor1.get_current_angle(); |
MAHCSnijders | 4:64d4fdf5437c | 28 | 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 |
MAHCSnijders | 2:9d23d93d097f | 29 | { |
MAHCSnijders | 2:9d23d93d097f | 30 | time_passed_in_this_state1++; |
MAHCSnijders | 4:64d4fdf5437c | 31 | calibLED1 = 0; // LED turns blue |
MAHCSnijders | 4:64d4fdf5437c | 32 | wait(0.1f); |
MAHCSnijders | 4:64d4fdf5437c | 33 | calibLED1 = 1; |
MAHCSnijders | 2:9d23d93d097f | 34 | } |
MAHCSnijders | 2:9d23d93d097f | 35 | else |
MAHCSnijders | 2:9d23d93d097f | 36 | { |
MAHCSnijders | 2:9d23d93d097f | 37 | motor_angle1 = motor1.get_current_angle(); |
MAHCSnijders | 4:64d4fdf5437c | 38 | motor_angle1 = motor_angle1 - 5*motor_threshold; // Subtracting five degree angle from the current angle |
MAHCSnijders | 2:9d23d93d097f | 39 | motor1.set_target_angle(motor_angle1); |
MAHCSnijders | 4:64d4fdf5437c | 40 | time_passed_in_this_state1 = 0; // Set time passed in this state back to zero |
MAHCSnijders | 4:64d4fdf5437c | 41 | calibLED2 = 0; // LED turns red |
MAHCSnijders | 4:64d4fdf5437c | 42 | wait(0.1f); |
MAHCSnijders | 4:64d4fdf5437c | 43 | calibLED2 = 1; |
MAHCSnijders | 2:9d23d93d097f | 44 | } |
MAHCSnijders | 2:9d23d93d097f | 45 | |
MAHCSnijders | 2:9d23d93d097f | 46 | if (time_passed_in_this_state1 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib) |
MAHCSnijders | 1:9c75e4cca419 | 47 | { |
MAHCSnijders | 3:5b8a12611a1e | 48 | motor1.define_current_angle_as_x_radians(0.785398); // Defines beginstate motor 2 as -42 degrees |
MAHCSnijders | 4:64d4fdf5437c | 49 | motor_calib1.detach(); // Stop looping the function |
MAHCSnijders | 4:64d4fdf5437c | 50 | calibLED3 = 0; // LED becomes red (purple if both motors are calibrated) |
MAHCSnijders | 4:64d4fdf5437c | 51 | wait(0.1f); |
MAHCSnijders | 4:64d4fdf5437c | 52 | calibLED3 = 1; |
MAHCSnijders | 2:9d23d93d097f | 53 | } |
MAHCSnijders | 2:9d23d93d097f | 54 | } |
MAHCSnijders | 2:9d23d93d097f | 55 | |
MAHCSnijders | 2:9d23d93d097f | 56 | void Motor2_Calibration() |
MAHCSnijders | 2:9d23d93d097f | 57 | { |
MAHCSnijders | 4:64d4fdf5437c | 58 | float current_motor2 = motor2.get_current_angle(); |
MAHCSnijders | 4:64d4fdf5437c | 59 | 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 |
MAHCSnijders | 2:9d23d93d097f | 60 | { |
MAHCSnijders | 2:9d23d93d097f | 61 | time_passed_in_this_state2++; |
MAHCSnijders | 4:64d4fdf5437c | 62 | calibLED1 = 0; // LED becomes blue |
MAHCSnijders | 4:64d4fdf5437c | 63 | wait(0.1f); |
MAHCSnijders | 4:64d4fdf5437c | 64 | calibLED1 = 1; |
MAHCSnijders | 2:9d23d93d097f | 65 | } |
MAHCSnijders | 2:9d23d93d097f | 66 | else |
MAHCSnijders | 2:9d23d93d097f | 67 | { |
MAHCSnijders | 2:9d23d93d097f | 68 | motor_angle2 = motor2.get_current_angle(); |
MAHCSnijders | 4:64d4fdf5437c | 69 | motor_angle2 = motor_angle2 - 5*motor_threshold; // Subtracting five degree angle from the current angle |
MAHCSnijders | 2:9d23d93d097f | 70 | motor2.set_target_angle(motor_angle2); |
MAHCSnijders | 4:64d4fdf5437c | 71 | time_passed_in_this_state2 = 0; // Set time passed in this state back to zero |
MAHCSnijders | 4:64d4fdf5437c | 72 | calibLED2 = 0; |
MAHCSnijders | 4:64d4fdf5437c | 73 | wait(0.1f); |
MAHCSnijders | 4:64d4fdf5437c | 74 | calibLED2 = 1; |
MAHCSnijders | 1:9c75e4cca419 | 75 | } |
MAHCSnijders | 1:9c75e4cca419 | 76 | |
MAHCSnijders | 2:9d23d93d097f | 77 | if (time_passed_in_this_state2 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib) |
MAHCSnijders | 1:9c75e4cca419 | 78 | { |
MAHCSnijders | 3:5b8a12611a1e | 79 | motor2.define_current_angle_as_x_radians(-0.733038); // Defines beginstate motor 2 as -42 degrees |
MAHCSnijders | 4:64d4fdf5437c | 80 | motor_calib2.detach(); // Stop looping the function |
MAHCSnijders | 4:64d4fdf5437c | 81 | calibLED3 = 0; // LED becomes red (purple if both motors are calibrated) |
MAHCSnijders | 4:64d4fdf5437c | 82 | wait(0.1f); |
MAHCSnijders | 4:64d4fdf5437c | 83 | calibLED3 = 1; |
MAHCSnijders | 1:9c75e4cca419 | 84 | } |
MAHCSnijders | 0:61f4586742be | 85 | } |
MAHCSnijders | 0:61f4586742be | 86 | |
MAHCSnijders | 0:61f4586742be | 87 | int main() |
MAHCSnijders | 0:61f4586742be | 88 | { |
MAHCSnijders | 1:9c75e4cca419 | 89 | calibLED1 = 1; |
MAHCSnijders | 1:9c75e4cca419 | 90 | calibLED2 = 1; |
MAHCSnijders | 4:64d4fdf5437c | 91 | calibLED3 = 1; |
MAHCSnijders | 1:9c75e4cca419 | 92 | motor1.set_pid_k_values(Kp, Ki, Kd); // Attach PID-controller values |
MAHCSnijders | 0:61f4586742be | 93 | motor2.set_pid_k_values(Kp, Ki, Kd); |
MAHCSnijders | 2:9d23d93d097f | 94 | motor1.start(pid_period); // Attach PID sample time |
MAHCSnijders | 0:61f4586742be | 95 | motor2.start(pid_period); |
MAHCSnijders | 4:64d4fdf5437c | 96 | motor_calib1.attach(Motor1_Calibration,0.5); // Ticker for motor calibration fucntion |
MAHCSnijders | 4:64d4fdf5437c | 97 | motor_calib2.attach(Motor2_Calibration,0.5); |
MAHCSnijders | 0:61f4586742be | 98 | while (true) {} // Empty while loop to keep function from stopping |
MAHCSnijders | 0:61f4586742be | 99 | } |