File content as of revision 24:5c67674a40cf:

#include "robot.h"

/*
    Constants
*/

// Coordinates of the highest point the roller will go
const float topY = h + 0.5 * d + 0.3 * reach;
const float topX = 30;

// Coordinates of the lowest point the roller will go
const float bottomY = h + 0.5 * d - 0.3 * reach;
const float bottomX = 30;

/*
    Controller Class
*/

class RobotController {
    private:
        // Robot
        Robot robot;
        // Serial communication for debugging purposes
        Serial debug;
        // Ticker
        Ticker ticker;
        // Controllers
        // -- Controls direct movement for the upper arm
        PIDController upperArmController;
        // -- Controls direct movement for the lower arm
        PIDController lowerArmController;
        // -- Controls the velocity at which the upper arm compensates for the dx induced by the lower arm
        PIDController upperXController;
        // -- Controls the velocity at which the lower arm compensates for the dx induced by the upper arm
        PIDController lowerXController;
        
        // Reference arm lengths
        volatile float upperArmLengthReference;
        volatile float lowerArmLengthReference;
        // Reference coordinates for straight movement
        volatile float referenceX;
        volatile float referenceY;
        
        // Control state flags
        // -- Indicates the controller needs to move based on the reference arm lengths that have been set
        volatile bool MOVE_BY_REFERENCE;
        // -- Indicates the controller needs to move the arms to the highest location, while describing a straight line
        volatile bool PAINT_MOVE_UP;
        // -- Indicates the controller needs to move the arms to the lowest location, while describing a straight line
        volatile bool PAINT_MOVE_DOWN;
        
        // Set all flags to false
        void clearFlags();
        
        void doTick();
        
    public:
        // Constructor
        RobotController();
        // Move the roller to a x,y coordinate
        void moveTo(float x, float y);
        // Move the arms to the indicates arm lengths
        void setArmLengths(float upper, float lower);
        // Move to max y
        void goToTop();
        // Move to min y
        void goToBottom();
        // Make a straight painting movement upwards
        void paintUp();
        // Make a straight painting movement downwards
        void paintDown();
        
        // Robot queries
        bool isKilled();
        float getUpperArmLength();
        float getLowerArmLength();
        
        // Get the robot instance
        Robot* getRobot();
    
};