Diff: main.cpp

Revision:

44:340cdc4b6e47

Parent:

43:5648b7083fe5

Child:

45:5e1dd4117ed2

--- a/main.cpp	Sat May 07 12:50:49 2022 +0200
+++ b/main.cpp	Fri May 13 14:56:01 2022 +0200
@@ -1,5 +1,6 @@
 #include <mbed.h>
 #include <math.h>
+#include <vector>
 
 #include "PM2_Libary.h"
 #include "Eigen/Dense.h"
@@ -43,10 +44,13 @@
 float k_gear = 100.0f / 78.125f;            // define additional ratio in case you are using a dc motor with a different gear box, e.g. 100:1
 float kp = 0.1f;                            // define custom kp, this is the default speed controller gain for gear box 78.125:1
 
-SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1); // default 78.125:1 gear box  with default contoller parameters
-SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear box  with default contoller parameters
+
+//SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1); // default 78.125:1 gear box  with default contoller parameters
+//SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear box  with default contoller parameters
 //SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 100:1 gear
 
+
+
 // sparkfun line follower array
 I2C i2c(PB_9, PB_8); // I2C (PinName sda, PinName scl)
 SensorBar sensor_bar(i2c, 0.1175f);
@@ -64,7 +68,14 @@
 float fcn_vel_cntrl_v2(float wheel_speed_max, float b, float robot_omega);
 
 int main()
-{    
+{   
+    //SpeedController* speedController_ptr[2];
+    //speedController_ptr[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
+    //speedController_ptr[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
+    std::vector<SpeedController*> speedController_ptr;
+    speedController_ptr.push_back( new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1) );
+    speedController_ptr.push_back( new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2) );
+
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);
@@ -120,8 +131,8 @@
             // read analog input
             ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
 
-            speedController_M1.setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
-            speedController_M2.setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
+            speedController_ptr[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
+            speedController_ptr[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
 
             /*
             uint8_t sensor_bar_raw_value = sensor_bar.getRaw();
@@ -154,8 +165,8 @@
 
             ir_distance_mV = 0.0f;
 
-            speedController_M1.setDesiredSpeedRPS(0.0f);
-            speedController_M2.setDesiredSpeedRPS(0.0f);
+            speedController_ptr[0]->setDesiredSpeedRPS(0.0f);
+            speedController_ptr[1]->setDesiredSpeedRPS(0.0f);
         }
 
         user_led = !user_led;