Diff: main.cpp

Revision:

3:d22942631cd7

Parent:

2:7c03fefb77ef

Child:

4:67506e285ad0

diff -r 7c03fefb77ef -r d22942631cd7 main.cpp
--- a/main.cpp	Thu Apr 01 11:18:01 2021 +0000
+++ b/main.cpp	Thu Apr 01 14:31:52 2021 +0000
@@ -4,8 +4,7 @@
 #include "FATFileSystem.h"
 #include "EncoderCounter.h"
 #include "Servo.h"
-#include "Controller.h"
-
+#include "SpeedController.h"
 
 int main()
 {
@@ -13,24 +12,25 @@
     DigitalIn user_button(USER_BUTTON);
     
     // initialise PWM
-    PwmOut pwm_motor1(PB_13);
-    PwmOut pwm_motor2(PA_9);
-    PwmOut pwm_motor3(PA_10);
+    PwmOut pwm_motor_0(PB_13);
+    PwmOut pwm_motor_1(PA_9);
+    PwmOut pwm_motor_2(PA_10);
     
     // crete Encoder read objects
-    EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
-    EncoderCounter counter2(PB_6, PB_7);
-    EncoderCounter counter3(PA_0, PA_1);
+    EncoderCounter encoderCounter_0(PA_6, PC_7);
+    EncoderCounter encoderCounter_1(PB_6, PB_7);
+    EncoderCounter encoderCounter_2(PA_0, PA_1);
     
     // create controller
-    Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);
-    
-    DigitalOut enable(PB_15);
+    SpeedController speedController_0(pwm_motor_0, encoderCounter_0);
+    SpeedController speedController_1(pwm_motor_1, encoderCounter_1);
+        
+    DigitalOut enable_dc_motors(PB_15);
     
     // create servo objects
-    Servo  S0(PB_2);
-    Servo  S1(PC_8);
-    Servo  S2(PC_6);
+    Servo  servo_0(PB_2);
+    Servo  servo_1(PC_8);
+    Servo  servo_2(PC_6);
     
     SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
     printf("BlockDevice created\r\n"); 
@@ -39,22 +39,21 @@
     // Initialise the digital pin LED1 as an output
     DigitalOut myled(LED1);
     
-    
-    /**/
     // initialise PWM
-    pwm_motor1.period(0.00005f);// 0.05ms 20KHz
-    pwm_motor1.write(0.5f);
-    pwm_motor2.period(0.00005f);// 0.05ms 20KHz
-    pwm_motor2.write(0.5f);
-    pwm_motor3.period(0.00005f);// 0.05ms 20KHz
-    pwm_motor3.write(0.5f);
+    pwm_motor_0.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor_0.write(0.5f);
+    pwm_motor_1.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor_1.write(0.5f);
+    pwm_motor_2.period(0.00005f);// 0.05ms 20KHz
+    pwm_motor_2.write(0.5f);
     
     // initialise and test Servo
-    S0.Enable(1000,20000); // 1 ms / 20 ms
-    S1.Enable(1000,20000);
-    S2.Enable(1000,20000);
-    
+    servo_0.Enable(1000, 20000); // 1 ms / 20 ms
+    servo_1.Enable(1000, 20000);
+    servo_2.Enable(1000, 20000);
     int servo_desval_0 = 0;
+    int servo_desval_1 = 0;
+    int servo_desval_2 = 0;
     
     /*
     printf("Test writing... ");
@@ -79,42 +78,47 @@
     */
     
     // enable driver DC motors
-    enable = 1;
+    enable_dc_motors = 1;
     
     while (true) {
         
         if(!user_button) {
-            // LED off, set controller speed, pwm2, position servo
+            // LED off, set speedController speed, pwm2, position servo
             myled = 0;
-            controller.setDesiredSpeedLeft(50.0f);
-            // controller.setDesiredSpeedRight(50.0f);
-            // pwm_motor1.write(0.7f);
-            pwm_motor3.write(0.7f);
+            
+            speedController_0.setDesiredSpeed(  60.0f);
+            speedController_1.setDesiredSpeed(-120.0f);
+            pwm_motor_2.write(0.7f);
             
-            
-            S0.SetPosition(servo_desval_0);
-            S1.SetPosition(1000);
-            S2.SetPosition(1000);
-            if(servo_desval_0 < 2000) servo_desval_0 += 100;
+            servo_0.SetPosition(servo_desval_0);
+            servo_1.SetPosition(servo_desval_1);
+            servo_2.SetPosition(servo_desval_2);
+            if(servo_desval_0 < 10000) servo_desval_0 += 100;
+            if(servo_desval_1 < 10000) servo_desval_1 += 100;
+            if(servo_desval_2 < 10000) servo_desval_2 += 100;
 
         } else {
-            // LED on, reset controller speed, pwm2, position servo
+            // LED on, reset speedController speed, pwm2, position servo
             myled = 1;
-            controller.setDesiredSpeedLeft(0.0f);
-            // controller.setDesiredSpeedRight(0.0f);
-            // pwm_motor1.write(0.5f);
-            pwm_motor3.write(0.5f);
+            
+            speedController_0.setDesiredSpeed(0.0f);
+            speedController_1.setDesiredSpeed(0.0f);
+            pwm_motor_2.write(0.5f);
 
-            servo_desval_0 = 0;
-            S0.SetPosition(servo_desval_0);
-            S1.SetPosition(1500);
-            S2.SetPosition(1500);
+            servo_desval_0 = 0.0f;
+            servo_desval_1 = 0.0f;
+            servo_desval_2 = 0.0f;
+            servo_0.SetPosition(servo_desval_0);
+            servo_1.SetPosition(servo_desval_1);
+            servo_2.SetPosition(servo_desval_2);
 
         }
-
         
-        //printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
-        printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
+        // printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
+        // printf("counter1 = %d counter2 = %d counter3 = %d\r\n", encoderCounter_0.read(), encoderCounter_1.read(), encoderCounter_2.read());
+        printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f), 
+                                                                                          static_cast<int>(speedController_1.getSpeed()*1000.0f), 
+                                                                                          encoderCounter_2.read());
         
         thread_sleep_for(500);
     }