Raphael Schmidt
Projekt
Diff: main.cpp
- Revision:
- 30:1e8295770bc1
- Parent:
- 29:d6f1ccf42a31
- Child:
- 31:12faf1ba9bc7
--- 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