Diff: main.cpp

Revision:

48:3ae406d7554a

Parent:

47:5ce234723e3a

Child:

50:fec2ffc2a443

--- a/main.cpp	Sat May 14 09:56:31 2022 +0200
+++ b/main.cpp	Sat May 14 10:17:37 2022 +0200
@@ -1,12 +1,9 @@
 #include <mbed.h>
 #include <math.h>
-//#include <vector>
 
 #include "PM2_Libary.h"
 #include "Eigen/Dense.h"
 
-#include "Motion.h"
-
 # define M_PI 3.14159265358979323846  // number pi
 
 // logical variable main task
@@ -23,14 +20,11 @@
 float vel_cntrl_v1_fcn(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle);
 float vel_cntrl_v2_fcn(const float& wheel_speed_max, const float& b, const float& robot_omega, const Eigen::Matrix2f& Cwheel2robot);
 
-float speed = 0.0f;
-float motionspeed = 0.0f;
-
 int main()
 {
     // while loop gets executed every main_task_period_ms milliseconds
     const int main_task_period_ms = 10;   // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
-    Timer main_task_timer;          // create Timer object which we use to run the main task every main task period time in ms
+    Timer main_task_timer;                // create Timer object which we use to run the main task every main task period time in ms
 
     // led on nucleo board
     DigitalOut user_led(LED1);      // create DigitalOut object to command user led
@@ -53,34 +47,28 @@
     //const 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
     //const float kp = 0.1f;                            // define custom kp, this is the default speed controller gain for gear box 78.125:1
 
-    SpeedController* speedControllers[1];
+    SpeedController* speedControllers[2];
     speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
-    //speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
-    speedControllers[0]->setMaxAccelerationRPS(0.36f * max_voltage * kn * 0.1f);
-    //speedControllers[1]->setMaxAccelerationRPM(22.0f * max_voltage * kn * 0.1f);
-    PositionController* positionController = new PositionController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
-    positionController->setMaxAccelerationRPS(0.36f * max_voltage * kn * 0.1f);
-    positionController->setMaxVelocityRPS(2.0f);
-    //std::vector<SpeedController*> speedControllers;
-    //speedControllers.push_back( new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1) );
-    //speedControllers.push_back( new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2) );
-
+    speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
+    //speedControllers[0]->setMaxAccelerationRPS(0.36f * max_voltage * kn * 0.1f);
+    //speedControllers[1]->setMaxAccelerationRPS(0.36f * max_voltage * kn * 0.1f);
+    
     // create SensorBar object for sparkfun line follower array
     I2C i2c(PB_9, PB_8);
-    SensorBar sensor_bar(i2c, 0.1175f);
+    SensorBar sensor_bar(i2c, 0.1175f); // second input argument is distance from bar to wheel axis
 
     // robot kinematics
-    const float r_wheel = 0.0358f / 2.0f;
-    const float L_wheel = 0.143f;
+    const float r_wheel = 0.0358f / 2.0f; // wheel radius
+    const float L_wheel = 0.143f;         // distance from wheel to wheel
     Eigen::Matrix2f Cwheel2robot; // transform wheel to robot
-    Eigen::Matrix2f Crobot2wheel; // transform robot to wheel
-    Eigen::Vector2f robot_coord;  // contains v and w (robot translational and rotational velocities)
-    Eigen::Vector2f wheel_speed;  // w1 w2 (wheel speed)
     Cwheel2robot << r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
                     r_wheel / L_wheel, -r_wheel / L_wheel;
+    Eigen::Matrix2f Crobot2wheel; // transform robot to wheel
     Crobot2wheel << 1.0f / r_wheel,  L_wheel / (2.0f * r_wheel),
                     1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
+    Eigen::Vector2f robot_coord;  // contains v and w (robot translational and rotational velocities)
     robot_coord.setZero();
+    Eigen::Vector2f wheel_speed;  // w1 w2 (wheel speed)
     wheel_speed.setZero();
 
     // attach button fall and rise functions to user button object
@@ -90,22 +78,14 @@
     // start timer
     main_task_timer.start();
 
-    // enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled
-    enable_motors = 1;
-
-
-    Motion motion;
-    motion.setProfileVelocity(10.0f);
-    motion.setProfileAcceleration(5.0f);
-    motion.setProfileDeceleration(5.0f);
-
     while (true) { // this loop will run forever
 
         main_task_timer.reset();
 
         if (do_execute_main_task) {
 
-            
+            // enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled
+            enable_motors = 1;
 
             // read SensorBar
             static float sensor_bar_avgAngleRad = 0.0f; // by making this static it will not be overwritten (only fist time set to zero)
@@ -113,7 +93,7 @@
                 sensor_bar_avgAngleRad = sensor_bar.getAvgAngleRad();
             }
 
-            const static float Kp = 2.0f;
+            const static float Kp = 2.0f; // by making this const static it will not be overwritten and only initiliazed once
             const static float Kp_nl = 17.0f;
             robot_coord(1) = ang_cntrl_fcn(Kp, Kp_nl, sensor_bar_avgAngleRad);
 
@@ -138,21 +118,17 @@
             // read analog input
             ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
 
-            //speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
-            //speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
-            speedControllers[0]->setDesiredSpeedRPS(speed);
-            //speedControllers[1]->setDesiredSpeedRPS(speed);
-            positionController->setDesiredRotation(speed);
+            speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
+            speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
 
         } else {
 
+            enable_motors = 0;
+
             ir_distance_mV = 0.0f;
 
-            speedControllers[0]->setDesiredSpeedRPS(speed);
-            //speedControllers[1]->setDesiredSpeedRPS(speed);
-            positionController->setDesiredRotation(speed);
-            
-
+            speedControllers[0]->setDesiredSpeedRPS(0.0f);
+            speedControllers[1]->setDesiredSpeedRPS(0.0f);
 
         }
 
@@ -162,11 +138,6 @@
         //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());
 
-        motion.incrementToPosition(motionspeed, static_cast<float>(main_task_period_ms) * 1.0e-3f);
-        float motionpositionactual = motion.getPosition();
-        float motionspeedactual = motion.getVelocity();
-        printf("%f, %f, %f, %f, %f\r\n", speedControllers[0]->getSpeedRPS(), positionController->getRotation(), positionController->getSpeedRPS(), motionpositionactual, motionspeedactual);
-
         // 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();
         thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
@@ -186,13 +157,6 @@
     user_button_timer.stop();
     if (user_button_elapsed_time_ms > 200) {
         do_execute_main_task = !do_execute_main_task;
-        if(do_execute_main_task) {
-            motionspeed =  12.0f;
-            speed = 2.7f;
-        } else {
-            motionspeed = -12.0f;
-            speed = -2.7f;
-        }
     }
 }