Diff: main.cpp

Revision:

1:9c75e4cca419

Parent:

0:61f4586742be

Child:

2:9d23d93d097f

diff -r 61f4586742be -r 9c75e4cca419 main.cpp
--- a/main.cpp	Wed Oct 31 09:12:19 2018 +0000
+++ b/main.cpp	Wed Oct 31 09:30:19 2018 +0000
@@ -2,15 +2,32 @@
 
 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
 
-const double Kp = 10.0;
-const double Ki = 0.1;
-const double Kd = 0.5;
+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
 
-Ticker motor_cali;
+Ticker motor_calib;                         // Ticker for motor calibration
 
 void Motor_Calibration()
-{
+{   
+    if (motor_angle1 = motor1.get_current_angle)
+    {
+        motor1.set_current_angle_as_zero();
+        calibLED1 = 0;
+        break
+    }
+    
+    if (motor_angle2 = motor2.get_current_angle)
+    {
+        motor2.set_current_angle_as_zero();
+        calibLED2 = 0;
+        break
+    }
+    
     float motor_angle1 = motor1.get_current_angle;
     motor_angle1--;
     motor1.set_target_angle(motor_angle1);   
@@ -18,14 +35,18 @@
     float motor_angle2 = motor2.get_current_angle;
     motor_angle2--;
     motor2.set_target_angle(motor_angle2);
+
+    
 }
 
 int main()
 {
-    motor1.set_pid_k_values(Kp, Ki, Kd);    // Attach PID-controller
+    calibLED1 = 1;
+    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 time
+    motor1.start(pid_period)                // Attach PID sample time
     motor2.start(pid_period);
-    motor.attach(Motor_Calibration,0.5);    // Ticker for motor calibration fucntion
+    motor_calib.attach(Motor_Calibration,0.5);    // Ticker for motor calibration fucntion
     while (true) {}                         // Empty while loop to keep function from stopping
 }
\ No newline at end of file