Diff: main.cpp

Revision:

13:41bf923cd055

Parent:

12:3dfd8f2939ac

Child:

14:f1916a4caae0

--- a/main.cpp	Mon Apr 19 08:47:39 2021 +0000
+++ b/main.cpp	Mon Apr 19 15:54:20 2021 +0000
@@ -5,6 +5,7 @@
 #include "EncoderCounter.h"
 #include "Servo.h"
 #include "SpeedController.h"
+#include "PositionController.h"
 #include "FastPWM.h"
 #include "RangeFinder.h"
 
@@ -44,8 +45,9 @@
 float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
 float kn = 180.0f/12.0f;               // (RPM/V)
 float max_voltage = 12.0f;             // adjust this to 6.0f if only one batterypack is used
-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);
+SpeedController    speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
+PositionController positionController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
+float max_speed_rps = 1.0f;            // has to be smaller or equal to kn * max_voltage
 
 // 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
@@ -96,7 +98,7 @@
 
             /* command a speed to dc motors M1 and M2*/
             speedController_M1.setDesiredSpeedRPS( 1.0f);
-            speedController_M2.setDesiredSpeedRPS(-0.5f);
+            positionController_M2.setDesiredRotation(0.5f, max_speed_rps);
             /* write output voltage to motor M3 */
             pwmOut_M3.write(0.75);
 
@@ -122,7 +124,7 @@
             dist_IRSensor = 0.0f;
 
             speedController_M1.setDesiredSpeedRPS(0.0f);
-            speedController_M2.setDesiredSpeedRPS(0.0f);
+            positionController_M2.setDesiredRotation(0.0f, max_speed_rps);
             pwmOut_M3.write(0.5);
 
             servoOutput_mus_S1 = 0;
@@ -136,15 +138,15 @@
         }
 
         /* do only output via serial what's really necessary (this makes your code slow)*/
-        printf("%3.3f, %3d, %3d, %3d, %3.3f, %3.3f, %3.3f;\r\n",
+        printf("%3.3f, %3d, %3d, %3.3f, %3.3f, %3d, %3.3f;\r\n",
                dist_IRSensor,
                servoOutput_mus_S1,
                servoOutput_mus_S2,
+               speedController_M1.getSpeedRPS(),
+               positionController_M2.getRotation(),
                encoderCounter_M3.read(),
-               speedController_M1.getSpeedRPS(),
-               speedController_M2.getSpeedRPS(),
                dist_USSensor);
-
+               
         /* ------------- stop hacking ------------- -------------*/
 
         int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();