Sebastian Grünwald
/
PM2_Example_PES_board
PM2_Example
Diff: main.cpp
- Revision:
- 30:1e8295770bc1
- Parent:
- 29:d6f1ccf42a31
- Child:
- 31:1b2a1bd1bccb
diff -r d6f1ccf42a31 -r 1e8295770bc1 main.cpp --- a/main.cpp Fri Mar 18 09:44:52 2022 +0100 +++ b/main.cpp Tue Mar 22 10:45:49 2022 +0100 @@ -32,41 +32,41 @@ 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 used open loop -FastPWM pwm_M2(PA_9); // motor M2 is speed controlled -FastPWM pwm_M3(PA_10); // motor M3 is position controlled (angle controlled) +FastPWM pwm_M2(PA_9); // motor M2 is closed-loop speed controlled (angle velocity) +FastPWM pwm_M3(PA_10); // motor M3 is closed-loop position controlled (angle controlled) -EncoderCounter encoder_M1(PA_6, PC_7); // create encoder read objects +EncoderCounter encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values EncoderCounter encoder_M2(PB_6, PB_7); EncoderCounter encoder_M3(PA_0, PA_1); -// create SpeedController and PositionController controller objects, default parametrization is for 78.125:1 gear box +// 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 = 25.0f / 78.125f; // define additional ratio in case you are using a dc motor with a different gear box, e.g. 25:1 -// float kp = 0.1f; // define custom kp, this is the default speed controller gain for gear box 78.125:1 +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_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 25:1 gear +// 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 float max_speed_rps = 0.5f; // define maximum speed that the position controller is changig the speed, has to be smaller or equal to kn * max_voltage -PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters -// PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 25:1 gear, we need a different speed controller gain here +// PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters +PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 100:1 gear, we need a different speed controller gain here // Futaba Servo S3001 20mm 3kg Analog Servo servo_S1(PB_2); // create servo objects Servo servo_S2(PC_8); -int servo_pos_S1_mus = 0; // servo S1 position, the desired position gets commanded as a time -int servo_pos_S2_mus = 0; // servo S2 position +float servo_S1_angle = 0; // servo S1 normalized angle +float servo_S2_angle = 0; // servo S2 normalized angle int servo_period_mus = 20000; // define servo period time in mus int servo_counter = 0; // define servo counter, this is an additional variable to make the servos move -int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)main_task_period_ms))); // define loops per second +int loops_per_seconds = static_cast<int>(ceilf(1.0f/(0.001f*(float)main_task_period_ms))); // define loops per second // Groove Ultrasonic Ranger V2.0 float us_distance_cm = 0.0f; // define variable to store measurement -RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f, 17500); -// RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f, 7000); // create range finder object (ultra sonic distance sensor), 1/main_task_period_ms = 20 Hz parametrization +RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f, 17500); // create range finder object (ultra sonic distance sensor), 20 Hz parametrization +// RangeFinder us_range_finder(PB_12, 5782.0f, 0.02f, 7000); // create range finder object (ultra sonic distance sensor), 50 Hz parametrization // LSM9DS1 IMU, carefull: not all PES boards have an imu (chip shortage) // LSM9DS1 imu(PC_9, PA_8); // create LSM9DS1 comunication object, if you want to be able to use the imu you need to #include "LSM9DS1_i2c.h" @@ -88,11 +88,11 @@ // motor M1 is used open-loop, we need to initialize the pwm and set pwm output to zero at the beginning, range: 0...1 -> u_min...u_max: 0.5 -> 0 V pwm_M1.period(pwm_period_s); - pwm_M1.write(0.5); + pwm_M1.write(0.5f); // enable servos, you can also disable them at any point in your program if you don't want your servos to become warm - servo_S1.Enable(servo_pos_S1_mus, servo_period_mus); - servo_S2.Enable(servo_pos_S2_mus, servo_period_mus); + servo_S1.Enable(servo_S1_angle, servo_period_mus); + servo_S2.Enable(servo_S2_angle, servo_period_mus); while (true) { // this loop will run forever @@ -105,23 +105,26 @@ // command dc motors if mechanical button is pressed if (mechanical_button.read()) { - pwm_M1.write(0.75); // write output voltage to motor M1 + pwm_M1.write(0.75f); // write output voltage to motor M1 speedController_M2.setDesiredSpeedRPS(0.5f); // set a desired speed for speed controlled dc motors M2 positionController_M3.setDesiredRotation(1.5f, max_speed_rps); // set a desired rotation for position controlled dc motors M3 } else { - pwm_M1.write(0.5); + pwm_M1.write(0.5f); speedController_M2.setDesiredSpeedRPS(0.0f); positionController_M3.setDesiredRotation(0.0f, max_speed_rps); } + // check if servos are enabled + if (!servo_S1.isEnabled()) servo_S1.Enable(servo_S1_angle, servo_period_mus); + if (!servo_S2.isEnabled()) servo_S2.Enable(servo_S2_angle, servo_period_mus); // command servo position, this needs to be calibrated - servo_S1.SetPosition(servo_pos_S1_mus); - servo_S2.SetPosition(servo_pos_S2_mus); - if (servo_pos_S1_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) { - servo_pos_S1_mus += 100; + servo_S1.SetPosition(servo_S1_angle); + if (servo_S1_angle <= 1.0f & servo_counter%loops_per_seconds == 0 & servo_counter != 0) { + servo_S1_angle += 0.01f; } - if (servo_pos_S2_mus <= servo_period_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) { - servo_pos_S2_mus += 100; + servo_S2.SetPosition(servo_S2_angle); + if (servo_S2_angle <= 1.0f & servo_counter%loops_per_seconds == 0 & servo_counter != 0) { + servo_S2_angle += 0.01f; } servo_counter++; @@ -135,14 +138,16 @@ ir_distance_mV = 0.0f; - pwm_M1.write(0.5); + pwm_M1.write(0.5f); speedController_M2.setDesiredSpeedRPS(0.0f); positionController_M3.setDesiredRotation(0.0f, max_speed_rps); - servo_pos_S1_mus = 0; - servo_pos_S2_mus = 0; - servo_S1.SetPosition(servo_pos_S1_mus); - servo_S2.SetPosition(servo_pos_S2_mus); + servo_S1_angle = 0; + servo_S2_angle = 0; + // servo_S1.SetPosition(servo_S1_angle); + // servo_S2.SetPosition(servo_S2_angle); + if (servo_S1.isEnabled()) servo_S1.Disable(); + if (servo_S2.isEnabled()) servo_S2.Disable(); us_distance_cm = 0.0f; @@ -152,13 +157,13 @@ user_led = !user_led; // do only output via serial what's really necessary (this makes your code slow) - printf("IR sensor (mV): %3.3f, Encoder M1: %3d, Speed M2 (rps) %3.3f, Position M3 (rot): %3.3f, Servo S1 position (ms): %3d, Servo S2 position (ms): %3d, US sensor (cm): %3.3f\r\n", + printf("IR sensor (mV): %3.3f, Encoder M1: %3d, Speed M2 (rps) %3.3f, Position M3 (rot): %3.3f, Servo S1 angle (normalized): %3.3f, Servo S2 angle (normalized): %3.3f, US sensor (cm): %3.3f\r\n", ir_distance_mV, encoder_M1.read(), speedController_M2.getSpeedRPS(), positionController_M3.getRotation(), - servo_pos_S1_mus, - servo_pos_S2_mus, + servo_S1_angle, + servo_S2_angle, us_distance_cm); // read out the imu, the actual frames of the sensor reading needs to be figured out