File content as of revision 7:cb07cdb35b6c:

class Robot {
    private:
    float static const distanceBetweenWheels = 0.19; //currently not used may be used later
    int static const numberOfSamples = 100; //number of samples the line voltage array will hold
    int static const numberOfSensors = 6; //how many sensors you want to use
    float AF; //attenuation factor just a number you want to minimize max speed by, obviously dont go higher than 1
    Wheel* leftWheel; //a pointer to the left wheel
    Wheel* rightWheel;
    lineSensor* sensorArray[numberOfSensors];
    PID2 controller;
    float lineVoltages[numberOfSamples];
    int lvIndex;
    int sensorNumber;
    
    Ticker updater;
    
    int endOfLineDetection;
    float Reflec;
    
    float RoboticAngularVelocity;
    
    char state;
    
    public:
    
    Robot(Wheel* LW, Wheel* RW, lineSensor* SA[]) : controller(2.0f, 0.0f, 0.0f, 0.0006f) //this controller actually does nothing atm
    {
        updater.detach();
        AF  = 0.3f; //change this value to change max speedzzzzzzzzzzzzzzzzz
        state = 'S';
        
        Reflec = 0;        
        lvIndex = 0;
        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.assignLimitAddress(rightWheel->returnMinAVptr(),leftWheel->returnMaxAVptr());
    };
    
    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)
    {
            //negative is a right turn
            if (W < 0)
            {
                rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*AF);
                leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel()+W)*AF);
            }
            else
            {
                leftWheel->adjustAngularVelocity(leftWheel->returnMaxAngularVel()*AF);
                rightWheel->adjustAngularVelocity((leftWheel->returnMaxAngularVel()-W)*AF);
            }   
    }
    
    void robotUpdates(void) //sampling rate the ticker is attached I.E the wheel speed is updated everytime this function is called
        {

                float ambientLight;
                float reading;
                ambientLight = sensorArray[sensorNumber]->calcLineVoltage();  
                sensorArray[sensorNumber]->sample();
                sensorArray[sensorNumber]->calcLineVoltage();
                switch (sensorNumber)
                {
                case 0:
                    lineVoltages[lvIndex%numberOfSamples] = ((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -400.0f);
                    break;
                case 1:
                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -200.0f);
                    break;
                case 2:
                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * -100.0f);
                    break;
                case 3:
                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 100.0f);
                    break;
                case 4: 
                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 200.0f);
                    break;
                case 5:
                    lineVoltages[lvIndex%numberOfSamples] = lineVoltages[lvIndex%numberOfSamples]+((sensorArray[sensorNumber]->returnLineVoltage() - ambientLight) * 400.0f);
                    break;
                }
                sensorNumber ++;
                if (reading <= Reflec*11) {endOfLineDetection++;}
            
                if (sensorNumber >= numberOfSensors)
                {
                    if (endOfLineDetection <= 6)
                    {
                        AF  = 0.3f;
                        sensorNumber = 0;
                        adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples]);
                        lvIndex++;
                    }
                    else 
                    {
                        if (AF > 0) //this will Slowly grind the buggy to a HALT to overcome line breaks and overshooting in turns -> dont want it to instantly stop
                        {
                        AF  -= 0.1f;
                        adjustAngularVelocity(lineVoltages[lvIndex%numberOfSamples-1]); //base your action on the previous measured value
                        }
                        else //well shit, you're buggy must have totally overshot the line, lets just hope while spinning it can find a line, direction not guaranteed
                        {
                        spin(); //spin like mad and hope you find a line
                        }
                    }
                }
        }
    
    void stopMovement(void)
    {
        leftWheel->adjustAngularVelocity(0);
        rightWheel->adjustAngularVelocity(0);
    }
    
    float returnLineVoltage()
    {
        return lineVoltages[lvIndex%numberOfSamples];
    }
    
    void rbtInit()
    {
    sensorArray[0]->sample();
    Reflec = sensorArray[0]->calcLineVoltage();
    controller.setInputLimits(-300.0f,300.0f);
    controller.setSetPoint(0.0f);
    updater.attach(callback(this, &Robot::robotUpdates),0.0001f);   
    }
    
    void spin()
    {
        rightWheel->adjustAngularVelocity(rightWheel->returnMaxAngularVel()*0.3f);
        leftWheel->adjustAngularVelocity((rightWheel->returnMaxAngularVel())*-0.3f);
    }
    
    void setState(char S)
    {
        state = S;
    }

};

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

*/