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@65:15f23db048ca, 2022-05-30 (annotated)

Committer:
eversonrosed
Date:
Mon May 30 09:52:38 2022 +0200
Branch:
a-star
Revision:
65:15f23db048ca
Parent:
63:4a9836ef3be0
Child:
66:d9f2bcf77e52
implement movement

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"
seas726 56:3fce0a9bb6df 5 #include <cstdint>
seas726 49:7da71f479dac 6 #include <cstdio>
eversonrosed 63:4a9836ef3be0 7 #include <cmath>
seas726 49:7da71f479dac 8
eversonrosed 60:85a40e69ced6 9 Robot::Robot()
eversonrosed 60:85a40e69ced6 10 : dist(PB_1), // initialize all of the physical ports
eversonrosed 60:85a40e69ced6 11 bit0(PH_1), bit1(PC_2), bit2(PC_3),
eversonrosed 60:85a40e69ced6 12 ir_sensor_0(dist, bit0, bit1, bit2, 0), // one IR sensor
eversonrosed 60:85a40e69ced6 13 i2c2(PB_9, PB_8), // line sensor
eversonrosed 60:85a40e69ced6 14 line_sensor(i2c2), pwm_M1(PA_9), // motors + encoders
eversonrosed 60:85a40e69ced6 15 pwm_M2(PA_8), encoder_M1(PA_6, PC_7), encoder_M2(PB_6, PB_7) {
eversonrosed 60:85a40e69ced6 16 // initialize all variables
eversonrosed 60:85a40e69ced6 17 wheel_to_robot << -WHEEL_RADIUS / 2.0f, WHEEL_RADIUS / 2.0f,
eversonrosed 60:85a40e69ced6 18 -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS,
eversonrosed 60:85a40e69ced6 19 -WHEEL_RADIUS / DISTANCE_BETWEEN_WHEELS; // transformation matrix
eversonrosed 60:85a40e69ced6 20 robot_to_wheel = wheel_to_robot.inverse();
seas726 56:3fce0a9bb6df 21
eversonrosed 60:85a40e69ced6 22 robot_speed_desired.setZero(); // zero out all speeds
eversonrosed 60:85a40e69ced6 23 wheel_speed_desired.setZero();
eversonrosed 60:85a40e69ced6 24 wheel_speed_smooth.setZero();
eversonrosed 60:85a40e69ced6 25 robot_speed_actual.setZero();
eversonrosed 60:85a40e69ced6 26 wheel_speed_actual.setZero();
eversonrosed 60:85a40e69ced6 27
eversonrosed 60:85a40e69ced6 28 // MOTORS + MOTION
seas726 49:7da71f479dac 29
eversonrosed 60:85a40e69ced6 30 // TRAJECTORY PLANNERS
eversonrosed 60:85a40e69ced6 31 trajectoryPlanners[0] = new Motion();
eversonrosed 60:85a40e69ced6 32 trajectoryPlanners[1] = new Motion();
seas726 49:7da71f479dac 33
eversonrosed 60:85a40e69ced6 34 trajectoryPlanners[0]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
eversonrosed 60:85a40e69ced6 35 trajectoryPlanners[1]->setProfileVelocity(MAX_MOTOR_VOLTAGE * KN / 60.0f);
eversonrosed 60:85a40e69ced6 36 trajectoryPlanners[0]->setProfileAcceleration(10.0f);
eversonrosed 60:85a40e69ced6 37 trajectoryPlanners[1]->setProfileAcceleration(10.0f);
eversonrosed 60:85a40e69ced6 38 trajectoryPlanners[0]->setProfileDeceleration(10.0f);
eversonrosed 60:85a40e69ced6 39 trajectoryPlanners[1]->setProfileDeceleration(10.0f);
seas726 49:7da71f479dac 40
eversonrosed 60:85a40e69ced6 41 // SPEED CONTROLLERS
eversonrosed 60:85a40e69ced6 42 speedControllers[0] = new SpeedController(
eversonrosed 60:85a40e69ced6 43 COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M1, encoder_M1);
eversonrosed 60:85a40e69ced6 44 speedControllers[1] = new SpeedController(
eversonrosed 60:85a40e69ced6 45 COUNTS_PER_TURN, KN, MAX_MOTOR_VOLTAGE, pwm_M2, encoder_M2);
seas726 49:7da71f479dac 46
eversonrosed 60:85a40e69ced6 47 speedControllers[0]->setSpeedCntrlGain(
eversonrosed 60:85a40e69ced6 48 0.04f); // adjust speed controller gains
eversonrosed 60:85a40e69ced6 49 speedControllers[1]->setSpeedCntrlGain(0.04f);
eversonrosed 60:85a40e69ced6 50 speedControllers[0]->setMaxAccelerationRPS(
eversonrosed 60:85a40e69ced6 51 999.0f); // adjust max. acceleration for smooth movement
eversonrosed 60:85a40e69ced6 52 speedControllers[1]->setMaxAccelerationRPS(999.0f);
seas726 49:7da71f479dac 53 }
seas726 49:7da71f479dac 54
eversonrosed 60:85a40e69ced6 55 Robot::~Robot() {
eversonrosed 60:85a40e69ced6 56 delete trajectoryPlanners[0];
eversonrosed 60:85a40e69ced6 57 delete trajectoryPlanners[1];
eversonrosed 60:85a40e69ced6 58 delete speedControllers[0];
eversonrosed 60:85a40e69ced6 59 delete speedControllers[1];
eversonrosed 54:b442660523df 60 }
eversonrosed 54:b442660523df 61
seas726 49:7da71f479dac 62 void Robot::Update() {
seas726 49:7da71f479dac 63
eversonrosed 60:85a40e69ced6 64 controller.Update();
seas726 49:7da71f479dac 65
eversonrosed 65:15f23db048ca 66 wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
eversonrosed 65:15f23db048ca 67 robot_speed_actual = Cwheel2robot * wheel_speed_actual;
eversonrosed 65:15f23db048ca 68
eversonrosed 60:85a40e69ced6 69 printf("STATE: %d \r\n", state);
eversonrosed 60:85a40e69ced6 70 switch (state) {
eversonrosed 60:85a40e69ced6 71 case INITIAL:
eversonrosed 60:85a40e69ced6 72 Initial();
eversonrosed 60:85a40e69ced6 73 break;
eversonrosed 60:85a40e69ced6 74 case IDLE:
eversonrosed 60:85a40e69ced6 75 Idle();
eversonrosed 60:85a40e69ced6 76 break;
eversonrosed 60:85a40e69ced6 77 case FOLLOWING_LINE:
eversonrosed 60:85a40e69ced6 78 FollowingLine();
eversonrosed 60:85a40e69ced6 79 break;
eversonrosed 60:85a40e69ced6 80 case RIGHT_TURN_90:
eversonrosed 60:85a40e69ced6 81 RightTurn_90();
eversonrosed 60:85a40e69ced6 82 break;
eversonrosed 60:85a40e69ced6 83 case LEFT_TURN_90:
eversonrosed 60:85a40e69ced6 84 LeftTurn_90();
eversonrosed 60:85a40e69ced6 85 break;
eversonrosed 60:85a40e69ced6 86 default:
eversonrosed 60:85a40e69ced6 87 state = IDLE; // on default, stop the car
eversonrosed 60:85a40e69ced6 88 }
eversonrosed 65:15f23db048ca 89
eversonrosed 65:15f23db048ca 90 wheel_speed_desired = Crobot2wheel * robot_speed_desired;
eversonrosed 65:15f23db048ca 91
eversonrosed 65:15f23db048ca 92 // smooth desired wheel_speeds
eversonrosed 65:15f23db048ca 93 trajectoryPlaners[0]->incrementToVelocity(wheel_speed_desired(0) / (2.0f * M_PI), main_task_period);
eversonrosed 65:15f23db048ca 94 trajectoryPlaners[1]->incrementToVelocity(wheel_speed_desired(1) / (2.0f * M_PI), main_task_period);
eversonrosed 65:15f23db048ca 95 wheel_speed_smooth << trajectoryPlaners[0]->getVelocity(), trajectoryPlaners[1]->getVelocity();
eversonrosed 65:15f23db048ca 96
eversonrosed 65:15f23db048ca 97 // command speedController objects
eversonrosed 65:15f23db048ca 98 speedControllers[0]->setDesiredSpeedRPS(wheel_speed_smooth(0));
eversonrosed 65:15f23db048ca 99 speedControllers[1]->setDesiredSpeedRPS(wheel_speed_smooth(1));
seas726 49:7da71f479dac 100 }
seas726 49:7da71f479dac 101
eversonrosed 60:85a40e69ced6 102 void Robot::Initial() {
eversonrosed 60:85a40e69ced6 103 printf("Initial State\n"); // TODO: REMOVE PRINT
eversonrosed 60:85a40e69ced6 104 // initialize the robot.
eversonrosed 60:85a40e69ced6 105 // enable_motors = 1;
eversonrosed 60:85a40e69ced6 106 // motors_enabled = false;
eversonrosed 60:85a40e69ced6 107 robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
eversonrosed 60:85a40e69ced6 108 robot_speed_desired(1) = 0.0f;
seas726 49:7da71f479dac 109
eversonrosed 60:85a40e69ced6 110 if (controller.GetTurnedOn()) // check to see if blue button is toggled
eversonrosed 60:85a40e69ced6 111 {
eversonrosed 60:85a40e69ced6 112 state = FOLLOWING_LINE;
eversonrosed 60:85a40e69ced6 113 }
seas726 49:7da71f479dac 114 }
seas726 49:7da71f479dac 115
eversonrosed 60:85a40e69ced6 116 void Robot::Idle() {
eversonrosed 60:85a40e69ced6 117 printf("Idle\n"); // TODO: REMOVE PRINT
eversonrosed 60:85a40e69ced6 118 robot_speed_desired(0) = 0.0f; // set speed and rotational velocity to zero
eversonrosed 60:85a40e69ced6 119 robot_speed_desired(1) = 0.0f;
seas726 49:7da71f479dac 120 }
seas726 49:7da71f479dac 121
seas726 56:3fce0a9bb6df 122 void Robot::FollowingLine() // Updates once per cycle.
seas726 49:7da71f479dac 123 {
eversonrosed 60:85a40e69ced6 124 if (!controller.GetTurnedOn()) {
eversonrosed 60:85a40e69ced6 125 state = IDLE;
eversonrosed 60:85a40e69ced6 126 return;
eversonrosed 60:85a40e69ced6 127 }
seas726 56:3fce0a9bb6df 128
eversonrosed 60:85a40e69ced6 129 printf("FollowingLine\n"); // TODO: REMOVE PRINT
eversonrosed 60:85a40e69ced6 130 printf("%d", line_sensor.getRaw()); // print raw line sensor data
eversonrosed 60:85a40e69ced6 131 uint8_t binary_sensor_data =
eversonrosed 60:85a40e69ced6 132 line_sensor.getRaw(); // convert line sensor data into binary
eversonrosed 60:85a40e69ced6 133 // representation of it
eversonrosed 60:85a40e69ced6 134
eversonrosed 60:85a40e69ced6 135 // if(IsSharpTurn(binary_sensor_data)) { return; } // check if the sensor
eversonrosed 60:85a40e69ced6 136 // reads in any sharp turns. if so, exit the PID movement and turn sharply.
eversonrosed 60:85a40e69ced6 137 // first test PID movement. it is possible that PID movement works just as
eversonrosed 60:85a40e69ced6 138 // well.
eversonrosed 60:85a40e69ced6 139
eversonrosed 60:85a40e69ced6 140 PID_Move(binary_sensor_data); // move the robot smoothly with error
eversonrosed 60:85a40e69ced6 141 // calculation and stuff?
seas726 56:3fce0a9bb6df 142 }
seas726 56:3fce0a9bb6df 143
eversonrosed 60:85a40e69ced6 144 void Robot::RightTurn_90() {
eversonrosed 60:85a40e69ced6 145 // count encoder values and turn until the motor has rotated ~ 90 degrees
eversonrosed 60:85a40e69ced6 146 // im actually not sure if we need this, try testing with just the PID system
eversonrosed 60:85a40e69ced6 147 // first
seas726 56:3fce0a9bb6df 148 }
seas726 56:3fce0a9bb6df 149
eversonrosed 60:85a40e69ced6 150 void Robot::LeftTurn_90() {
eversonrosed 60:85a40e69ced6 151 // count encoder values and turn until the motor has rotated ~ 90 degrees
eversonrosed 60:85a40e69ced6 152 // im actually not sure if we need this, try testing with just the PID system
eversonrosed 60:85a40e69ced6 153 // first
seas726 56:3fce0a9bb6df 154 }
seas726 56:3fce0a9bb6df 155
eversonrosed 60:85a40e69ced6 156 void Robot::PID_Move(std::uint8_t s_binary) // for following smooth lines ONLY
seas726 56:3fce0a9bb6df 157 {
seas726 56:3fce0a9bb6df 158
eversonrosed 63:4a9836ef3be0 159 // movement tracking (smooth)
eversonrosed 63:4a9836ef3be0 160 float dt = controller.Period();
eversonrosed 63:4a9836ef3be0 161 float v = robot_speed_actual(0);
eversonrosed 63:4a9836ef3be0 162 float omega = robot_speed_actual(1);
eversonrosed 63:4a9836ef3be0 163 theta += omega * dt;
eversonrosed 63:4a9836ef3be0 164 float cos = cosf(theta);
eversonrosed 63:4a9836ef3be0 165 float sin = sinf(theta);
eversonrosed 63:4a9836ef3be0 166 robot_x += v * dt * cos;
eversonrosed 63:4a9836ef3be0 167 robot_y += v * dt * sin;
eversonrosed 63:4a9836ef3be0 168
eversonrosed 60:85a40e69ced6 169 float errval = 0;
seas726 56:3fce0a9bb6df 170
eversonrosed 60:85a40e69ced6 171 if (s_binary & 0b00000001)
eversonrosed 60:85a40e69ced6 172 errval += error_angles[3];
eversonrosed 60:85a40e69ced6 173 else if (s_binary & 0b00000010)
eversonrosed 60:85a40e69ced6 174 errval += error_angles[2];
eversonrosed 60:85a40e69ced6 175 else if (s_binary & 0b00000100)
eversonrosed 60:85a40e69ced6 176 errval += error_angles[1];
eversonrosed 60:85a40e69ced6 177 else if (s_binary & 0b00001000)
eversonrosed 60:85a40e69ced6 178 errval += error_angles[0];
seas726 49:7da71f479dac 179
eversonrosed 60:85a40e69ced6 180 if (s_binary & 0b10000000)
eversonrosed 60:85a40e69ced6 181 errval -= error_angles[3];
eversonrosed 60:85a40e69ced6 182 else if (s_binary & 0b01000000)
eversonrosed 60:85a40e69ced6 183 errval -= error_angles[2];
eversonrosed 60:85a40e69ced6 184 else if (s_binary & 0b00100000)
eversonrosed 60:85a40e69ced6 185 errval -= error_angles[1];
eversonrosed 60:85a40e69ced6 186 else if (s_binary & 0b00010000)
eversonrosed 60:85a40e69ced6 187 errval -= error_angles[0];
seas726 56:3fce0a9bb6df 188
eversonrosed 60:85a40e69ced6 189 integral_error += dt * errval;
seas726 49:7da71f479dac 190
eversonrosed 60:85a40e69ced6 191 float proportional_term = k_prop * errval;
eversonrosed 60:85a40e69ced6 192 float integral_term = integral_error / t_int;
eversonrosed 60:85a40e69ced6 193 float derivative_term = t_deriv * (errval - previous_error_value) / dt;
seas726 49:7da71f479dac 194
eversonrosed 60:85a40e69ced6 195 float control_input =
eversonrosed 60:85a40e69ced6 196 (proportional_term + integral_term + derivative_term) / error_scale;
eversonrosed 60:85a40e69ced6 197
eversonrosed 60:85a40e69ced6 198 robot_speed_desired(0) = TRANSLATIONAL_VELOCITY;
eversonrosed 60:85a40e69ced6 199 robot_speed_desired(1) = ROTATIONAL_VELOCITY * control_input;
eversonrosed 60:85a40e69ced6 200 previous_error_value = errval;
eversonrosed 60:85a40e69ced6 201
eversonrosed 60:85a40e69ced6 202 // Delay total_error/2. not exactly sure why.
seas726 56:3fce0a9bb6df 203 }
seas726 56:3fce0a9bb6df 204
eversonrosed 60:85a40e69ced6 205 bool Robot::IsSharpTurn(int binary_sensor_data) {
eversonrosed 60:85a40e69ced6 206 return binary_sensor_data & 0b11110000 || binary_sensor_data & 0b00001111;
seas726 56:3fce0a9bb6df 207 }
seas726 56:3fce0a9bb6df 208
eversonrosed 53:bfe5eaefa695 209 void Robot::LeftTurn()
eversonrosed 53:bfe5eaefa695 210 {
eversonrosed 53:bfe5eaefa695 211 // do nothing
eversonrosed 53:bfe5eaefa695 212 }
seas726 49:7da71f479dac 213
eversonrosed 53:bfe5eaefa695 214 void Robot::RightTurn()
eversonrosed 53:bfe5eaefa695 215 {
eversonrosed 53:bfe5eaefa695 216 // do nothing
eversonrosed 53:bfe5eaefa695 217 }
seas726 49:7da71f479dac 218
eversonrosed 60:85a40e69ced6 219 void Robot::PID_Delay(int ms) {
eversonrosed 60:85a40e69ced6 220 // add in delay ?
eversonrosed 60:85a40e69ced6 221 // implement
seas726 56:3fce0a9bb6df 222 }