Whose Line is it Anyways / Mbed OS PM2_WLiiA_robot_code

robot code for summer school

Dependencies:   PM2_Libary Eigen

Fork of PM2_Example_Summer_School by Alex Hawkins

Robot_Library/robot.cpp@77:19cf9072bc22, 2022-06-01 (annotated)

Committer:
eversonrosed
Date:
Wed Jun 01 08:18:10 2022 +0200
Revision:
77:19cf9072bc22
Parent:
76:2302f2b51e63
Child:
78:d53f1d68ca65
target movement code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
seas726 49:7da71f479dac 1 #include "robot.h"
seas726 49:7da71f479dac 2 #include "EncoderCounter.h"
seas726 49:7da71f479dac 3 #include "PeripheralNames.h"
seas726 49:7da71f479dac 4 #include "PinNames.h"
eversonrosed 73:667d568da72a 5 #include <array>
seas726 56:3fce0a9bb6df 6 #include <cstdint>
seas726 49:7da71f479dac 7 #include <cstdio>
eversonrosed 73:667d568da72a 8 #include <cmath>
eversonrosed 73:667d568da72a 9
eversonrosed 74:76c7a805f63d 10 static float ComputeAngle(std::uint8_t raw, float default_val);
eversonrosed 74:76c7a805f63d 11 static float Multiplier(float angle);
seas726 49:7da71f479dac 12
eversonrosed 60:85a40e69ced6 13 Robot::Robot()
eversonrosed 60:85a40e69ced6 14 : dist(PB_1), // initialize all of the physical ports
eversonrosed 60:85a40e69ced6 15 bit0(PH_1), bit1(PC_2), bit2(PC_3),
eversonrosed 60:85a40e69ced6 16 ir_sensor_0(dist, bit0, bit1, bit2, 0), // one IR sensor
eversonrosed 60:85a40e69ced6 17 i2c2(PB_9, PB_8), // line sensor
eversonrosed 73:667d568da72a 18 line_sensor(i2c2), enable_motors(PB_15), pwm_M1(PB_13),
eversonrosed 73:667d568da72a 19 pwm_M2(PA_9), // motors + encoders
eversonrosed 76:2302f2b51e63 20 encoder_M1(PA_6, PC_7), encoder_M2(PB_6, PB_7),
eversonrosed 77:19cf9072bc22 21 theta(0.0f), target_theta(0.0f),
eversonrosed 77:19cf9072bc22 22 robot_x(0.0f), target_x(0.0f),
eversonrosed 77:19cf9072bc22 23 robot_y(0.0f), target_y(0.0f) {
eversonrosed 60:85a40e69ced6 24 // initialize all variables
eversonrosed 76:2302f2b51e63 25 wheel_to_robot << WHEEL_RADIUS / 2.0f, -WHEEL_RADIUS / 2.0f,
eversonrosed 60:85a40e69ced6 26 -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS,
eversonrosed 60:85a40e69ced6 27 -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS; // transformation matrix
eversonrosed 60:85a40e69ced6 28 robot_to_wheel = wheel_to_robot.inverse();
seas726 56:3fce0a9bb6df 29
eversonrosed 60:85a40e69ced6 30 robot_speed_desired.setZero(); // zero out all speeds
eversonrosed 60:85a40e69ced6 31 wheel_speed_desired.setZero();
eversonrosed 60:85a40e69ced6 32 wheel_speed_smooth.setZero();
eversonrosed 60:85a40e69ced6 33 robot_speed_actual.setZero();
eversonrosed 60:85a40e69ced6 34 wheel_speed_actual.setZero();
eversonrosed 60:85a40e69ced6 35
eversonrosed 60:85a40e69ced6 36 // MOTORS + MOTION
seas726 49:7da71f479dac 37
eversonrosed 60:85a40e69ced6 38 // TRAJECTORY PLANNERS
eversonrosed 60:85a40e69ced6 39 trajectoryPlanners[0] = new Motion();
eversonrosed 60:85a40e69ced6 40 trajectoryPlanners[1] = new Motion();
seas726 49:7da71f479dac 41
eversonrosed 60:85a40e69ced6 42 trajectoryPlanners[0]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
eversonrosed 60:85a40e69ced6 43 trajectoryPlanners[1]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
eversonrosed 60:85a40e69ced6 44 trajectoryPlanners[0]->setProfileAcceleration(10.0f);
eversonrosed 60:85a40e69ced6 45 trajectoryPlanners[1]->setProfileAcceleration(10.0f);
eversonrosed 60:85a40e69ced6 46 trajectoryPlanners[0]->setProfileDeceleration(10.0f);
eversonrosed 60:85a40e69ced6 47 trajectoryPlanners[1]->setProfileDeceleration(10.0f);
seas726 49:7da71f479dac 48
eversonrosed 60:85a40e69ced6 49 // SPEED CONTROLLERS
eversonrosed 60:85a40e69ced6 50 speedControllers[0] = new SpeedController(
eversonrosed 60:85a40e69ced6 51 COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M1, encoder_M1);
eversonrosed 60:85a40e69ced6 52 speedControllers[1] = new SpeedController(
eversonrosed 60:85a40e69ced6 53 COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M2, encoder_M2);
seas726 49:7da71f479dac 54
eversonrosed 60:85a40e69ced6 55 speedControllers[0]->setSpeedCntrlGain(
eversonrosed 60:85a40e69ced6 56 0.04f); // adjust speed controller gains
eversonrosed 60:85a40e69ced6 57 speedControllers[1]->setSpeedCntrlGain(0.04f);
eversonrosed 60:85a40e69ced6 58 speedControllers[0]->setMaxAccelerationRPS(
eversonrosed 60:85a40e69ced6 59 999.0f); // adjust max. acceleration for smooth movement
eversonrosed 60:85a40e69ced6 60 speedControllers[1]->setMaxAccelerationRPS(999.0f);
seas726 49:7da71f479dac 61 }
seas726 49:7da71f479dac 62
eversonrosed 60:85a40e69ced6 63 Robot::~Robot() {
eversonrosed 60:85a40e69ced6 64 delete trajectoryPlanners[0];
eversonrosed 60:85a40e69ced6 65 delete trajectoryPlanners[1];
eversonrosed 60:85a40e69ced6 66 delete speedControllers[0];
eversonrosed 60:85a40e69ced6 67 delete speedControllers[1];
eversonrosed 54:b442660523df 68 }
eversonrosed 54:b442660523df 69
seas726 49:7da71f479dac 70 void Robot::Update() {
seas726 49:7da71f479dac 71
eversonrosed 60:85a40e69ced6 72 controller.Update();
eversonrosed 73:667d568da72a 73
eversonrosed 73:667d568da72a 74 wheel_speed_actual << speedControllers[0]->getSpeedRPS(),
eversonrosed 73:667d568da72a 75 speedControllers[1]->getSpeedRPS();
eversonrosed 67:90f378806cbe 76 robot_speed_actual = wheel_to_robot * wheel_speed_actual;
seas726 49:7da71f479dac 77
eversonrosed 60:85a40e69ced6 78 switch (state) {
eversonrosed 73:667d568da72a 79 case INITIAL:
eversonrosed 73:667d568da72a 80 Initial();
eversonrosed 73:667d568da72a 81 break;
eversonrosed 73:667d568da72a 82 case IDLE:
eversonrosed 73:667d568da72a 83 Idle();
eversonrosed 73:667d568da72a 84 break;
eversonrosed 73:667d568da72a 85 case FOLLOWING_LINE:
eversonrosed 73:667d568da72a 86 FollowingLine();
eversonrosed 73:667d568da72a 87 break;
eversonrosed 77:19cf9072bc22 88 case TARGETING:
eversonrosed 77:19cf9072bc22 89 MoveToTarget();
eversonrosed 73:667d568da72a 90 break;
eversonrosed 73:667d568da72a 91 case AVOIDING_OBSTACLE:
eversonrosed 73:667d568da72a 92 AvoidObstacle();
eversonrosed 73:667d568da72a 93 break;
eversonrosed 73:667d568da72a 94 default:
eversonrosed 73:667d568da72a 95 state = IDLE; // on default, stop the car
eversonrosed 60:85a40e69ced6 96 }
eversonrosed 73:667d568da72a 97
eversonrosed 76:2302f2b51e63 98 if (!controller.GetTurnedOn()) {
eversonrosed 76:2302f2b51e63 99 state = IDLE;
eversonrosed 76:2302f2b51e63 100 return;
eversonrosed 76:2302f2b51e63 101 }
eversonrosed 76:2302f2b51e63 102
eversonrosed 77:19cf9072bc22 103 float dt = controller.Period();
eversonrosed 77:19cf9072bc22 104 float v = robot_speed_actual(0);
eversonrosed 77:19cf9072bc22 105 float omega = robot_speed_actual(1);
eversonrosed 77:19cf9072bc22 106 theta += omega * dt;
eversonrosed 77:19cf9072bc22 107 robot_x += cos(theta) * v * dt;
eversonrosed 77:19cf9072bc22 108 robot_y -= sin(theta) * v * dt; // since theta increases clockwise
eversonrosed 76:2302f2b51e63 109
eversonrosed 67:90f378806cbe 110 wheel_speed_desired = robot_to_wheel * robot_speed_desired;
eversonrosed 67:90f378806cbe 111
eversonrosed 67:90f378806cbe 112 // smooth desired wheel_speeds
eversonrosed 73:667d568da72a 113 trajectoryPlanners[0]->incrementToVelocity(
eversonrosed 73:667d568da72a 114 wheel_speed_desired(0) / (2.0f * M_PI), controller.Period());
eversonrosed 73:667d568da72a 115 trajectoryPlanners[1]->incrementToVelocity(
eversonrosed 73:667d568da72a 116 wheel_speed_desired(1) / (2.0f * M_PI), controller.Period());
eversonrosed 73:667d568da72a 117 wheel_speed_smooth << trajectoryPlanners[0]->getVelocity(),
eversonrosed 73:667d568da72a 118 trajectoryPlanners[1]->getVelocity();
eversonrosed 67:90f378806cbe 119
eversonrosed 67:90f378806cbe 120 // command speedController objects
eversonrosed 67:90f378806cbe 121 speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0));
eversonrosed 67:90f378806cbe 122 speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1));
seas726 49:7da71f479dac 123 }
seas726 49:7da71f479dac 124
eversonrosed 60:85a40e69ced6 125 void Robot::Initial() {
eversonrosed 60:85a40e69ced6 126 // initialize the robot.
eversonrosed 72:9325748d2d02 127 enable_motors = 0;
eversonrosed 60:85a40e69ced6 128 // motors_enabled = false;
eversonrosed 60:85a40e69ced6 129 robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
eversonrosed 60:85a40e69ced6 130 robot_speed_desired(1) = 0.0f;
seas726 49:7da71f479dac 131
eversonrosed 60:85a40e69ced6 132 if (controller.GetTurnedOn()) // check to see if blue button is toggled
eversonrosed 60:85a40e69ced6 133 {
eversonrosed 72:9325748d2d02 134 enable_motors = 1;
eversonrosed 60:85a40e69ced6 135 state = FOLLOWING_LINE;
eversonrosed 60:85a40e69ced6 136 }
seas726 49:7da71f479dac 137 }
seas726 49:7da71f479dac 138
eversonrosed 60:85a40e69ced6 139 void Robot::Idle() {
eversonrosed 72:9325748d2d02 140 enable_motors = 0;
eversonrosed 60:85a40e69ced6 141 robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
eversonrosed 60:85a40e69ced6 142 robot_speed_desired(1) = 0.0f;
eversonrosed 73:667d568da72a 143 if (controller.GetTurnedOn()) {
eversonrosed 72:9325748d2d02 144 enable_motors = 1;
eversonrosed 72:9325748d2d02 145 state = FOLLOWING_LINE;
eversonrosed 72:9325748d2d02 146 }
seas726 49:7da71f479dac 147 }
seas726 49:7da71f479dac 148
seas726 56:3fce0a9bb6df 149 void Robot::FollowingLine() // Updates once per cycle.
seas726 49:7da71f479dac 150 {
eversonrosed 73:667d568da72a 151 uint8_t raw = line_sensor.getRaw();
eversonrosed 76:2302f2b51e63 152 // if ((raw & 0xF0) == 0xF0) {
eversonrosed 76:2302f2b51e63 153 // state = LEFT_TURN;
eversonrosed 77:19cf9072bc22 154 // target_x = robot_x + 40e-3;
eversonrosed 76:2302f2b51e63 155 // target_theta = theta + M_PI / 2;
eversonrosed 76:2302f2b51e63 156 // return;
eversonrosed 76:2302f2b51e63 157 // } else if ((raw & 0xF) == 0xF) {
eversonrosed 76:2302f2b51e63 158 // state = RIGHT_TURN;
eversonrosed 77:19cf9072bc22 159 // target_x = robot_x + 40e-3;
eversonrosed 76:2302f2b51e63 160 // target_theta = theta - M_PI / 2;
eversonrosed 76:2302f2b51e63 161 // return;
eversonrosed 76:2302f2b51e63 162 // }
eversonrosed 74:76c7a805f63d 163 float angle = ComputeAngle(raw, previous_error_value);
eversonrosed 60:85a40e69ced6 164
eversonrosed 60:85a40e69ced6 165 // if(IsSharpTurn(binary_sensor_data)) { return; } // check if the sensor
eversonrosed 60:85a40e69ced6 166 // reads in any sharp turns. if so, exit the PID movement and turn sharply.
eversonrosed 60:85a40e69ced6 167 // first test PID movement. it is possible that PID movement works just as
eversonrosed 60:85a40e69ced6 168 // well.
eversonrosed 60:85a40e69ced6 169
eversonrosed 73:667d568da72a 170 PID_Move(angle); // move the robot smoothly with error
eversonrosed 73:667d568da72a 171 // calculation and stuff?
seas726 56:3fce0a9bb6df 172 }
seas726 56:3fce0a9bb6df 173
eversonrosed 77:19cf9072bc22 174 void Robot::MoveToTarget()
eversonrosed 77:19cf9072bc22 175 {
eversonrosed 77:19cf9072bc22 176 float dx = target_x - robot_x;
eversonrosed 77:19cf9072bc22 177 float dy = target_y - robot_y;
eversonrosed 77:19cf9072bc22 178 float motion_angle = -atan2(dy, dx);
eversonrosed 77:19cf9072bc22 179 float diff = motion_angle - theta;
eversonrosed 77:19cf9072bc22 180 if (abs(robot_x - target_x) <= 0.005 && abs(robot_y - target_y) <= 0.005) {
eversonrosed 77:19cf9072bc22 181 if (abs(theta - target_theta) <= 0.05) {
eversonrosed 77:19cf9072bc22 182 state = FOLLOWING_LINE;
eversonrosed 77:19cf9072bc22 183 } else {
eversonrosed 77:19cf9072bc22 184 float orient = target_theta - theta;
eversonrosed 77:19cf9072bc22 185 int sign = (0 < orient) - (orient < 0);
eversonrosed 77:19cf9072bc22 186 robot_speed_desired(0) = 0.0f;
eversonrosed 77:19cf9072bc22 187 robot_speed_desired(1) = sign * ROTATIONAL_VELOCITY;
eversonrosed 77:19cf9072bc22 188 }
eversonrosed 77:19cf9072bc22 189 } else if (abs(theta - motion_angle) <= 0.05) {
eversonrosed 77:19cf9072bc22 190 robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
eversonrosed 77:19cf9072bc22 191 robot_speed_desired(1) = 0.0f;
eversonrosed 77:19cf9072bc22 192 } else {
eversonrosed 77:19cf9072bc22 193 robot_speed_desired(0) = 0.0f;
eversonrosed 77:19cf9072bc22 194 int sign = (0 < diff) - (diff < 0);
eversonrosed 77:19cf9072bc22 195 robot_speed_desired(1) = sign * ROTATIONAL_VELOCITY;
eversonrosed 76:2302f2b51e63 196 }
seas726 56:3fce0a9bb6df 197 }
seas726 56:3fce0a9bb6df 198
eversonrosed 73:667d568da72a 199 void Robot::AvoidObstacle() {
eversonrosed 70:0e9e3c6223d1 200 // TODO
eversonrosed 70:0e9e3c6223d1 201 }
eversonrosed 70:0e9e3c6223d1 202
eversonrosed 73:667d568da72a 203 void Robot::PID_Move(float errval) // for following smooth lines ONLY
seas726 56:3fce0a9bb6df 204 {
eversonrosed 60:85a40e69ced6 205 float dt = controller.Period();
eversonrosed 60:85a40e69ced6 206
eversonrosed 60:85a40e69ced6 207 integral_error += dt * errval;
seas726 49:7da71f479dac 208
eversonrosed 73:667d568da72a 209 float integral_term = k_int * integral_error;
eversonrosed 74:76c7a805f63d 210 float derivative_term = k_deriv * robot_speed_actual(1);
seas726 49:7da71f479dac 211
eversonrosed 73:667d568da72a 212 float control_input = k_prop * errval + integral_term + derivative_term;
eversonrosed 74:76c7a805f63d 213 float multiplier = sqrt(Multiplier(errval));
eversonrosed 60:85a40e69ced6 214
eversonrosed 74:76c7a805f63d 215 robot_speed_desired(0) = multiplier * TRANSLATIONAL_VELOCITY;
eversonrosed 76:2302f2b51e63 216 robot_speed_desired(1) = control_input * ROTATIONAL_VELOCITY / multiplier;
eversonrosed 60:85a40e69ced6 217 previous_error_value = errval;
eversonrosed 60:85a40e69ced6 218
eversonrosed 60:85a40e69ced6 219 // Delay total_error/2. not exactly sure why.
seas726 56:3fce0a9bb6df 220 }
seas726 56:3fce0a9bb6df 221
eversonrosed 60:85a40e69ced6 222 bool Robot::IsSharpTurn(int binary_sensor_data) {
eversonrosed 60:85a40e69ced6 223 return binary_sensor_data & 0b11110000 || binary_sensor_data & 0b00001111;
seas726 56:3fce0a9bb6df 224 }
seas726 56:3fce0a9bb6df 225
eversonrosed 60:85a40e69ced6 226 void Robot::PID_Delay(int ms) {
eversonrosed 60:85a40e69ced6 227 // add in delay ?
eversonrosed 60:85a40e69ced6 228 // implement
seas726 56:3fce0a9bb6df 229 }
eversonrosed 73:667d568da72a 230
eversonrosed 74:76c7a805f63d 231 static float ComputeAngle(std::uint8_t raw, float default_val) {
eversonrosed 73:667d568da72a 232 const int line_sensor_count = 8;
eversonrosed 73:667d568da72a 233 uint8_t line_sensor_bits_count;
eversonrosed 73:667d568da72a 234 std::array<float, line_sensor_count> line_sensor_weight{
eversonrosed 73:667d568da72a 235 3.5, 2.5, 1.5, 0.5, -0.5, -1.5, -2.5, -3.5}; // [m]
eversonrosed 73:667d568da72a 236 std::array<float, line_sensor_count> line_sensor_value{};
eversonrosed 73:667d568da72a 237 float line_sensor_position; // [m]
eversonrosed 73:667d568da72a 238 const float line_sensor_distance = 12.5e-3; // [m]
eversonrosed 73:667d568da72a 239
eversonrosed 73:667d568da72a 240 // fill bits into the sensor array
eversonrosed 73:667d568da72a 241 // also count the sensors detecting a black line
eversonrosed 73:667d568da72a 242 line_sensor_bits_count = 0;
eversonrosed 73:667d568da72a 243 for (int i = 0; i < line_sensor_count; i++) {
eversonrosed 73:667d568da72a 244 line_sensor_value[i] = (raw >> i) & 0x01;
eversonrosed 73:667d568da72a 245 line_sensor_bits_count += line_sensor_value[i];
eversonrosed 73:667d568da72a 246 }
eversonrosed 73:667d568da72a 247
eversonrosed 73:667d568da72a 248 // getting the line position as a weighted mean
eversonrosed 74:76c7a805f63d 249 if (line_sensor_bits_count == 8) {
eversonrosed 74:76c7a805f63d 250 return default_val;
eversonrosed 74:76c7a805f63d 251 } else if (line_sensor_bits_count > 0) {
eversonrosed 74:76c7a805f63d 252 for (int i = 0; i < line_sensor_count; i++) {
eversonrosed 73:667d568da72a 253 line_sensor_position += line_sensor_value[i] * line_sensor_weight[i] /
eversonrosed 73:667d568da72a 254 float(line_sensor_bits_count);
eversonrosed 73:667d568da72a 255 }
eversonrosed 76:2302f2b51e63 256 } else { // line_sensor_bits_count = 0
eversonrosed 76:2302f2b51e63 257 return default_val;
eversonrosed 73:667d568da72a 258 }
eversonrosed 73:667d568da72a 259 line_sensor_position *= line_sensor_distance;
eversonrosed 73:667d568da72a 260
eversonrosed 73:667d568da72a 261 float line_sensor_angle; // [rad]
eversonrosed 73:667d568da72a 262 const float line_sensor_center_distance = 40e-3; // [m] Set this parameter yourself
eversonrosed 73:667d568da72a 263
eversonrosed 73:667d568da72a 264 // getting an angle out of the position
eversonrosed 73:667d568da72a 265 line_sensor_angle = atan2(line_sensor_position, line_sensor_center_distance);
eversonrosed 73:667d568da72a 266 return line_sensor_angle;
eversonrosed 73:667d568da72a 267 }
eversonrosed 74:76c7a805f63d 268
eversonrosed 74:76c7a805f63d 269 static float Multiplier(float angle)
eversonrosed 74:76c7a805f63d 270 {
eversonrosed 76:2302f2b51e63 271 return 1 / (1 + 1.5 * angle * angle);
eversonrosed 74:76c7a805f63d 272 }