Biorobotics 7 / Mbed 2 deprecated Motor_calibration

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?

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 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 }