Diff: main.cpp

Revision:

53:155c309f7d75

Parent:

52:60a710ecc162

--- a/main.cpp	Mon May 16 11:03:12 2022 +0200
+++ b/main.cpp	Wed Jun 22 17:44:18 2022 +0200
@@ -4,7 +4,27 @@
 #include "PM2_Libary.h"
 #include "Eigen/Dense.h"
 
-# define M_PI 3.14159265358979323846  // number pi
+#define NEW_PES_BOARD_VERSION
+
+#ifdef NEW_PES_BOARD_VERSION
+    #define PN_enable_Motors PB_15
+    #define PN_pwm_M1 PB_13
+    #define PN_pwm_M2 PA_9
+    #define PN_encoder_M1_A PA_6
+    #define PN_encoder_M1_B PC_7
+    #define PN_encoder_M2_A PB_6
+    #define PN_encoder_M2_B PB_7
+#else
+    #define PN_enable_Motors PB_2
+    #define PN_pwm_M1 PA_8
+    #define PN_pwm_M2 PA_9
+    #define PN_encoder_M1_A PB_6
+    #define PN_encoder_M1_B PB_7
+    #define PN_encoder_M2_A PA_6
+    #define PN_encoder_M2_B PC_7
+#endif
+
+#define M_PI 3.14159265358979323846  // number pi
 
 // logical variable main task
 bool do_execute_main_task = false;  // this variable will be toggled via the user button (blue button) to or not to execute the main task
@@ -34,13 +54,13 @@
     AnalogIn ir_analog_in(PC_2);    // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
 
     // 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
-    DigitalOut enable_motors(PB_15);    // create DigitalOut object to enable dc motors
+    DigitalOut enable_motors(PN_enable_Motors);    // create DigitalOut object to enable dc motors
 
     // create SpeedController objects
-    FastPWM pwm_M1(PB_13);  // motor M1 is closed-loop speed controlled (angle velocity)
-    FastPWM pwm_M2(PA_9);   // motor M2 is closed-loop speed controlled (angle velocity)
-    EncoderCounter  encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
-    EncoderCounter  encoder_M2(PB_6, PB_7);
+    FastPWM pwm_M1(PN_pwm_M1);  // motor M1 is closed-loop speed controlled (angle velocity)
+    FastPWM pwm_M2(PN_pwm_M2);   // motor M2 is closed-loop speed controlled (angle velocity)
+    EncoderCounter  encoder_M1(PN_encoder_M1_A, PN_encoder_M1_B); // create encoder objects to read in the encoder counter values
+    EncoderCounter  encoder_M2(PN_encoder_M2_A, PN_encoder_M2_B);
     const float max_voltage = 12.0f;                  // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
     const float counts_per_turn = 20.0f * 78.125f;    // define counts per turn at gearbox end: counts/turn * gearratio
     const float kn = 180.0f / 12.0f;                  // define motor constant in rpm per V
@@ -135,8 +155,7 @@
         user_led = !user_led;
 
         // do only output via serial what's really necessary (this makes your code slow)
-        //printf("%d, %d\r\n", sensor_bar_raw_value_time_ms, sensor_bar_position_time_ms);
-        //printf("%f, %f\r\n", speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS());
+        printf("%f, %f, %f\r\n", speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS(), sensor_bar.getAvgAngleRad() * 180.0f / M_PI);
 
         // read timer and make the main thread sleep for the remaining time span (non blocking)
         int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();