File content as of revision 12:8295c02d740f:

#include "robot.h"

// Maximum y value the roller can reach
//const float maxY = h + 0.5 * d + 0.5 * reach;
// Minimum y value the roller can reach
//const float minY = h + 0.5 * d - 0.5 * reach;
// Max x value for which the robot can paint a straight line
//const float maxX = 
// Min x value for which the robot can paint a straight line
//const float minX = L_min;

const float topY = h + 0.5 * d + 0.3 * reach;
const float topX = 35;

const float bottomY = h + 0.5 * d - 0.3 * reach;
const float bottomX = 35;

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
        volatile bool MOVE_BY_REFERENCE;
        volatile bool PAINT_MOVE_UP;
        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();
    
};