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@73:667d568da72a, 2022-05-30 (annotated)

Committer:
eversonrosed
Date:
Mon May 30 17:02:27 2022 +0200
Branch:
distance-sensor
Revision:
73:667d568da72a
Parent:
72:9325748d2d02
Child:
74:76c7a805f63d
robot completes course

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