main.cpp

00001 #include <mbed.h>
00002 #include <math.h>
00003 
00004 #include "PM2_Libary.h"
00005 #include "Eigen/Dense.h"
00006 
00007 #define NEW_PES_BOARD_VERSION
00008 
00009 #ifdef NEW_PES_BOARD_VERSION
00010     #define PN_enable_Motors PB_15
00011     #define PN_pwm_M1 PB_13
00012     #define PN_pwm_M2 PA_9
00013     #define PN_encoder_M1_A PA_6
00014     #define PN_encoder_M1_B PC_7
00015     #define PN_encoder_M2_A PB_6
00016     #define PN_encoder_M2_B PB_7
00017 #else
00018     #define PN_enable_Motors PB_2
00019     #define PN_pwm_M1 PA_8
00020     #define PN_pwm_M2 PA_9
00021     #define PN_encoder_M1_A PB_6
00022     #define PN_encoder_M1_B PB_7
00023     #define PN_encoder_M2_A PA_6
00024     #define PN_encoder_M2_B PC_7
00025 #endif
00026 
00027 #define M_PI 3.14159265358979323846  // number pi
00028 
00029 // logical variable main task
00030 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
00031 
00032 // user button on nucleo board
00033 Timer user_button_timer;            // create Timer object which we use to check if user button was pressed for a certain time (robust against signal bouncing)
00034 InterruptIn user_button(PC_13);     // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
00035 void user_button_pressed_fcn();     // custom functions which gets executed when user button gets pressed and released, definition below
00036 void user_button_released_fcn();
00037 
00038 // controller functions
00039 float ang_cntrl_fcn(const float& Kp, const float& Kp_nl, const float& angle);
00040 float vel_cntrl_v1_fcn(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle);
00041 float vel_cntrl_v2_fcn(const float& wheel_speed_max, const float& b, const float& robot_omega, const Eigen::Matrix2f& Cwheel2robot);
00042 
00043 int main()
00044 {
00045     // while loop gets executed every main_task_period_ms milliseconds
00046     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
00047     Timer main_task_timer;                // create Timer object which we use to run the main task every main task period time in ms
00048 
00049     // led on nucleo board
00050     DigitalOut user_led(LED1);      // create DigitalOut object to command user led
00051 
00052     // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor
00053     float ir_distance_mV = 0.0f;    // define variable to store measurement
00054     AnalogIn ir_analog_in(PC_2);    // create AnalogIn object to read in infrared distance sensor, 0...3.3V are mapped to 0...1
00055 
00056     // 78:1, 100:1, ... Metal Gearmotor 20Dx44L mm 12V CB
00057     DigitalOut enable_motors(PN_enable_Motors);    // create DigitalOut object to enable dc motors
00058 
00059     // create SpeedController objects
00060     FastPWM pwm_M1(PN_pwm_M1);  // motor M1 is closed-loop speed controlled (angle velocity)
00061     FastPWM pwm_M2(PN_pwm_M2);   // motor M2 is closed-loop speed controlled (angle velocity)
00062     EncoderCounter  encoder_M1(PN_encoder_M1_A, PN_encoder_M1_B); // create encoder objects to read in the encoder counter values
00063     EncoderCounter  encoder_M2(PN_encoder_M2_A, PN_encoder_M2_B);
00064     const float max_voltage = 12.0f;                  // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
00065     const float counts_per_turn = 20.0f * 78.125f;    // define counts per turn at gearbox end: counts/turn * gearratio
00066     const float kn = 180.0f / 12.0f;                  // define motor constant in rpm per V
00067     
00068     // create SpeedController objects
00069     SpeedController* speedControllers[2];
00070     speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
00071     speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
00072     //speedControllers[0]->setMaxAccelerationRPS(999.0f); // big number, so it is no doing anything
00073     //speedControllers[1]->setMaxAccelerationRPS(999.0f); // big number, so it is no doing anything
00074 
00075     // create SensorBar object for sparkfun line follower array
00076     I2C i2c(PB_9, PB_8);
00077     SensorBar sensor_bar(i2c, 0.1175f); // second input argument is distance from bar to wheel axis
00078 
00079     // robot kinematics
00080     const float r_wheel = 0.0358f / 2.0f; // wheel radius
00081     const float L_wheel = 0.143f;         // distance from wheel to wheel
00082     Eigen::Matrix2f Cwheel2robot; // transform wheel to robot
00083     //Eigen::Matrix2f Crobot2wheel; // transform robot to wheel
00084     Cwheel2robot <<  r_wheel / 2.0f   ,  r_wheel / 2.0f   ,
00085                      r_wheel / L_wheel, -r_wheel / L_wheel;
00086     //Crobot2wheel << 1.0f / r_wheel,  L_wheel / (2.0f * r_wheel),
00087     //                1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
00088     Eigen::Vector2f robot_coord;  // contains v and w (robot translational and rotational velocities)
00089     Eigen::Vector2f wheel_speed;  // w1 w2 (wheel speed)
00090     robot_coord.setZero();
00091     wheel_speed.setZero();
00092 
00093     // attach button fall and rise functions to user button object
00094     user_button.fall(&user_button_pressed_fcn);
00095     user_button.rise(&user_button_released_fcn);
00096 
00097     // start timer
00098     main_task_timer.start();
00099 
00100     while (true) { // this loop will run forever
00101 
00102         main_task_timer.reset();
00103 
00104         if (do_execute_main_task) {
00105 
00106             // enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled
00107             enable_motors = 1;
00108 
00109             // read SensorBar
00110             static float sensor_bar_avgAngleRad = 0.0f; // by making this static it will not be overwritten (only fist time set to zero)
00111             if (sensor_bar.isAnyLedActive()) {
00112                 sensor_bar_avgAngleRad = sensor_bar.getAvgAngleRad();
00113             }
00114 
00115             const static float Kp = 2.0f; // by making this const static it will not be overwritten and only initiliazed once
00116             const static float Kp_nl = 17.0f;
00117             robot_coord(1) = ang_cntrl_fcn(Kp, Kp_nl, sensor_bar_avgAngleRad);
00118 
00119             // nonlinear controllers version 1 (whatever came to my mind)
00120             /*
00121             const static float vel_max = 0.3374f; //0.10f;
00122             const static float vel_min = 0.00f; //0.02f;
00123             const static float ang_max = 27.0f * M_PI / 180.0f;
00124             robot_coord(0) = vel_cntrl_v1_fcn(vel_max, vel_min, ang_max, sensor_bar_avgAngleRad);
00125             */
00126 
00127             // nonlinear controllers version 2 (one wheel always at full speed controller)
00128             ///*
00129             const static float wheel_speed_max = max_voltage * kn / 60.0f * 2.0f * M_PI;
00130             const static float b = L_wheel / (2.0f * r_wheel);
00131             robot_coord(0) = vel_cntrl_v2_fcn(wheel_speed_max, b, robot_coord(1), Cwheel2robot);
00132             //*/
00133 
00134             // transform robot coordinates to wheel speed
00135             wheel_speed = Cwheel2robot.inverse() * robot_coord;
00136 
00137             // read analog input
00138             ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f;
00139 
00140             // command speedController objects
00141             speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
00142             speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
00143 
00144         } else {
00145 
00146             enable_motors = 0;
00147 
00148             ir_distance_mV = 0.0f;
00149 
00150             speedControllers[0]->setDesiredSpeedRPS(0.0f);
00151             speedControllers[1]->setDesiredSpeedRPS(0.0f);
00152 
00153         }
00154 
00155         user_led = !user_led;
00156 
00157         // do only output via serial what's really necessary (this makes your code slow)
00158         printf("%f, %f, %f\r\n", speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS(), sensor_bar.getAvgAngleRad() * 180.0f / M_PI);
00159 
00160         // read timer and make the main thread sleep for the remaining time span (non blocking)
00161         int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
00162         thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
00163     }
00164 }
00165 
00166 void user_button_pressed_fcn()
00167 {
00168     user_button_timer.start();
00169     user_button_timer.reset();
00170 }
00171 
00172 void user_button_released_fcn()
00173 {
00174     // read timer and toggle do_execute_main_task if the button was pressed longer than the below specified time
00175     int user_button_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
00176     user_button_timer.stop();
00177     if (user_button_elapsed_time_ms > 200) {
00178         do_execute_main_task = !do_execute_main_task;
00179     }
00180 }
00181 
00182 float ang_cntrl_fcn(const float& Kp, const float& Kp_nl, const float& angle)
00183 {
00184     static float retval = 0.0f;
00185     if (angle > 0) {
00186         retval = Kp * angle + Kp_nl * angle * angle;
00187     } else if (angle <= 0) {
00188         retval = Kp * angle - Kp_nl * angle * angle;
00189     }
00190     return retval;
00191 }
00192 
00193 float vel_cntrl_v1_fcn(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle)
00194 {
00195     const static float gain = (vel_min - vel_max) / ang_max;
00196     const static float offset = vel_max;
00197     return gain * fabs(angle) + offset;
00198 }
00199 
00200 float vel_cntrl_v2_fcn(const float& wheel_speed_max, const float& b, const float& robot_omega, const Eigen::Matrix2f& Cwheel2robot)
00201 {
00202     static Eigen::Matrix<float, 2, 2> _wheel_speed;
00203     static Eigen::Matrix<float, 2, 2> _robot_coord;
00204     if (robot_omega > 0) {
00205         _wheel_speed(0) = wheel_speed_max;
00206         _wheel_speed(1) = wheel_speed_max - 2*b*robot_omega;
00207     } else {
00208         _wheel_speed(0) = wheel_speed_max + 2*b*robot_omega;
00209         _wheel_speed(1) = wheel_speed_max;
00210     }
00211     _robot_coord = Cwheel2robot * _wheel_speed;
00212     return _robot_coord(0);
00213 }