-
Notifications
You must be signed in to change notification settings - Fork 145
/
main_calib_kinematic_ss24.cpp
183 lines (154 loc) · 8.38 KB
/
main_calib_kinematic_ss24.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
#include "mbed.h"
// pes board pin map
#include "pm2_drivers/PESBoardPinMap.h"
// drivers
#include "pm2_drivers/DebounceIn.h"
#include "pm2_drivers/DCMotor.h"
#include "eigen/Dense.h"
#define M_PIf 3.14159265358979323846f /* pi */
bool do_execute_main_task = false; // this variable will be toggled via the user button (blue button) and
// decides whether to execute the main task or not
bool do_reset_all_once = false; // this variable is used to reset certain variables and objects and
// shows how you can run a code segment only once
// objects for user button (blue button) handling on nucleo board
DebounceIn user_button(USER_BUTTON); // create DebounceIn object to evaluate the user button
// falling and rising edge
void toggle_do_execute_main_fcn(); // custom function which is getting executed when user
// button gets pressed, definition below
// main runs as an own thread
int main()
{
// attach button fall function address to user button object, button has a pull-up resistor
user_button.fall(&toggle_do_execute_main_fcn);
// while loop gets executed every main_task_period_ms milliseconds, this is a
// simple approach to repeatedly execute main
const int main_task_period_ms = 20; // define main task period time in ms e.g. 20 ms, there for
// the main task will run 50 times per second
Timer main_task_timer; // create Timer object which we use to run the main task
// every main_task_period_ms
// led on nucleo board
DigitalOut user_led(USER_LED);
// additional led
// create DigitalOut object to command extra led, you need to add an aditional resistor, e.g. 220...500 Ohm
// a led has an anode (+) and a cathode (-), the cathode needs to be connected to ground via a resistor
DigitalOut led1(PB_9);
// create object to enable power electronics for the dc motors
DigitalOut enable_motors(PB_ENABLE_DCMOTORS);
const float voltage_max = 12.0f; // maximum voltage of battery packs, adjust this to
// 6.0f V if you only use one battery pack
const float gear_ratio = 78.125f;
const float kn = 180.0f / 12.0f;
// motor M1 and M2, do NOT enable motion planner, disabled per default
DCMotor motor_M1(PB_PWM_M1, PB_ENC_A_M1, PB_ENC_B_M1, gear_ratio, kn, voltage_max);
DCMotor motor_M2(PB_PWM_M2, PB_ENC_A_M2, PB_ENC_B_M2, gear_ratio, kn, voltage_max);
// const float r1_wheel = 0.0357f / 2.0f; // right wheel radius in meters
// const float r2_wheel = 0.0357f / 2.0f; // left wheel radius in meters
// const float b_wheel = 0.1520f; // wheelbase, distance from wheel to wheel in meters
const float r1_wheel = 0.01783f; // right wheel radius in meters
const float r2_wheel = 0.01787f; // left wheel radius in meters
const float b_wheel = 0.15572f; // wheelbase, distance from wheel to wheel in meters
const float radius_ratio = r1_wheel / r2_wheel;
// additional dc motor settings (don't use them for the line follower)
motor_M1.enableMotionPlanner();
motor_M1.setMaxVelocity(motor_M1.getMaxVelocity() * 0.6f);
motor_M1.setMaxAcceleration(motor_M1.getMaxAcceleration() * 0.2f);
motor_M1.setVelocityCntrlIntegratorLimitsPercent(100.0f);
motor_M2.enableMotionPlanner();
motor_M2.setMaxVelocity(motor_M2.getMaxVelocity() * 0.6f * radius_ratio);
motor_M2.setMaxAcceleration(motor_M2.getMaxAcceleration() * 0.2f * radius_ratio);
motor_M2.setVelocityCntrlIntegratorLimitsPercent(100.0f);
// transforms robot to wheel velocities
Eigen::Matrix2f Cwheel2robot;
Cwheel2robot << r1_wheel / 2.0f , r2_wheel / 2.0f ,
r1_wheel / b_wheel, -r2_wheel / b_wheel;
const float L_square = 1.4f; // forward distance in meters
const float turn = -M_PIf / 2.0f; // rotation angle in radians
// -90 deg for right turn (CW), +90 deg for left turn (CCW)
// calculate pure forward and pure turn movement as wheel angles
Eigen::Vector2f robot_coord_forward = {L_square, 0.0f};
Eigen::Vector2f robot_coord_turn = {0.0, turn};
Eigen::Vector2f wheel_angle_forward = Cwheel2robot.inverse() * robot_coord_forward;
Eigen::Vector2f wheel_angle_turn = Cwheel2robot.inverse() * robot_coord_turn;
// set up states for state machine
enum RobotState {
FORWARD,
TURN,
FORWARD_OR_RESET,
RESET
} robot_state = RobotState::FORWARD;
const float angle_threshold = 0.005f;
int turn_cntr = 0;
// start timer
main_task_timer.start();
// this loop will run forever
while (true) {
main_task_timer.reset();
if (do_execute_main_task) {
// visual feedback that the main task is executed, setting this once would actually be enough
led1 = 1;
enable_motors = 1;
// state machine
switch (robot_state) {
case RobotState::FORWARD:
motor_M1.setRotationRelative(wheel_angle_forward(0) / (2.0f * M_PIf));
motor_M2.setRotationRelative(wheel_angle_forward(1) / (2.0f * M_PIf));
robot_state = RobotState::TURN;
break;
case RobotState::TURN:
if ((fabs(motor_M1.getRotationTarget() - motor_M1.getRotation()) < angle_threshold) &&
(fabs(motor_M2.getRotationTarget() - motor_M2.getRotation()) < angle_threshold)) {
motor_M1.setRotationRelative(wheel_angle_turn(0) / (2.0f * M_PIf));
motor_M2.setRotationRelative(wheel_angle_turn(1) / (2.0f * M_PIf));
robot_state = RobotState::FORWARD_OR_RESET;
}
break;
case RobotState::FORWARD_OR_RESET:
if ((fabs(motor_M1.getRotationTarget() - motor_M1.getRotation()) < angle_threshold) &&
(fabs(motor_M2.getRotationTarget() - motor_M2.getRotation()) < angle_threshold)) {
turn_cntr++;
if (turn_cntr == 4) {
robot_state = RobotState::RESET;
} else {
robot_state = RobotState::FORWARD;
}
}
break;
case RobotState::RESET:
toggle_do_execute_main_fcn();
turn_cntr = 0;
robot_state = RobotState::FORWARD;
break;
default:
break;
}
// debugging
// printf("RS: %d, M1 SP: %.3f, M2 SP: %.3f, M1: %.3f, M2: %.3f, TC: %d\n", robot_state
// , motor_M1.getRotationTarget()
// , motor_M2.getRotationTarget()
// , motor_M1.getRotation()
// , motor_M2.getRotation()
// , turn_cntr);
} else {
// the following code block gets executed only once
if (do_reset_all_once) {
do_reset_all_once = false;
// reset variables and objects
led1 = 0;
// enable_motors = 0; // do not disable motors for this specific example
}
}
// toggling the user led
user_led = !user_led;
// 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();
thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
}
}
void toggle_do_execute_main_fcn()
{
// toggle do_execute_main_task if the button was pressed
do_execute_main_task = !do_execute_main_task;
// set do_reset_all_once to true if do_execute_main_task changed from false to true
if (do_execute_main_task)
do_reset_all_once = true;
}