Diff: main.cpp

Revision:

2:7c03fefb77ef

Parent:

1:93d997d6b232

Child:

3:d22942631cd7

--- a/main.cpp	Tue Mar 30 14:20:41 2021 +0000
+++ b/main.cpp	Thu Apr 01 11:18:01 2021 +0000
@@ -1,8 +1,3 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2019 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
- */
-
 #include "mbed.h"
 #include "platform/mbed_thread.h"
 #include "SDBlockDevice.h"
@@ -45,20 +40,23 @@
     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_motor2.write(0.5f);
     pwm_motor3.period(0.00005f);// 0.05ms 20KHz
     pwm_motor3.write(0.5f);
     
     // initialise and test Servo
-    S0.Enable(1000,20000);
+    S0.Enable(1000,20000); // 1 ms / 20 ms
     S1.Enable(1000,20000);
     S2.Enable(1000,20000);
     
+    int servo_desval_0 = 0;
+    
+    /*
     printf("Test writing... ");
     FILE* fp = fopen("/fs/data.csv", "w");
     fprintf(fp, "test %.5f\r\n",1.23);
@@ -78,6 +76,7 @@
     } else {
         printf("Reading failed!\n");
     }
+    */
     
     // enable driver DC motors
     enable = 1;
@@ -88,31 +87,35 @@
             // LED off, set controller speed, pwm2, position servo
             myled = 0;
             controller.setDesiredSpeedLeft(50.0f);
-            controller.setDesiredSpeedRight(50.0f);
+            // controller.setDesiredSpeedRight(50.0f);
+            // pwm_motor1.write(0.7f);
             pwm_motor3.write(0.7f);
             
-
-            S0.SetPosition(1000);
+            
+            S0.SetPosition(servo_desval_0);
             S1.SetPosition(1000);
             S2.SetPosition(1000);
+            if(servo_desval_0 < 2000) servo_desval_0 += 100;
 
         } else {
             // LED on, reset controller speed, pwm2, position servo
             myled = 1;
             controller.setDesiredSpeedLeft(0.0f);
-            controller.setDesiredSpeedRight(0.0f);
+            // controller.setDesiredSpeedRight(0.0f);
+            // pwm_motor1.write(0.5f);
             pwm_motor3.write(0.5f);
 
-            S0.SetPosition(1500);
+            servo_desval_0 = 0;
+            S0.SetPosition(servo_desval_0);
             S1.SetPosition(1500);
             S2.SetPosition(1500);
 
         }
 
         
-        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",counter1.read(), counter2.read(), counter3.read());
         
-        thread_sleep_for(200);
+        thread_sleep_for(500);
     }
 }