Diff: main.cpp

Revision:

12:b87a1e165fb8

Parent:

11:5053ce50a993

Child:

13:35f55464048e

--- a/main.cpp	Mon Apr 12 08:47:13 2021 +0000
+++ b/main.cpp	Mon Apr 19 09:37:44 2021 +0000
@@ -6,7 +6,7 @@
 #include "Servo.h"
 #include "SpeedController.h"
 #include "FastPWM.h"
-
+#define AUFLOESUNG 0.25412f
 using namespace std::chrono;
 
 InterruptIn user_button(USER_BUTTON);
@@ -21,8 +21,10 @@
 void button_rise();
 
 /* create analog input object */
+AnalogIn adc_vbat(ADC_VBAT);
 AnalogIn analogIn(PC_2);
 float    dist = 0.0f;
+float    batt = 0.0f;
 
 /* create enable dc motor digital out object */
 DigitalOut enable_motors(PB_15);
@@ -42,16 +44,8 @@
 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);
 
-/* create servo objects */
-Servo servo_S1(PB_2);
-Servo servo_S2(PC_8);
-Servo servo_S3(PC_6); // not needed in this example
-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)));
+
+
 
 int main()
 {
@@ -66,13 +60,9 @@
     /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
     pwmOut_M3.write(0.5);
 
-    /* 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) {
-
+        batt = adc_vbat.read()* (3.3f/AUFLOESUNG);
         loop_timer.reset();
 
         /* ------------- start hacking ------------- -------------*/
@@ -84,25 +74,11 @@
 
             /* command a speed to dc motors M1 and M2*/
             speedController_M1.setDesiredSpeedRPS( 2.0f);
-            speedController_M2.setDesiredSpeedRPS(-0.5f);
+ //           speedController_M2.setDesiredSpeedRPS(-0.5f);
             /* write output voltage to motor M3 */
             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 */
             led = !led;
 
@@ -111,25 +87,18 @@
             dist = 0.0f;
 
             speedController_M1.setDesiredSpeedRPS(0.0f);
-            speedController_M2.setDesiredSpeedRPS(0.0f);
+ //           speedController_M2.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, %3d, %3.3f, %3.3f;\r\n",
+        printf("%3.3f, %3.3f\n, %3d, %3.3f, %3.3f;\r\n",
                dist,
-               servoOutput_mus_S1,
-               servoOutput_mus_S2,
-               servoOutput_mus_S3,
+               batt,
                encoderCounter_M3.read(),
                speedController_M1.getSpeedRPS(),
                speedController_M2.getSpeedRPS());