Biorobotics 7 / Mbed 2 deprecated Motor_calibration

Motor calibration

Dependencies:   BioroboticsMotorControl mbed BioroboticsEMGFilter MODSERIAL

main.cpp@5:43aa136aecda, 2018-10-31 (annotated)

Committer:
MAHCSnijders
Date:
Wed Oct 31 12:44:47 2018 +0000
Revision:
5:43aa136aecda
Parent:
4:64d4fdf5437c
Child:
6:656fb0834a1a
Foute versie waarin niets werkt maar wat wel zou moeten werken

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 5:43aa136aecda 15 const double motor_threshold = 0.5*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 5:43aa136aecda 24 float last_angle1 = -2; // Last angle
MAHCSnijders 5:43aa136aecda 25 float last_angle2 = -2;
MAHCSnijders 2:9d23d93d097f 26
MAHCSnijders 2:9d23d93d097f 27 void Motor1_Calibration()
MAHCSnijders 1:9c75e4cca419 28 {
MAHCSnijders 5:43aa136aecda 29 float current_angle1 = motor1.get_current_angle();
MAHCSnijders 5:43aa136aecda 30 if (current_angle1 - motor_threshold <= last_angle1 && last_angle1 <= current_angle1 + motor_threshold) // If the motor angle is within a margin of the current motor angle
MAHCSnijders 2:9d23d93d097f 31 {
MAHCSnijders 2:9d23d93d097f 32 time_passed_in_this_state1++;
MAHCSnijders 5:43aa136aecda 33 calibLED1 = !calibLED1; // LED turns blue
MAHCSnijders 2:9d23d93d097f 34 }
MAHCSnijders 2:9d23d93d097f 35 else
MAHCSnijders 2:9d23d93d097f 36 {
MAHCSnijders 5:43aa136aecda 37 motor_angle1 = current_angle1;
MAHCSnijders 5:43aa136aecda 38 motor_angle1 = motor_angle1 - 30*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 5:43aa136aecda 41 calibLED2 = !calibLED2; // LED turns red
MAHCSnijders 2:9d23d93d097f 42 }
MAHCSnijders 2:9d23d93d097f 43
MAHCSnijders 2:9d23d93d097f 44 if (time_passed_in_this_state1 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib)
MAHCSnijders 1:9c75e4cca419 45 {
MAHCSnijders 3:5b8a12611a1e 46 motor1.define_current_angle_as_x_radians(0.785398); // Defines beginstate motor 2 as -42 degrees
MAHCSnijders 4:64d4fdf5437c 47 motor_calib1.detach(); // Stop looping the function
MAHCSnijders 5:43aa136aecda 48 calibLED3 = !calibLED3; // LED becomes red (purple if both motors are calibrated)
MAHCSnijders 2:9d23d93d097f 49 }
MAHCSnijders 5:43aa136aecda 50
MAHCSnijders 5:43aa136aecda 51 last_angle1 = current_angle1;
MAHCSnijders 2:9d23d93d097f 52 }
MAHCSnijders 2:9d23d93d097f 53
MAHCSnijders 2:9d23d93d097f 54 void Motor2_Calibration()
MAHCSnijders 2:9d23d93d097f 55 {
MAHCSnijders 5:43aa136aecda 56 float current_angle2 = motor2.get_current_angle();
MAHCSnijders 5:43aa136aecda 57 if (current_angle2 - motor_threshold <= last_angle2 && last_angle2 <= current_angle2 + motor_threshold) // If the motor angle is within a margin of the current motor angle
MAHCSnijders 2:9d23d93d097f 58 {
MAHCSnijders 2:9d23d93d097f 59 time_passed_in_this_state2++;
MAHCSnijders 5:43aa136aecda 60 calibLED1 = !calibLED1; // LED turns blue
MAHCSnijders 2:9d23d93d097f 61 }
MAHCSnijders 2:9d23d93d097f 62 else
MAHCSnijders 2:9d23d93d097f 63 {
MAHCSnijders 5:43aa136aecda 64 motor_angle2 = current_angle2;
MAHCSnijders 5:43aa136aecda 65 motor_angle2 = motor_angle2 - 30*motor_threshold; // Subtracting five degree angle from the current angle
MAHCSnijders 2:9d23d93d097f 66 motor2.set_target_angle(motor_angle2);
MAHCSnijders 4:64d4fdf5437c 67 time_passed_in_this_state2 = 0; // Set time passed in this state back to zero
MAHCSnijders 5:43aa136aecda 68 calibLED2 = !calibLED2; // LED turns red
MAHCSnijders 1:9c75e4cca419 69 }
MAHCSnijders 1:9c75e4cca419 70
MAHCSnijders 2:9d23d93d097f 71 if (time_passed_in_this_state2 >= 10) // After 5 seconds have passed (10 times Ticker motor_calib)
MAHCSnijders 1:9c75e4cca419 72 {
MAHCSnijders 3:5b8a12611a1e 73 motor2.define_current_angle_as_x_radians(-0.733038); // Defines beginstate motor 2 as -42 degrees
MAHCSnijders 4:64d4fdf5437c 74 motor_calib2.detach(); // Stop looping the function
MAHCSnijders 5:43aa136aecda 75 calibLED3 = !calibLED3; // LED becomes red (purple if both motors are calibrated)
MAHCSnijders 1:9c75e4cca419 76 }
MAHCSnijders 5:43aa136aecda 77
MAHCSnijders 5:43aa136aecda 78 last_angle2 = current_angle2;
MAHCSnijders 0:61f4586742be 79 }
MAHCSnijders 0:61f4586742be 80
MAHCSnijders 5:43aa136aecda 81
MAHCSnijders 0:61f4586742be 82 int main()
MAHCSnijders 0:61f4586742be 83 {
MAHCSnijders 1:9c75e4cca419 84 calibLED1 = 1;
MAHCSnijders 1:9c75e4cca419 85 calibLED2 = 1;
MAHCSnijders 4:64d4fdf5437c 86 calibLED3 = 1;
MAHCSnijders 1:9c75e4cca419 87 motor1.set_pid_k_values(Kp, Ki, Kd); // Attach PID-controller values
MAHCSnijders 0:61f4586742be 88 motor2.set_pid_k_values(Kp, Ki, Kd);
MAHCSnijders 2:9d23d93d097f 89 motor1.start(pid_period); // Attach PID sample time
MAHCSnijders 0:61f4586742be 90 motor2.start(pid_period);
MAHCSnijders 5:43aa136aecda 91 motor_calib1.attach(Motor1_Calibration,0.2); // Ticker for motor calibration fucntion
MAHCSnijders 5:43aa136aecda 92 motor_calib2.attach(Motor2_Calibration,0.2);
MAHCSnijders 0:61f4586742be 93 while (true) {} // Empty while loop to keep function from stopping
MAHCSnijders 0:61f4586742be 94 }