Diff: main.cpp

Revision:

11:633e4fbb7d7c

Parent:

10:c5d85e35758c

--- a/main.cpp	Wed Apr 07 12:13:50 2021 +0000
+++ b/main.cpp	Mon Apr 12 08:46:44 2021 +0000
@@ -41,14 +41,17 @@
 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_M3(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M3);
 
 /* 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);
+
 int servoPeriod_mus = 20000;
 int servoOutput_mus_S1 = 0;
 int servoOutput_mus_S2 = 0;
+int servoOutput_mus_S3 = 0;
 int servo_counter = 0;
 int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));
 
@@ -68,6 +71,8 @@
     /* enable servos, you can also disable them */
     servo_S1.Enable(servoOutput_mus_S1, servoPeriod_mus);
     servo_S2.Enable(servoOutput_mus_S2, servoPeriod_mus);
+    servo_S3.Enable(servoOutput_mus_S3, servoPeriod_mus);
+
 
     while (true) {
 
@@ -83,18 +88,24 @@
             /* command a speed to dc motors M1 and M2*/
             speedController_M1.setDesiredSpeedRPS( 1.0f);
             speedController_M2.setDesiredSpeedRPS(-0.5f);
+            speedController_M3.setDesiredSpeedRPS(-0.5f);
             /* write output voltage to motor M3 */
-            pwmOut_M3.write(0.75);
+            pwmOut_M3.write(-0.75);
 
             /* command servo position via output time, this needs to be calibrated */
             servo_S1.SetPosition(servoOutput_mus_S1);
             servo_S2.SetPosition(servoOutput_mus_S2);
+            servo_S3.SetPosition(servoOutput_mus_S3);
+
             if (servoOutput_mus_S1 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
                 servoOutput_mus_S1 += 100;
             }
             if (servoOutput_mus_S2 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
                 servoOutput_mus_S2 += 100;
             }
+            if (servoOutput_mus_S3 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
+                servoOutput_mus_S3 += 100;
+            }
             servo_counter++;
 
             /* visual feedback that the main task is executed */
@@ -106,24 +117,34 @@
 
             speedController_M1.setDesiredSpeedRPS(0.0f);
             speedController_M2.setDesiredSpeedRPS(0.0f);
+            speedController_M3.setDesiredSpeedRPS(0.0f);
+
             pwmOut_M3.write(0.5);
 
+
             servoOutput_mus_S1 = 0;
             servoOutput_mus_S2 = 0;
+            servoOutput_mus_S3 = 0;
             servo_S1.SetPosition(servoOutput_mus_S1);
             servo_S2.SetPosition(servoOutput_mus_S2);
+            servo_S3.SetPosition(servoOutput_mus_S3);
+
 
             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",
+        printf("%3.3f, %3d, %3d, %3d, %3.3f, %3.3f, %3.3f;\r\n",
                dist,
                servoOutput_mus_S1,
                servoOutput_mus_S2,
+               servoOutput_mus_S3,
                encoderCounter_M3.read(),
                speedController_M1.getSpeedRPS(),
-               speedController_M2.getSpeedRPS());
+               speedController_M2.getSpeedRPS(),
+               speedController_M3.getSpeedRPS());
+               
+
 
         /* ------------- stop hacking ------------- -------------*/