File content as of revision 78:d53f1d68ca65:

#ifndef ROBOT
#define ROBOT

#include "I2C.h"
#include "SensorBar.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,
            // TARGETING,
            AVOIDING_OBSTACLE,
        };
        
        MovementStates state;

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

        //PID

        void PID_Move(float position);
        void PID_Delay(int ms);
        bool IsSharpTurn(int b);

        const float k_prop = 2.5f;
        const float k_int = 1.25f;
        const float k_deriv = 1.0f;

        float previous_error_value;
        float integral_error;

        /* CONSTANTS */

        const float DISTANCE_THRESHOLD = 0.2f;        // minimum allowed distance to obstacle in [m]
        const float TRANSLATIONAL_VELOCITY = 0.15f;   // translational velocity in [m/s]
        const float ROTATIONAL_VELOCITY = 1.2f;       // 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 = 0.0306f;
        const float DISTANCE_BETWEEN_WHEELS = 0.14794f;

        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 //

        DigitalOut enable_motors;
        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

        // tracking
        // float theta;
        // float target_theta;
        // float robot_x;
        // float target_x;
        // float robot_y;
        // float target_y;
};

#endif