File content as of revision 3:01b5e80d842d:

class Robot {
    private:
    float static const distanceBetweenWheels = 0.19;
    int static const numberOfSamples = 100;
    int static const numberOfSensors = 6;
    
    Wheel* leftWheel;
    Wheel* rightWheel;
    lineSensor* sensorArray[numberOfSensors];
    PID controller;
    float lineVoltages[numberOfSamples];
    int lvIndex;
    int sensorNumber;
    Ticker updater;
    
    public:
    
    Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(1,0,10,0.1)
    {
        lvIndex = 1;
        sensorArray[0] = SA[0];
        leftWheel = LW;
        rightWheel= RW;
        
        lineVoltages[0] = -16.5f;
        //controller will output a value between +- max speed of wheels
        controller.setOutputLimits(-1.0f*leftWheel->returnMaxAngularVel(),rightWheel->returnMaxAngularVel());
        controller.setInputLimits(-16.5f, 16.5f); //max limits from sensor readings
        controller.setMode(1);
        controller.setSetPoint(0);
        //based on the fact that it takes 0.2 seconds to take a reading per sensor combo and so we should update our buggy every time it takes a reading from every single sensor
        updater.attach(callback(this, &Robot::robotUpdates),1); 
    };
    
    
    float calculateTranslationalVelocity()
    {
        return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
    }
        
    float calculateAngularVelocity()
    {
        return (((leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity())/distanceBetweenWheels));
    }
    
    //attempts to modify the angular velocity of the buggy
    void adjustAngularVelocity(float W)
    {
        //if input value is greater than the maximum value the left wheel can go, go full right TURN
        if (W > leftWheel->returnMaxAngularVel())
        {
            leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel());
            rightWheel->adjustAngularVelocity(0); 
        }
        else if (W < -1.0f*rightWheel->returnMaxAngularVel()) //if input value is less than the fastest the right wheel can go in reverse go full left TURN
        {
            rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel());
            leftWheel->adjustAngularVelocity(0); 
        }
        else if (W == 0)
        {
            rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.8f);
            leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*0.8f);  
        }
        else
        {
            //i actually have no idea what is going on here anymore
            float temp = W - calculateAngularVelocity();
            if (W < 0)
            {
                leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()*0.8f-rightWheel->returnAngularVelocity()) + temp);
                rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.8f);
            }
            else
            {
                rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()*0.8f-leftWheel->returnAngularVelocity()) + temp);
                leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*0.8f);
            }
        }
        
    }
    
    void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
        {
            lineVoltages[lvIndex] += 0.5f;
            controller.setProcessValue(lineVoltages[lvIndex]);
            adjustAngularVelocity(controller.compute());
        }
    
    void stopMovement(void)
    {
        leftWheel->adjustAngularVelocity(0);
        rightWheel->adjustAngularVelocity(0);
    }

};


/*
Timeout timeToStop,
void turn(float degrees) //+ive -> right Turn -ive -> leftTurn
    {
        float ratioOfTurn = ((abs((int)degrees) % 360)/360); //returns a value 0 -> 0.9999999 which is how much turning it has to do
        if (ratioOfTurn == 0.0f) {return;}
        rightWheel->adjustAngularVelocity(0);
        leftWheel->adjustAngularVelocity(0);
        wait(0.1);
        if (degrees > 0 )
        {
        rightWheel->adjustAngularVelocity(rightWheel->maxAngularVel);
        leftWheel->adjustAngularVelocity((leftWheel->maxAngularVel)*-1.0f);
        }
        else
        {
        rightWheel->adjustAngularVelocity((rightWheel->maxAngularVel)*-1.0f);
        leftWheel->adjustAngularVelocity(leftWheel->maxAngularVel);    
        } 
            
        timeToStop.attach(callback(this, &Robot::stopMovement),ratioOfTurn*((distanceBetweenWheels*(float)PI)/(((rightWheel->maxAngularVel)*-1.0f) * 256.0f * 2.0f * (float)PI * Wheel::wheelDiameter)));
    }
    
    void travelDistance(float d)//in metres
    {
        timeToStop.attach(callback(this, &Robot::stopMovement), d/(calculateTranslationalVelocity()*(float)PI*Wheel::wheelDiameter));
    }
    
    void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
        {
            sensorArray[sensorNumber]->sample();
            
            if (sensorNumber < (numberOfSensors/2))
            {
                lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-3);
            }
            else
            }
                lineVoltages[(lvIndex%numberOfSamples)] += sensorArray[sensorNumber]->returnLineVoltage()*(sensorNumber-2);
            }
            
            sensorNumber++;
            if (sensorNumber % numberOfSensors == 0)
            {
            sensorNumber = 0;
            controller.setProcessValue(lineVoltages[(lvIndex%numberOfSamples)];
            adjustAngularVelocity(controller.compute());
            lvIndex++;
            }
        }
    
*/