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@74:76c7a805f63d, 2022-05-30 (annotated)

Committer:
eversonrosed
Date:
Mon May 30 20:19:56 2022 +0200
Branch:
distance-sensor
Revision:
74:76c7a805f63d
Parent:
73:667d568da72a
Child:
76:2302f2b51e63
robot works better

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
eversonrosed 73:667d568da72a 40 void PID_Move(float position);
seas726 56:3fce0a9bb6df 41 void PID_Delay(int ms);
seas726 56:3fce0a9bb6df 42 bool IsSharpTurn(int b);
seas726 56:3fce0a9bb6df 43
eversonrosed 73:667d568da72a 44 const float k_prop = 3.0f;
eversonrosed 74:76c7a805f63d 45 const float k_int = 1.5f;
eversonrosed 73:667d568da72a 46 const float k_deriv = 0.0f;
seas726 56:3fce0a9bb6df 47
eversonrosed 60:85a40e69ced6 48 float previous_error_value;
eversonrosed 60:85a40e69ced6 49 float integral_error;
seas726 49:7da71f479dac 50
seas726 49:7da71f479dac 51 /* CONSTANTS */
seas726 49:7da71f479dac 52
seas726 49:7da71f479dac 53 const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
eversonrosed 74:76c7a805f63d 54 const float TRANSLATIONAL_VELOCITY = 0.12f; // translational velocity in [m/s]
seas726 49:7da71f479dac 55 const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s]
seas726 49:7da71f479dac 56
eversonrosed 73:667d568da72a 57 const float WHEEL_RADIUS = 0.0306f;
eversonrosed 73:667d568da72a 58 const float DISTANCE_BETWEEN_WHEELS = 0.14794f;
seas726 49:7da71f479dac 59
seas726 49:7da71f479dac 60 const float MAX_MOTOR_VOLTAGE = 12.0f; // define maximum motor voltage
seas726 49:7da71f479dac 61 const float COUNTS_PER_TURN = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio
seas726 49:7da71f479dac 62 const float KN = 530.0f / 12.0f;
seas726 49:7da71f479dac 63
eversonrosed 58:c9a55b0c3121 64 Eigen::Matrix2f wheel_to_robot;// Transformation matrix
seas726 49:7da71f479dac 65 Eigen::Matrix2f robot_to_wheel;
seas726 49:7da71f479dac 66 /* HARDWARE SETUP */
seas726 49:7da71f479dac 67
seas726 49:7da71f479dac 68 //--- SENSORS ---//
seas726 49:7da71f479dac 69 AnalogIn dist; // data in from the IR sensor TODO: UNDERSTAND THIS!
seas726 49:7da71f479dac 70 DigitalOut bit0;
seas726 49:7da71f479dac 71 DigitalOut bit1;
seas726 49:7da71f479dac 72 DigitalOut bit2;
seas726 49:7da71f479dac 73 IRSensor ir_sensor_0; // one IR sensor
seas726 49:7da71f479dac 74
seas726 49:7da71f479dac 75 I2C i2c2; // not sure what this does
seas726 49:7da71f479dac 76 SensorBar line_sensor; // Something about using raw value only
seas726 49:7da71f479dac 77
seas726 49:7da71f479dac 78 /// MOTORS + MOTION //
seas726 49:7da71f479dac 79
eversonrosed 72:9325748d2d02 80 DigitalOut enable_motors;
seas726 49:7da71f479dac 81 FastPWM pwm_M1; // motor M1 is closed-loop speed controlled (angle velocity)
seas726 49:7da71f479dac 82 FastPWM pwm_M2; // motor M2 is closed-loop speed controlled (angle velocity)
seas726 49:7da71f479dac 83 EncoderCounter encoder_M1; // create encoder objects to read in the encoder counter values
seas726 49:7da71f479dac 84 EncoderCounter encoder_M2;
seas726 49:7da71f479dac 85
eversonrosed 58:c9a55b0c3121 86 Motion* trajectoryPlanners[2]; // two trajectory planners, 1 for each motor
seas726 49:7da71f479dac 87 SpeedController* speedControllers[2];
seas726 49:7da71f479dac 88
seas726 49:7da71f479dac 89 Eigen::Vector2f robot_speed_desired; // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
seas726 49:7da71f479dac 90 Eigen::Vector2f wheel_speed_desired; // Wheel speeds [w_R, w_L] in [rad/s]
seas726 49:7da71f479dac 91 Eigen::Vector2f wheel_speed_smooth; // Wheel speeds limited and smoothed
seas726 49:7da71f479dac 92 Eigen::Vector2f wheel_speed_actual; // Measured wheel speeds
seas726 49:7da71f479dac 93 Eigen::Vector2f robot_speed_actual; // Measured robot speed
seas726 49:7da71f479dac 94 };
seas726 49:7da71f479dac 95
seas726 49:7da71f479dac 96 #endif