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@57:8bf0b5a70065, 2022-05-30 (annotated)

Committer:
seas726
Date:
Mon May 30 08:45:32 2022 +0200
Revision:
57:8bf0b5a70065
Parent:
56:3fce0a9bb6df
Parent:
54:b442660523df
Child:
58:c9a55b0c3121
adding pID feature

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