File content as of revision 4:208f5279143a:

class Robot {
    private:
    float static const distanceBetweenWheels = 0.19;
    int static const numberOfSamples = 100;
    int static const numberOfSensors = 6;
    //attenuation factor
    float static const AF = 1.0f;
    
    LWheel* leftWheel;
    RWheel* rightWheel;
    lineSensor* sensorArray[numberOfSensors];
    PID controller;
    float lineVoltages[numberOfSamples];
    int lvIndex;
    int sensorNumber;
    Ticker updater;
    
    float RoboticAngularVelocity;
    
    public:
    
    Robot(LWheel* LW, RWheel* RW, lineSensor* SA[]) : controller(1.0f,0.0f,0.0f,0.1f)
    {
        lvIndex = 1;
        sensorNumber = 0;
        sensorArray[0] = SA[0];
        sensorArray[1] = SA[1];
        sensorArray[2] = SA[2];
        sensorArray[3] = SA[3];
        sensorArray[4] = SA[4];
        sensorArray[5] = SA[5];
        leftWheel = LW;
        rightWheel= RW;
        
        lineVoltages[lvIndex] = 0.0f;
        //controller will output a value between +- max speed of wheels
        controller.setOutputLimits(-1.0f*AF*rightWheel->returnMaxAngularVel(),AF*(leftWheel->returnMaxAngularVel()));
        controller.setInputLimits(-1980.0f,1980.0f);
        controller.setSetPoint(0.0f);
        updater.attach(callback(this, &Robot::robotUpdates),0.1f);
    };
    
    
    float calculateTranslationalVelocity()
    {
        return ((leftWheel->returnAngularVelocity() + rightWheel->returnAngularVelocity())/2.0f);
    }
    
    //difference between angular velocities.
    void dW()
    {
        RoboticAngularVelocity = leftWheel->returnAngularVelocity() - rightWheel->returnAngularVelocity();
    }
    
    float returnRoboticAngularVelocity()
    {
        return RoboticAngularVelocity;   
    }
    
    //attempts to modify the angular velocity of the buggy
    void adjustAngularVelocity(float W)
    {
            float temp = W - RoboticAngularVelocity;
            //negative is a right turn
            if (W < 0)
            {
                //leftWheel a smaller number depending on difference
                leftWheel->adjustAngularVelocity((leftWheel->returnAngularVelocity()+(temp+(rightWheel->returnMaxAngularVel()-rightWheel->returnAngularVelocity()))*AF));
                //right Wheel on maximum
                rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF); 
            }
            else
            {
                rightWheel->adjustAngularVelocity((rightWheel->returnAngularVelocity()-(temp-(leftWheel->returnMaxAngularVel()-leftWheel->returnAngularVelocity()))*AF));
                leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
            }   
    }
    
    void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
        {
            float ambientLight;
            sensorArray[sensorNumber]->calcLineVoltage();
            ambientLight = sensorArray[sensorNumber]->returnLineVoltage(); 
            sensorArray[sensorNumber]->sample();
            switch (sensorNumber)
            {
                case 0:
                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f;
                    break;
                case 1:
                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f;
                    break;
                case 2:
                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f;
                    break;
                case 3:
                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f;
                    break;
                case 4:
                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f;
                    break;
                case 5:
                    lineVoltages[lvIndex] += (sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f;
                    break;
            }
            sensorNumber ++;
            if (sensorNumber >= 4)
            {
            sensorNumber = 1;
            adjustAngularVelocity(controller.compute(lineVoltages[lvIndex]));
            lvIndex++;
            }
        }
    
    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++;
            }
        }
    
*/
//lineVoltages[lvIndex] += 0.5f;



/*if input value is greater than the maximum value the left wheel can go, go full right TURN
        if (W > leftWheel->returnMaxAngularVel()*AF)
        {
            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()*AF);
            leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);  
        }
        else
        {

*/