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@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 #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,
seas726 49:7da71f479dac 24 LEFT_TURN_90
seas726 49:7da71f479dac 25 };
seas726 49:7da71f479dac 26
seas726 49:7da71f479dac 27 MovementStates state;
seas726 49:7da71f479dac 28
seas726 49:7da71f479dac 29 // Finite state machine functions
seas726 49:7da71f479dac 30 void Initial();
seas726 49:7da71f479dac 31 void Idle();
seas726 56:3fce0a9bb6df 32 void FollowingLine(); // takes in rotational velocity?
seas726 56:3fce0a9bb6df 33 void LeftTurn_90();
seas726 56:3fce0a9bb6df 34 void RightTurn_90();
seas726 56:3fce0a9bb6df 35
seas726 56:3fce0a9bb6df 36 //PID
seas726 56:3fce0a9bb6df 37
seas726 56:3fce0a9bb6df 38 void PID_Move(std::uint8_t s_binary);
seas726 56:3fce0a9bb6df 39 void PID_Delay(int ms);
seas726 56:3fce0a9bb6df 40 bool IsSharpTurn(int b);
seas726 56:3fce0a9bb6df 41
seas726 56:3fce0a9bb6df 42 const float kP = 0.5f;
seas726 56:3fce0a9bb6df 43 const float kI = 0.5f;
seas726 56:3fce0a9bb6df 44 const float kD = 0.5f;
seas726 56:3fce0a9bb6df 45
seas726 56:3fce0a9bb6df 46 int previous_error_value;
seas726 56:3fce0a9bb6df 47 int intergal_error;
seas726 49:7da71f479dac 48
seas726 49:7da71f479dac 49 //bool motors_enabled;
seas726 49:7da71f479dac 50
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
seas726 49:7da71f479dac 66 Eigen::Matrix2f wheel_to_robot;// Transoformation 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
seas726 49:7da71f479dac 83
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
seas726 49:7da71f479dac 89 Motion* trajectoryPlanners[2]; // two directory 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
seas726 49:7da71f479dac 99 // TODO, pas in motors as seperate objects n stuff ? not sure
seas726 49:7da71f479dac 100
seas726 49:7da71f479dac 101 };
seas726 49:7da71f479dac 102
seas726 49:7da71f479dac 103 #endif