Diff: main.cpp

Revision:

2:9d23d93d097f

Parent:

1:9c75e4cca419

Child:

3:5b8a12611a1e

--- a/main.cpp	Wed Oct 31 09:30:19 2018 +0000
+++ b/main.cpp	Wed Oct 31 10:16:56 2018 +0000
@@ -1,4 +1,6 @@
 #include "mbed.h"
+#include "MODSERIAL.h"
+#include "motor.h"
 
 Motor motor1(D6, D7, D13, D12);             // Defining motor pins (PWM, direction, encoder)
 Motor motor2(D5, D4, D10, D11);             // Defining motor pins (PWM, direction, encoder)
@@ -12,31 +14,51 @@
 
 Ticker motor_calib;                         // Ticker for motor calibration
 
-void Motor_Calibration()
+float motor_angle1 = 2;                     // Set motor angle to arbitrary value for first loop
+float motor_angle2 = 2;
+float time_passed_in_this_state1 = 0;       // Time passed in the final state of motor 1
+float time_passed_in_this_state2 = 0;       // Time passed in the final state of motor 2
+
+void Motor1_Calibration()
 {   
-    if (motor_angle1 = motor1.get_current_angle)
+    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
+    {
+        time_passed_in_this_state1++;
+    }
+    else
+    {
+        motor_angle1 = motor1.get_current_angle();
+        motor_angle1 = motor_angle1 - 0.0174533;    // Subtracting one 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
+    }
+    
+    if (time_passed_in_this_state1 >= 10)           // After 5 seconds have passed (10 times Ticker motor_calib)
     {
         motor1.set_current_angle_as_zero();
-        calibLED1 = 0;
-        break
+        calibLED1 = 0;                              // LED becomes blue (purple if both motors are calibrated)
+    }
+}
+
+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
+    {
+        time_passed_in_this_state2++;
+    }
+    else
+    {
+        motor_angle2 = motor2.get_current_angle();
+        motor_angle2 = motor_angle2 - 0.0174533;    // Subtracting one 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
     }
     
-    if (motor_angle2 = motor2.get_current_angle)
+    if (time_passed_in_this_state2 >= 10)           // After 5 seconds have passed (10 times Ticker motor_calib)
     {
         motor2.set_current_angle_as_zero();
-        calibLED2 = 0;
-        break
+        calibLED2 = 0;                              // LED becomes red (purple if both motors are calibrated)
     }
-    
-    float motor_angle1 = motor1.get_current_angle;
-    motor_angle1--;
-    motor1.set_target_angle(motor_angle1);   
-    
-    float motor_angle2 = motor2.get_current_angle;
-    motor_angle2--;
-    motor2.set_target_angle(motor_angle2);
-
-    
 }
 
 int main()
@@ -45,8 +67,8 @@
     calibLED2 = 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
+    motor1.start(pid_period);               // Attach PID sample time
     motor2.start(pid_period);
-    motor_calib.attach(Motor_Calibration,0.5);    // Ticker for motor calibration fucntion
+    motor_calib.attach(Motor1_Calibration,0.5);    // Ticker for motor calibration fucntion
     while (true) {}                         // Empty while loop to keep function from stopping
 }
\ No newline at end of file