File content as of revision 63:4a9836ef3be0:

#ifndef ROBOT
#define ROBOT

#include "mbed.h"
#include "I2C.h"
#include "SensorBar.h"
#include "SDFileSystem.h"
#include "controller.h"


class Robot {

    public:
        Robot();
        ~Robot();
        void Update();

    private:
        Controller controller; // a reference to the robots controller
        
        enum MovementStates {
            INITIAL, 
            IDLE, 
            FOLLOWING_LINE,
            RIGHT_TURN_90,
            LEFT_TURN_90
        };
        
        MovementStates state;

        // Finite state machine functions
        void Initial();
        void Idle();
        void FollowingLine(); // takes in rotational velocity?
        void LeftTurn_90();
        void RightTurn_90();

        //PID

        void PID_Move(std::uint8_t s_binary);
        void PID_Delay(int ms);
        bool IsSharpTurn(int b);

        const float k_prop = 0.5f;
        const float t_int = 0.5f;
        const float t_deriv = 0.5f;
        const float error_angles[4] = {0, 1, 2, 3};
        const float error_scale = 10;

        float previous_error_value;
        float integral_error;

        //bool motors_enabled;

        // Movement tracking
        float robot_x;
        float robot_y;
        float robot_theta;

        /* CONSTANTS */

        const float DISTANCE_THRESHOLD = 0.2f;        // minimum allowed distance to obstacle in [m]
        const float TRANSLATIONAL_VELOCITY = 0.4f;    // translational velocity in [m/s]
        const float ROTATIONAL_VELOCITY = 1.6f;       // rotational velocity in [rad/s]
        const float VELOCITY_THRESHOLD = 0.05;        // velocity threshold before switching off, in [m/s] and [rad/s]

        const float WHEEL_RADIUS = 1.0f;
        const float DISTANCE_BETWEEN_WHEELS = 1.0f;

        const float MAX_MOTOR_VOLTAGE = 12.0f; // define maximum motor voltage
        const float COUNTS_PER_TURN = 64.0f * 19.0f;    // define counts per turn at gearbox end: counts/turn * gearratio
        const float KN = 530.0f / 12.0f;      

        Eigen::Matrix2f wheel_to_robot;// Transformation matrix
        Eigen::Matrix2f robot_to_wheel;
        /* HARDWARE SETUP */

        //--- SENSORS ---//
        AnalogIn dist; // data in from the IR sensor TODO: UNDERSTAND THIS!
        DigitalOut bit0;
        DigitalOut bit1;
        DigitalOut bit2;
        IRSensor ir_sensor_0;  // one IR sensor

        I2C i2c2; // not sure what this does
        SensorBar line_sensor; // Something about using raw value only


        /// MOTORS + MOTION //


        FastPWM pwm_M1;  // motor M1 is closed-loop speed controlled (angle velocity)
        FastPWM pwm_M2;  // motor M2 is closed-loop speed controlled (angle velocity)
        EncoderCounter encoder_M1; // create encoder objects to read in the encoder counter values
        EncoderCounter encoder_M2;

        Motion* trajectoryPlanners[2]; // two trajectory planners, 1 for each motor
        SpeedController* speedControllers[2];

        Eigen::Vector2f robot_speed_desired;    // Robot speed vector [x_dt, alpha_dt] in [m/s] and [rad/s]
        Eigen::Vector2f wheel_speed_desired;    // Wheel speeds [w_R, w_L] in [rad/s]
        Eigen::Vector2f wheel_speed_smooth;     // Wheel speeds limited and smoothed
        Eigen::Vector2f wheel_speed_actual;     // Measured wheel speeds
        Eigen::Vector2f robot_speed_actual;     // Measured robot speed

        // data storage

        SDFileSystem sd;
        bool sd_on;

 // TODO, pass in motors as separate objects and stuff? not sure

};

#endif