Diff: main.cpp

Revision:

10:c5d85e35758c

Parent:

9:f10b974d01e0

Child:

11:af0f165f8761

--- a/main.cpp	Tue Apr 06 12:19:34 2021 +0000
+++ b/main.cpp	Wed Apr 07 12:13:50 2021 +0000
@@ -6,8 +6,6 @@
 #include "Servo.h"
 #include "SpeedController.h"
 #include "FastPWM.h"
-// #include "FATFileSystem.h"
-// #include "SDBlockDevice.h"
 
 using namespace std::chrono;
 
@@ -26,34 +24,33 @@
 AnalogIn analogIn(PC_2);
 float    dist = 0.0f;
 
-/* create pwm objects */
-FastPWM pwmOut_m0(PB_13);
-FastPWM pwmOut_m1(PA_9);
-FastPWM pwmOut_m2(PA_10);
-double  Ts_pwm_s = 0.00005; // this needs to be a double value
 /* create enable dc motor digital out object */
 DigitalOut enable_motors(PB_15);
+/* create pwm objects */
+FastPWM pwmOut_M1(PB_13);
+FastPWM pwmOut_M2(PA_9);
+FastPWM pwmOut_M3(PA_10);
+double  Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
 /* create encoder read objects */
-EncoderCounter  encoderCounter_m0(PA_6, PC_7);
-EncoderCounter  encoderCounter_m1(PB_6, PB_7);
-EncoderCounter  encoderCounter_m2(PA_0, PA_1);
-/* create speed controller objects, only m0 and m1, m2 is used open-loop */
-SpeedController speedController_m0(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m0, encoderCounter_m0);
-SpeedController speedController_m1(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m1, encoderCounter_m1);
+EncoderCounter  encoderCounter_M1(PA_6, PC_7);
+EncoderCounter  encoderCounter_M2(PB_6, PB_7);
+EncoderCounter  encoderCounter_M3(PA_0, PA_1);
+/* create speed controller objects, only M1 and M2, M3 is used open-loop */
+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);
 
 /* create servo objects */
-Servo servo_0(PB_2);
-Servo servo_1(PC_8);
-Servo servo_2(PC_6); // not used in this example
-int servoHolePeriod_mus = 20000;
-int servoPeriod_mus_0 = 0;
-int servoPeriod_mus_1 = 0;
-int counter = 0;
-int loops_per_second = (int)ceilf(1.0f/(0.001f*(float)Ts_ms));
-
-/* create sd object */
-// SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
-// FATFileSystem fs("fs", &sd);
+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 servo_counter = 0;
+int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));
 
 int main()
 {
@@ -61,39 +58,16 @@
     user_button.rise(&button_rise);
     loop_timer.start();
 
-    /* initialize pwm */
-    pwmOut_m2.period(Ts_pwm_s);
+    /* enable hardwaredriver dc motors */
+    enable_motors = 1;
+    /* initialize pwm for motor M3*/
+    pwmOut_M3.period(Ts_pwm_s);
     /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
-    pwmOut_m2.write(0.5);
-    /* enable driver DC motors */
-    enable_motors = 1;
-
-    /* initialize servo */
-    servo_0.Enable(servoPeriod_mus_0, servoHolePeriod_mus); // 1 ms / 20 ms
-    servo_1.Enable(servoPeriod_mus_0, servoHolePeriod_mus);
+    pwmOut_M3.write(0.5);
 
-    /*
-    // example code for sd card, not tested from pmic, 02.04.2021
-    printf("Test writing... ");
-    FILE* fp = fopen("/fs/data.csv", "w");
-    fprintf(fp, "test %.5f\r\n",1.23);
-    fclose(fp);
-    printf("done\r\n");
-
-    printf("Test reading... ");
-    // read from SD card
-    fp = fopen("/fs/data.csv", "r");
-    if (fp != NULL) {
-        char c = fgetc(fp);
-        if (c == 't')
-            printf("done\r\n");
-        else
-            printf("incorrect char (%c)!\n", c);
-        fclose(fp);
-    } else {
-        printf("Reading failed!\n");
-    }
-    */
+    /* enable servos, you can also disable them */
+    servo_S1.Enable(servoOutput_mus_S1, servoPeriod_mus);
+    servo_S2.Enable(servoOutput_mus_S2, servoPeriod_mus);
 
     while (true) {
 
@@ -106,19 +80,22 @@
             /* read analog input */
             dist = analogIn.read() * 3.3f;
 
-            speedController_m0.setDesiredSpeedRPS( 1.0f);
-            speedController_m1.setDesiredSpeedRPS(-0.5f);
-            pwmOut_m2.write(0.75);
+            /* command a speed to dc motors M1 and M2*/
+            speedController_M1.setDesiredSpeedRPS( 1.0f);
+            speedController_M2.setDesiredSpeedRPS(-0.5f);
+            /* write output voltage to motor M3 */
+            pwmOut_M3.write(0.75);
 
-            servo_0.SetPosition(servoPeriod_mus_0);
-            servo_1.SetPosition(servoPeriod_mus_1);
-            if (servoPeriod_mus_0 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
-                servoPeriod_mus_0 += 100;
+            /* command servo position via output time, this needs to be calibrated */
+            servo_S1.SetPosition(servoOutput_mus_S1);
+            servo_S2.SetPosition(servoOutput_mus_S2);
+            if (servoOutput_mus_S1 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
+                servoOutput_mus_S1 += 100;
             }
-            if (servoPeriod_mus_1 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
-                servoPeriod_mus_1 += 100;
+            if (servoOutput_mus_S2 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
+                servoOutput_mus_S2 += 100;
             }
-            counter++;
+            servo_counter++;
 
             /* visual feedback that the main task is executed */
             led = !led;
@@ -127,25 +104,26 @@
 
             dist = 0.0f;
 
-            speedController_m0.setDesiredSpeedRPS(0.0f);
-            speedController_m1.setDesiredSpeedRPS(0.0f);
-            pwmOut_m2.write(0.5);
+            speedController_M1.setDesiredSpeedRPS(0.0f);
+            speedController_M2.setDesiredSpeedRPS(0.0f);
+            pwmOut_M3.write(0.5);
 
-            servoPeriod_mus_0 = 0;
-            servoPeriod_mus_1 = 0;
-            servo_0.SetPosition(servoPeriod_mus_0);
-            servo_1.SetPosition(servoPeriod_mus_1);
-
-            dist = analogIn.read() * 3.3f;
+            servoOutput_mus_S1 = 0;
+            servoOutput_mus_S2 = 0;
+            servo_S1.SetPosition(servoOutput_mus_S1);
+            servo_S2.SetPosition(servoOutput_mus_S2);
 
             led = 0;
         }
 
-        /* do only output what's really necessary*/
-        printf("%3.3e, %3.3e, %3d, %3d; \r\n", speedController_m0.getSpeedRPS(),
-               speedController_m1.getSpeedRPS(),
-               servoPeriod_mus_0,
-               servoPeriod_mus_1);
+        /* 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,
+               servoOutput_mus_S1,
+               servoOutput_mus_S2,
+               encoderCounter_M3.read(),
+               speedController_M1.getSpeedRPS(),
+               speedController_M2.getSpeedRPS());
 
         /* ------------- stop hacking ------------- -------------*/