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@49:7da71f479dac, 2022-05-25 (annotated)

Committer:
seas726
Date:
Wed May 25 11:36:19 2022 +0200
Revision:
49:7da71f479dac
Child:
51:884fca7f02de
Child:
54:b442660523df
Child:
56:3fce0a9bb6df
speed control new files

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();
seas726 49:7da71f479dac 13 void Update();
seas726 49:7da71f479dac 14
seas726 49:7da71f479dac 15 private:
seas726 49:7da71f479dac 16 Controller controller; // a reference to the robots controller
seas726 49:7da71f479dac 17
seas726 49:7da71f479dac 18 enum MovementStates {
seas726 49:7da71f479dac 19 INITIAL,
seas726 49:7da71f479dac 20 IDLE,
seas726 49:7da71f479dac 21 FOLLOWING_LINE,
seas726 49:7da71f479dac 22 RIGHT_TURN_90,
seas726 49:7da71f479dac 23 LEFT_TURN_90
seas726 49:7da71f479dac 24 };
seas726 49:7da71f479dac 25
seas726 49:7da71f479dac 26 MovementStates state;
seas726 49:7da71f479dac 27
seas726 49:7da71f479dac 28 // Finite state machine functions
seas726 49:7da71f479dac 29 void Initial();
seas726 49:7da71f479dac 30 void Idle();
seas726 49:7da71f479dac 31 void FollowingLine();
seas726 49:7da71f479dac 32 void LeftTurn();
seas726 49:7da71f479dac 33 void RightTurn();
seas726 49:7da71f479dac 34
seas726 49:7da71f479dac 35 //bool motors_enabled;
seas726 49:7da71f479dac 36
seas726 49:7da71f479dac 37
seas726 49:7da71f479dac 38 /* CONSTANTS */
seas726 49:7da71f479dac 39
seas726 49:7da71f479dac 40 const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
seas726 49:7da71f479dac 41 const float TRANSLATIONAL_VELOCITY = 0.4f; // translational velocity in [m/s]
seas726 49:7da71f479dac 42 const float ROTATIONAL_VELOCITY = 1.6f; // rotational velocity in [rad/s]
seas726 49:7da71f479dac 43 const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s]
seas726 49:7da71f479dac 44
seas726 49:7da71f479dac 45 const float WHEEL_RADIUS = 1.0f;
seas726 49:7da71f479dac 46 const float DISTANCE_BETWEEN_WHEELS = 1.0f;
seas726 49:7da71f479dac 47
seas726 49:7da71f479dac 48 const float MAX_MOTOR_VOLTAGE = 12.0f; // define maximum motor voltage
seas726 49:7da71f479dac 49 const float COUNTS_PER_TURN = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio
seas726 49:7da71f479dac 50 const float KN = 530.0f / 12.0f;
seas726 49:7da71f479dac 51
seas726 49:7da71f479dac 52 Eigen::Matrix2f wheel_to_robot;// Transoformation matrix
seas726 49:7da71f479dac 53 Eigen::Matrix2f robot_to_wheel;
seas726 49:7da71f479dac 54 /* HARDWARE SETUP */
seas726 49:7da71f479dac 55
seas726 49:7da71f479dac 56 //--- SENSORS ---//
seas726 49:7da71f479dac 57 AnalogIn dist; // data in from the IR sensor TODO: UNDERSTAND THIS!
seas726 49:7da71f479dac 58 DigitalOut bit0;
seas726 49:7da71f479dac 59 DigitalOut bit1;
seas726 49:7da71f479dac 60 DigitalOut bit2;
seas726 49:7da71f479dac 61 IRSensor ir_sensor_0; // one IR sensor
seas726 49:7da71f479dac 62
seas726 49:7da71f479dac 63 I2C i2c2; // not sure what this does
seas726 49:7da71f479dac 64 SensorBar line_sensor; // Something about using raw value only
seas726 49:7da71f479dac 65
seas726 49:7da71f479dac 66
seas726 49:7da71f479dac 67 /// MOTORS + MOTION //
seas726 49:7da71f479dac 68
seas726 49:7da71f479dac 69
seas726 49:7da71f479dac 70 FastPWM pwm_M1; // motor M1 is closed-loop speed controlled (angle velocity)
seas726 49:7da71f479dac 71 FastPWM pwm_M2; // motor M2 is closed-loop speed controlled (angle velocity)
seas726 49:7da71f479dac 72 EncoderCounter encoder_M1; // create encoder objects to read in the encoder counter values
seas726 49:7da71f479dac 73 EncoderCounter encoder_M2;
seas726 49:7da71f479dac 74
seas726 49:7da71f479dac 75 Motion* trajectoryPlanners[2]; // two directory planners, 1 for each motor
seas726 49:7da71f479dac 76 SpeedController* speedControllers[2];
seas726 49:7da71f479dac 77
seas726 49:7da71f479dac 78 Eigen::Vector2f robot_speed_desired; // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
seas726 49:7da71f479dac 79 Eigen::Vector2f wheel_speed_desired; // Wheel speeds [w_R, w_L] in [rad/s]
seas726 49:7da71f479dac 80 Eigen::Vector2f wheel_speed_smooth; // Wheel speeds limited and smoothed
seas726 49:7da71f479dac 81 Eigen::Vector2f wheel_speed_actual; // Measured wheel speeds
seas726 49:7da71f479dac 82 Eigen::Vector2f robot_speed_actual; // Measured robot speed
seas726 49:7da71f479dac 83
seas726 49:7da71f479dac 84
seas726 49:7da71f479dac 85 // TODO, pas in motors as seperate objects n stuff ? not sure
seas726 49:7da71f479dac 86
seas726 49:7da71f479dac 87 };
seas726 49:7da71f479dac 88
seas726 49:7da71f479dac 89 #endif