Diff: main.cpp

Revision:

11:af0f165f8761

Parent:

10:c5d85e35758c

Child:

12:3dfd8f2939ac

diff -r c5d85e35758c -r af0f165f8761 main.cpp
--- a/main.cpp	Wed Apr 07 12:13:50 2021 +0000
+++ b/main.cpp	Wed Apr 14 12:15:40 2021 +0000
@@ -6,6 +6,7 @@
 #include "Servo.h"
 #include "SpeedController.h"
 #include "FastPWM.h"
+#include "RangeFinder.h"
 
 using namespace std::chrono;
 
@@ -20,10 +21,14 @@
 void button_fall();
 void button_rise();
 
+// SHARP GP2Y0A21Y IR Sensor
+// https://www.digitec.ch/de/s1/product/sharp-distanz-sensor-1-st-gp2y0a21y-aktive-bauelemente-8425699?gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hMUr4sljdd8LfsdhBBlhxKY5gyDmZQ49ghgiIRZaKWdj85ISUw5r4oaAmM9EALw_wcB&gclsrc=aw.ds
 /* create analog input object */
 AnalogIn analogIn(PC_2);
-float    dist = 0.0f;
+float    dist_IRSensor = 0.0f;
 
+// 78:1 Metal Gearmotor 20Dx43L mm 12V CB
+// https://www.pololu.com/product/3477
 /* create enable dc motor digital out object */
 DigitalOut enable_motors(PB_15);
 /* create pwm objects */
@@ -42,16 +47,25 @@
 SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
 SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
 
+// Futaba Servo S3001 20mm 3kg Analog 
+// https://www.modellmarkt24.ch/pi/RC-Elektronik/Servos/Standard-Servo-20mm/futaba-servo-s3001-20mm-3kg-analog.html?gclid=CjwKCAjw3pWDBhB3EiwAV1c5rK_-x_Bt19_wIY-IcS2C-RULXKBtYfY0byxejkZLjASro-EMPBUhrxoCgaQQAvD_BwE
 /* create servo objects */
 Servo servo_S1(PB_2);
 Servo servo_S2(PC_8);
-// Servo servo_S3(PC_6); // not needed in this example
+// Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below
 int servoPeriod_mus = 20000;
 int servoOutput_mus_S1 = 0;
 int servoOutput_mus_S2 = 0;
 int servo_counter = 0;
 int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));
 
+// Groove Ultrasonic Ranger V2.0
+// https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds 
+/* create range finder object (ultra sonic distance sensor) */
+RangeFinder rangeFinder(PC_6, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
+// RangeFinder rangefinder(PB_6, 5782.0f, 0.02f,  7000); // 1/Ts_ms = 50 Hz parametrization
+float       dist_USSensor = 0.0f;
+
 int main()
 {
     user_button.fall(&button_fall);
@@ -78,7 +92,7 @@
         if (executeMainTask) {
 
             /* read analog input */
-            dist = analogIn.read() * 3.3f;
+            dist_IRSensor = analogIn.read() * 3.3f;
 
             /* command a speed to dc motors M1 and M2*/
             speedController_M1.setDesiredSpeedRPS( 1.0f);
@@ -97,12 +111,15 @@
             }
             servo_counter++;
 
+            /* read ultra sonic distance sensor */
+            dist_USSensor = rangeFinder.read_cm();
+
             /* visual feedback that the main task is executed */
             led = !led;
 
         } else {
 
-            dist = 0.0f;
+            dist_IRSensor = 0.0f;
 
             speedController_M1.setDesiredSpeedRPS(0.0f);
             speedController_M2.setDesiredSpeedRPS(0.0f);
@@ -112,18 +129,21 @@
             servoOutput_mus_S2 = 0;
             servo_S1.SetPosition(servoOutput_mus_S1);
             servo_S2.SetPosition(servoOutput_mus_S2);
+            
+            dist_USSensor = 0.0f;
 
             led = 0;
         }
 
         /* do only output via serial what's really necessary (this makes your code slow)*/
-        printf("%3.3f, %3d, %3d, %3d, %3.3f, %3.3f;\r\n",
-               dist,
+        printf("%3.3f, %3d, %3d, %3d, %3.3f, %3.3f, %3.3f;\r\n",
+               dist_IRSensor,
                servoOutput_mus_S1,
                servoOutput_mus_S2,
                encoderCounter_M3.read(),
                speedController_M1.getSpeedRPS(),
-               speedController_M2.getSpeedRPS());
+               speedController_M2.getSpeedRPS(),
+               dist_USSensor);
 
         /* ------------- stop hacking ------------- -------------*/