Revision 4:64d4fdf5437c, committed 2018-10-31

Comitter:

MAHCSnijders

Date:

Wed Oct 31 12:10:43 2018 +0000

Parent:

3:5b8a12611a1e

Child:

5:43aa136aecda

Commit message:

Fixed stuff, should work, not tested, :(

Changed in this revision

--- a/BioroboticsMotorControl.lib	Wed Oct 31 10:38:04 2018 +0000
+++ b/BioroboticsMotorControl.lib	Wed Oct 31 12:10:43 2018 +0000
@@ -1,1 +1,1 @@
-http://os.mbed.com/teams/Biorobotics-7/code/BioroboticsMotorControl/#d83c79716ea1
+http://os.mbed.com/teams/Biorobotics-7/code/BioroboticsMotorControl/#34bd7f42c9db

--- a/main.cpp	Wed Oct 31 10:38:04 2018 +0000
+++ b/main.cpp	Wed Oct 31 12:10:43 2018 +0000
@@ -4,15 +4,18 @@
 
 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
+DigitalOut calibLED1(LED_BLUE);
+DigitalOut calibLED2(LED_RED); 
+DigitalOut calibLED3(LED_GREEN);
 
 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
+const double motor_threshold = 0.0174533;   // One degree
 
-Ticker motor_calib;                         // Ticker for motor calibration
+Ticker motor_calib1;                        // Ticker for motor1 calibration
+Ticker motor_calib2;                        // Ticker for motor2 calibration
 
 float motor_angle1 = 2;                     // Set motor angle to arbitrary value for first loop
 float motor_angle2 = 2;
@@ -21,43 +24,63 @@
 
 void Motor1_Calibration()
 {   
-    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
+    float current_motor1 = motor1.get_current_angle();
+    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
     {
         time_passed_in_this_state1++;
+        calibLED1 = 0;                              // LED turns blue
+        wait(0.1f);
+        calibLED1 = 1;
     }
     else
     {
         motor_angle1 = motor1.get_current_angle();
-        motor_angle1 = motor_angle1 - 0.0174533;    // Subtracting one degree angle from the current angle
+        motor_angle1 = motor_angle1 - 5*motor_threshold;    // Subtracting five degree angle from the current angle
         motor1.set_target_angle(motor_angle1); 
-        time_passed_in_this_state1 == 0;            // Set time passed in this state back to zero
+        time_passed_in_this_state1 = 0;            // Set time passed in this state back to zero
+        calibLED2 = 0;                              // LED turns red
+        wait(0.1f);
+        calibLED2 = 1;
     }
     
     if (time_passed_in_this_state1 >= 10)           // After 5 seconds have passed (10 times Ticker motor_calib)
     {
         motor1.define_current_angle_as_x_radians(0.785398); // Defines beginstate motor 2 as -42 degrees
-        calibLED1 = 0;                              // LED becomes blue (purple if both motors are calibrated)
+        motor_calib1.detach();                      // Stop looping the function
+        calibLED3 = 0;                              // LED becomes red (purple if both motors are calibrated)
+        wait(0.1f);
+        calibLED3 = 1;
     }
 }
 
 void Motor2_Calibration()
 {   
-    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
+    float current_motor2 = motor2.get_current_angle();
+    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
     {
         time_passed_in_this_state2++;
+        calibLED1 = 0;                              // LED becomes blue
+        wait(0.1f);
+        calibLED1 = 1;
     }
     else
     {
         motor_angle2 = motor2.get_current_angle();
-        motor_angle2 = motor_angle2 - 0.0174533;    // Subtracting one degree angle from the current angle
+        motor_angle2 = motor_angle2 - 5*motor_threshold;    // Subtracting five degree angle from the current angle
         motor2.set_target_angle(motor_angle2); 
-        time_passed_in_this_state2 == 0;            // Set time passed in this state back to zero
+        time_passed_in_this_state2 = 0;            // Set time passed in this state back to zero
+        calibLED2 = 0;
+        wait(0.1f);
+        calibLED2 = 1;
     }
     
     if (time_passed_in_this_state2 >= 10)           // After 5 seconds have passed (10 times Ticker motor_calib)
     {
         motor2.define_current_angle_as_x_radians(-0.733038); // Defines beginstate motor 2 as -42 degrees
-        calibLED2 = 0;                              // LED becomes red (purple if both motors are calibrated)
+        motor_calib2.detach();                      // Stop looping the function
+        calibLED3 = 0;                              // LED becomes red (purple if both motors are calibrated)
+        wait(0.1f);
+        calibLED3 = 1;
     }
 }
 
@@ -65,10 +88,12 @@
 {
     calibLED1 = 1;
     calibLED2 = 1;
+    calibLED3 = 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 sample time
     motor2.start(pid_period);
-    motor_calib.attach(Motor1_Calibration,0.5);    // Ticker for motor calibration fucntion
+    motor_calib1.attach(Motor1_Calibration,0.5);    // Ticker for motor calibration fucntion
+    motor_calib2.attach(Motor2_Calibration,0.5);
     while (true) {}                         // Empty while loop to keep function from stopping
 }
\ No newline at end of file