Example project for the Line Follower robot.
Dependencies: PM2_Libary Eigen
Diff: main.cpp
- Revision:
- 45:5e1dd4117ed2
- Parent:
- 44:340cdc4b6e47
- Child:
- 46:fd580fa68618
--- a/main.cpp Fri May 13 14:56:01 2022 +0200 +++ b/main.cpp Fri May 13 16:01:02 2022 +0200 @@ -16,66 +16,63 @@ void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below void user_button_released_fcn(); -// while loop gets executed every main_task_period_ms milliseconds -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 - -// led on nucleo board -DigitalOut user_led(LED1); // create DigitalOut object to command user led - -// Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor -float ir_distance_mV = 0.0f; // define variable to store measurement -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 - -float pwm_period_s = 0.00005f; // define pwm period time in seconds and create FastPWM objects to command dc motors -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); - -// create SpeedController and PositionController objects, default parametrization is for 78.125:1 gear box -float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack -float counts_per_turn = 20.0f * 78.125f; // define counts per turn at gearbox end: counts/turn * gearratio -float kn = 180.0f / 12.0f; // define motor constant in rpm per V -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_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); - -// transformations and stuff -float r_wheel = 0.0358f / 2.0f; -float L_wheel = 0.143f; -Eigen::Matrix<float, 2, 2> Cwheel2robot; // transform wheel to robot -Eigen::Matrix<float, 2, 2> Crobot2wheel; // transform robot to wheel -Eigen::Matrix<float, 2, 1> robot_coord; // contains v and w -Eigen::Matrix<float, 2, 1> wheel_speed; // w1 w2 - -float fcn_ang_cntrl(const float& Kp, const float& Kp_nl, const float& angle); -float fcn_vel_cntrl_v1(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle); -float fcn_vel_cntrl_v2(float wheel_speed_max, float b, float robot_omega); +// controller functions +float ang_cntrl_fcn(const float& Kp, const float& Kp_nl, const float& angle); +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); 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) ); + // 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 + + // led on nucleo board + DigitalOut user_led(LED1); // create DigitalOut object to command user led + + // Sharp GP2Y0A41SK0F, 4-40 cm IR Sensor + float ir_distance_mV = 0.0f; // define variable to store measurement + 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 + + // create SpeedController objects, default parametrization is for 78.125:1 gear box + 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); + 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 + 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); + //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) ); + + // create SensorBar object for sparkfun line follower array + I2C i2c(PB_9, PB_8); + SensorBar sensor_bar(i2c, 0.1175f); + + // robot kinematics + const float r_wheel = 0.0358f / 2.0f; + const float L_wheel = 0.143f; + 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; + Crobot2wheel << 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); @@ -86,14 +83,6 @@ // enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled enable_motors = 1; - // initialise matrizes and vectros - 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); - robot_coord << 0.06f, 0.0f; - wheel_speed << 0.0f, 0.0f; - while (true) { // this loop will run forever main_task_timer.reset(); @@ -106,23 +95,23 @@ sensor_bar_avgAngleRad = sensor_bar.getAvgAngleRad(); } - const static float Kp = 2.0f; //2.0f; - const static float Kp_nl = 17.0f; //10.0f; //5.0f; - robot_coord(1) = fcn_ang_cntrl(Kp, Kp_nl, sensor_bar_avgAngleRad); + const static float Kp = 2.0f; + const static float Kp_nl = 17.0f; + robot_coord(1) = ang_cntrl_fcn(Kp, Kp_nl, sensor_bar_avgAngleRad); // nonlinear controllers version 1 (whatever came to my mind) /* const static float vel_max = 0.3374f; //0.10f; const static float vel_min = 0.00f; //0.02f; const static float ang_max = 27.0f * M_PI / 180.0f; - robot_coord(0) = fcn_vel_cntrl_v1(vel_max, vel_min, ang_max, sensor_bar_avgAngleRad); + robot_coord(0) = vel_cntrl_v1_fcn(vel_max, vel_min, ang_max, sensor_bar_avgAngleRad); */ // nonlinear controllers version 2 (one wheel always at full speed controller) ///* - static float wheel_speed_max = max_voltage * kn / 60.0f * 2.0f * M_PI; - static float b = L_wheel / (2.0f * r_wheel); - robot_coord(0) = fcn_vel_cntrl_v2(wheel_speed_max, b, robot_coord(1)); + const static float wheel_speed_max = max_voltage * kn / 60.0f * 2.0f * M_PI; + const static float b = L_wheel / (2.0f * r_wheel); + robot_coord(0) = vel_cntrl_v2_fcn(wheel_speed_max, b, robot_coord(1), Cwheel2robot); //*/ // transform to robot coordinates @@ -131,42 +120,15 @@ // read analog input ir_distance_mV = 1.0e3f * ir_analog_in.read() * 3.3f; - 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(); - for( int i = 7; i >= 0; i-- ) { - printf("%d", (sensor_bar_raw_value >> i) & 0x01); - } - printf(", "); - */ - - /* - int8_t sensor_bar_binaryPosition = sensor_bar.getBinaryPosition(); - printf("%d, ", sensor_bar_binaryPosition); - - uint8_t sensor_bar_nrOfLedsActive = sensor_bar.getNrOfLedsActive(); - printf("%d, ", sensor_bar_nrOfLedsActive); - - float sensor_bar_angleRad = 0.0f; - float sensor_bar_avgAngleRad = 0.0f; - if (sensor_bar.isAnyLedActive()) { - sensor_bar_angleRad = sensor_bar.getAngleRad(); - sensor_bar_avgAngleRad = sensor_bar.getAvgAngleRad(); - } - printf("%f, ", sensor_bar_angleRad * 180.0f / M_PI); - printf("%f, ", sensor_bar_avgAngleRad * 180.0f / M_PI); - */ - - printf("%f, %f\r\n", wheel_speed(0) / (2.0f * M_PI), wheel_speed(1) / (2.0f * M_PI)); + 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 { ir_distance_mV = 0.0f; - speedController_ptr[0]->setDesiredSpeedRPS(0.0f); - speedController_ptr[1]->setDesiredSpeedRPS(0.0f); + speedControllers[0]->setDesiredSpeedRPS(0.0f); + speedControllers[1]->setDesiredSpeedRPS(0.0f); } user_led = !user_led; @@ -196,25 +158,25 @@ } } -float fcn_ang_cntrl(const float& Kp, const float& Kp_nl, const float& angle) +float ang_cntrl_fcn(const float& Kp, const float& Kp_nl, const float& angle) { - float retval = 0.0f; + static float retval = 0.0f; if (angle > 0) { retval = Kp * angle + Kp_nl * angle * angle; - } else if (angle < 0) { + } else if (angle <= 0) { retval = Kp * angle - Kp_nl * angle * angle; } return retval; } -float fcn_vel_cntrl_v1(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle) +float vel_cntrl_v1_fcn(const float& vel_max, const float& vel_min, const float& ang_max, const float& angle) { const static float gain = (vel_min - vel_max) / ang_max; const static float offset = vel_max; return gain * fabs(angle) + offset; } -float fcn_vel_cntrl_v2(float wheel_speed_max, float b, float robot_omega) +float vel_cntrl_v2_fcn(const float& wheel_speed_max, const float& b, const float& robot_omega, const Eigen::Matrix2f& Cwheel2robot) { static Eigen::Matrix<float, 2, 2> _wheel_speed; static Eigen::Matrix<float, 2, 2> _robot_coord;