Diff: main.cpp

Revision:

46:fd580fa68618

Parent:

45:5e1dd4117ed2

Child:

47:5ce234723e3a

--- a/main.cpp	Fri May 13 16:01:02 2022 +0200
+++ b/main.cpp	Fri May 13 20:53:37 2022 +0000
@@ -1,10 +1,12 @@
 #include <mbed.h>
 #include <math.h>
-#include <vector>
+//#include <vector>
 
 #include "PM2_Libary.h"
 #include "Eigen/Dense.h"
 
+#include "Motion.h"
+
 # define M_PI 3.14159265358979323846  // number pi
 
 // logical variable main task
@@ -21,8 +23,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
@@ -45,12 +50,17 @@
     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
-    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
+    //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[2];
+    SpeedController* speedControllers[1];
     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[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
+    speedControllers[0]->setMaxAccelerationRPM(22.0f * max_voltage * kn * 0.05f);
+    //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->setMaxAccelerationRPM(22.0f * max_voltage * kn * 0.05f);
+    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) );
@@ -66,13 +76,13 @@
     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;
+    Cwheel2robot << r_wheel / 2.0f,  r_wheel / 2.0f,
+                 r_wheel / L_wheel, -r_wheel / L_wheel;
     Crobot2wheel << 1.0f / r_wheel,  L_wheel / (2.0f * r_wheel),
-                    1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
+                 1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
     robot_coord.setZero();
     wheel_speed.setZero();
-    
+
     // attach button fall and rise functions to user button object
     user_button.fall(&user_button_pressed_fcn);
     user_button.rise(&user_button_released_fcn);
@@ -83,6 +93,12 @@
     // 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();
@@ -120,21 +136,34 @@
             // 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(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);
 
         } else {
 
             ir_distance_mV = 0.0f;
 
-            speedControllers[0]->setDesiredSpeedRPS(0.0f);
-            speedControllers[1]->setDesiredSpeedRPS(0.0f);
+            speedControllers[0]->setDesiredSpeedRPS(speed);
+            //speedControllers[1]->setDesiredSpeedRPS(speed);
+            positionController->setDesiredRotation(speed);
+            
+
+
         }
 
         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("%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();
@@ -155,6 +184,13 @@
     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;
+        }
     }
 }