File content as of revision 4:ac6b2e5b240b:

#include "DriveController.h"

#define REFLECTION_THRESHOLD = 0

DriveController::DriveController() : orientation(NORTH), i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
    wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23)
{
    
    PinName sensorPins[] = {PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1,
        PTC12, PTC13, PTC16, PTC17, PTD2, PTD0, PTD5, PTD13}; 
    command.direction = NORTH;
    command.distance = 0;
    
    for(int i = 0; i < 24; i++)
    {
        sensors.push_back(DigitalInOut(sensorPins[i]));
    }    
}

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

void DriveController::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;
}

double DriveController::calculateError()
{
    return 0;
}

void DriveController::forward(double correction)
{
    
}

bool DriveController::intersection()
{
    return sensorStates[8]&&sensorStates[9]&&sensorStates[10]&&sensorStates[12]&&sensorStates[13]&&sensorStates[14]&&sensorStates[15];
}

void DriveController::rotate(Direction diretion)
{
    
}

void DriveController::getCommand()
{
    
}

void DriveController::sendComplete()
{
    
}

void DriveController::readSensors()
{
    
}