File content as of revision 3:0d7687b6ef14:

#include "DriveController.h"

#define REFLECTION_THRESHOLD = 0

/*DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), sensor01(PTC7),
    sensor02(PTC0), sensor03(PTC3), sensor04(PTC4), sensor05(PTC5), sensor06(PTC6), sensor07(PTC10), sensor08(PTC11), 
    sensor09(PTC9), sensor10(PTC8), sensor11(PTA5), sensor12(PTA4), sensor13(PTA12), sensor14(PTD4), sensor15(PTA2),
    sensor16(PTA1), sensor17(PTC12), sensor18(PTC13), sensor19(PTC16), sensor20(PTC17), sensor21(PTD2), sensor22(PTD0),
    sensor23(PTD5), sensor24(PTD13), orientation(NORTH)
{
    command.direction = NORTH;
    command.distance = 0;
}*/

DriveController::DriveController() : wheel1(PTC7), wheel2(PTC2), wheel3(PTE29), wheel4(PTE30), orientation(NORTH)
{
    command.direction = NORTH;
    command.distance = 0;
    
    for(int i = 0; i < 24; i++)
    {
        sensors.push_back(new DigitalInOut(sensorPinNames[i]));
    }
}

DriveController::void go()
{
    while(true)
    {
        getCommand();
        
        if(currentCommand.distance != 0)
            move();
            
        sendComplete();
    }    
}

DriveController::void move()
{
    int travelled = 0;
    bool atIntersection = true;
    
    if(orientation != command.direction)
        rotate(command.direction);
        
    while(travelled < command.distance)
    {
        forward(calculateError());
        
        if(intersection() && !atIntersection)
        {
            travelled++;
            atIntersection = true;
        }
        else if(!intersection())
            atIntersection = false;
    }
    
    command.distance = 0;
}

DriveController::double calculateError()
{
    
}

DriveController::void forward(double correction)
{
    
}

DriveController::bool intersection()
{
    return (sensorStates[0]||sensorStates[8]||sensorStates[16])&&(sensorStates[1]||sensorStates[9]||sensorStates[17])
        &&(sensorStates[2]||sensorStates[10]||sensorStates[18])&&(sensorStates[4]||sensorStates[12]||sensorStates[20])
        &&(sensorStates[5]||sensorStates[13]||sensorStates[21])&&(sensorStates[6]||sensorStates[14]||sensorStates[22])
        &&(sensorStates[7]||sensorStates[15]||sensorStates[23]);
}

DriveController::void rotate(Direction diretion)
{
    
}

DriveController::void getCommand()
{
    
}

DriveControler::void readSensors()
{
    
}