Diff: main.cpp

Revision:

4:67506e285ad0

Parent:

3:d22942631cd7

Child:

6:e1fa1a2d7483

--- a/main.cpp	Thu Apr 01 14:31:52 2021 +0000
+++ b/main.cpp	Thu Apr 01 15:00:55 2021 +0000
@@ -50,11 +50,13 @@
     // initialise and test Servo
     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;
     
+    /* input your stuff here */
+    AnalogIn       analogIn(PC_2);
+    float          dist = 0.0f;
+        
     /*
     printf("Test writing... ");
     FILE* fp = fopen("/fs/data.csv", "w");
@@ -92,10 +94,10 @@
             
             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;
+            
+            dist = analogIn.read()*3.3f;
 
         } else {
             // LED on, reset speedController speed, pwm2, position servo
@@ -107,18 +109,19 @@
 
             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);
+            
+            dist = analogIn.read()*3.3f;
 
         }
         
         // 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());
+        printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d measured value in mV: %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f), 
+                                                                                                                   static_cast<int>(speedController_1.getSpeed()*1000.0f), 
+                                                                                                                   encoderCounter_2.read(), 
+                                                                                                                   (static_cast<int>(dist * 1e3)));
         
         thread_sleep_for(500);
     }