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.h@72:9325748d2d02, 2022-05-30 (annotated)

Committer:
eversonrosed
Date:
Mon May 30 15:08:21 2022 +0200
Branch:
distance-sensor
Revision:
72:9325748d2d02
Parent:
70:0e9e3c6223d1
Child:
73:667d568da72a
robot movement debugging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
seas726 49:7da71f479dac 1 #ifndef ROBOT
seas726 49:7da71f479dac 2 #define ROBOT
seas726 49:7da71f479dac 3
seas726 49:7da71f479dac 4 #include "I2C.h"
seas726 49:7da71f479dac 5 #include "SensorBar.h"
seas726 49:7da71f479dac 6 #include "controller.h"
seas726 49:7da71f479dac 7
seas726 49:7da71f479dac 8
seas726 49:7da71f479dac 9 class Robot {
seas726 49:7da71f479dac 10
seas726 49:7da71f479dac 11 public:
seas726 49:7da71f479dac 12 Robot();
eversonrosed 54:b442660523df 13 ~Robot();
seas726 49:7da71f479dac 14 void Update();
seas726 49:7da71f479dac 15
seas726 49:7da71f479dac 16 private:
seas726 49:7da71f479dac 17 Controller controller; // a reference to the robots controller
seas726 49:7da71f479dac 18
seas726 49:7da71f479dac 19 enum MovementStates {
seas726 49:7da71f479dac 20 INITIAL,
seas726 49:7da71f479dac 21 IDLE,
seas726 49:7da71f479dac 22 FOLLOWING_LINE,
seas726 49:7da71f479dac 23 RIGHT_TURN_90,
eversonrosed 70:0e9e3c6223d1 24 LEFT_TURN_90,
eversonrosed 70:0e9e3c6223d1 25 AVOIDING_OBSTACLE,
seas726 49:7da71f479dac 26 };
seas726 49:7da71f479dac 27
seas726 49:7da71f479dac 28 MovementStates state;
seas726 49:7da71f479dac 29
seas726 49:7da71f479dac 30 // Finite state machine functions
seas726 49:7da71f479dac 31 void Initial();
seas726 49:7da71f479dac 32 void Idle();
seas726 56:3fce0a9bb6df 33 void FollowingLine(); // takes in rotational velocity?
seas726 56:3fce0a9bb6df 34 void LeftTurn_90();
seas726 56:3fce0a9bb6df 35 void RightTurn_90();
eversonrosed 70:0e9e3c6223d1 36 void AvoidObstacle();
seas726 56:3fce0a9bb6df 37
seas726 56:3fce0a9bb6df 38 //PID
seas726 56:3fce0a9bb6df 39
seas726 56:3fce0a9bb6df 40 void PID_Move(std::uint8_t s_binary);
seas726 56:3fce0a9bb6df 41 void PID_Delay(int ms);
seas726 56:3fce0a9bb6df 42 bool IsSharpTurn(int b);
seas726 56:3fce0a9bb6df 43
eversonrosed 60:85a40e69ced6 44 const float k_prop = 0.5f;
eversonrosed 60:85a40e69ced6 45 const float t_int = 0.5f;
eversonrosed 60:85a40e69ced6 46 const float t_deriv = 0.5f;
eversonrosed 60:85a40e69ced6 47 const float error_angles[4] = {0, 1, 2, 3};
seas726 56:3fce0a9bb6df 48
eversonrosed 60:85a40e69ced6 49 float previous_error_value;
eversonrosed 60:85a40e69ced6 50 float integral_error;
seas726 49:7da71f479dac 51
seas726 49:7da71f479dac 52 /* CONSTANTS */
seas726 49:7da71f479dac 53
seas726 49:7da71f479dac 54 const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
seas726 49:7da71f479dac 55 const float TRANSLATIONAL_VELOCITY = 0.4f; // translational velocity in [m/s]
seas726 49:7da71f479dac 56 const float ROTATIONAL_VELOCITY = 1.6f; // rotational velocity in [rad/s]
seas726 49:7da71f479dac 57 const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s]
seas726 49:7da71f479dac 58
seas726 49:7da71f479dac 59 const float WHEEL_RADIUS = 1.0f;
seas726 49:7da71f479dac 60 const float DISTANCE_BETWEEN_WHEELS = 1.0f;
seas726 49:7da71f479dac 61
seas726 49:7da71f479dac 62 const float MAX_MOTOR_VOLTAGE = 12.0f; // define maximum motor voltage
seas726 49:7da71f479dac 63 const float COUNTS_PER_TURN = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio
seas726 49:7da71f479dac 64 const float KN = 530.0f / 12.0f;
seas726 49:7da71f479dac 65
eversonrosed 58:c9a55b0c3121 66 Eigen::Matrix2f wheel_to_robot;// Transformation matrix
seas726 49:7da71f479dac 67 Eigen::Matrix2f robot_to_wheel;
seas726 49:7da71f479dac 68 /* HARDWARE SETUP */
seas726 49:7da71f479dac 69
seas726 49:7da71f479dac 70 //--- SENSORS ---//
seas726 49:7da71f479dac 71 AnalogIn dist; // data in from the IR sensor TODO: UNDERSTAND THIS!
seas726 49:7da71f479dac 72 DigitalOut bit0;
seas726 49:7da71f479dac 73 DigitalOut bit1;
seas726 49:7da71f479dac 74 DigitalOut bit2;
seas726 49:7da71f479dac 75 IRSensor ir_sensor_0; // one IR sensor
seas726 49:7da71f479dac 76
seas726 49:7da71f479dac 77 I2C i2c2; // not sure what this does
seas726 49:7da71f479dac 78 SensorBar line_sensor; // Something about using raw value only
seas726 49:7da71f479dac 79
seas726 49:7da71f479dac 80
seas726 49:7da71f479dac 81 /// MOTORS + MOTION //
seas726 49:7da71f479dac 82
eversonrosed 72:9325748d2d02 83 DigitalOut enable_motors;
seas726 49:7da71f479dac 84 FastPWM pwm_M1; // motor M1 is closed-loop speed controlled (angle velocity)
seas726 49:7da71f479dac 85 FastPWM pwm_M2; // motor M2 is closed-loop speed controlled (angle velocity)
seas726 49:7da71f479dac 86 EncoderCounter encoder_M1; // create encoder objects to read in the encoder counter values
seas726 49:7da71f479dac 87 EncoderCounter encoder_M2;
seas726 49:7da71f479dac 88
eversonrosed 58:c9a55b0c3121 89 Motion* trajectoryPlanners[2]; // two trajectory planners, 1 for each motor
seas726 49:7da71f479dac 90 SpeedController* speedControllers[2];
seas726 49:7da71f479dac 91
seas726 49:7da71f479dac 92 Eigen::Vector2f robot_speed_desired; // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
seas726 49:7da71f479dac 93 Eigen::Vector2f wheel_speed_desired; // Wheel speeds [w_R, w_L] in [rad/s]
seas726 49:7da71f479dac 94 Eigen::Vector2f wheel_speed_smooth; // Wheel speeds limited and smoothed
seas726 49:7da71f479dac 95 Eigen::Vector2f wheel_speed_actual; // Measured wheel speeds
seas726 49:7da71f479dac 96 Eigen::Vector2f robot_speed_actual; // Measured robot speed
seas726 49:7da71f479dac 97
seas726 49:7da71f479dac 98
eversonrosed 58:c9a55b0c3121 99 // TODO, pass in motors as separate objects and stuff? not sure
seas726 49:7da71f479dac 100
seas726 49:7da71f479dac 101 };
seas726 49:7da71f479dac 102
seas726 49:7da71f479dac 103 #endif