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@78:d53f1d68ca65, 2022-06-01 (annotated)

Committer:
eversonrosed
Date:
Wed Jun 01 16:18:25 2022 +0200
Revision:
78:d53f1d68ca65
Parent:
77:19cf9072bc22 
have we gone anywhere in 24 hours?

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