File content as of revision 9:01c17b286a99:

#include "DriveController.h"

#define MOTOR_SCALE 0.5
#define CORRECTION_SCALE 0.09

DriveController::DriveController() : debugLog("log.txt"), i2c(PTC2, PTC1), wheelSpeed1(PTB0), wheelSpeed2(PTB1), wheelSpeed3(PTB2),
    wheelSpeed4(PTB3), wheelDirection1(PTE20), wheelDirection2(PTE21), wheelDirection3(PTE22), wheelDirection4(PTE23),
    sensors(PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11, PTC9, PTC8, PTA5, PTA4, PTA12, PTD4, PTA2, PTA1, PTC12, PTC13,
    PTC16, PTC17, PTD1, PTD0, PTD5, PTD13)
{
    
}

void DriveController::go()
{
    debugLog << "Starting Drive Controller!" << endl;
    while(true) //test loop
    {
        move();
        rotate('R');
        rotate('R');
        move();
    }
    
    /*while(true)
    {
        getCommand();
        
        switch(command)
        {
            case 0: move(); //move forward
                break;
            case 1: move('B'); //move backward
                break;
            case 2: rotate('R'); //rotate right
                break;
            case 3: rotate('L'); //rotate left
        }
            
        sendComplete();
    }*/    
}

void DriveController::move(char direction)
{
    bool atLine = true; //tracks whether the bot is still at the starting intersection
    int error = 0;
    
    if(direction == 'B')
    {   
        debugLog << "Moving Backward" << endl;
        wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 0;
    }
    else
    {
        debugLog << "Moving Forward" << endl;
        wheelDirection1 = wheelDirection2 = wheelDirection3 = wheelDirection4 = 1;
    }
        
    do
    {
        sensors.read();
        sensors.lineDetect(sensorStates);
        
        debugLog << "Updating Sensors" << endl;
        debugLog << "    1   2   3   4   5   6   7   8" << endl;
        debugLog << "----------------------------------" << endl;
        for(int i = 0; i < 3; i++)
        {
            debugLog << i + 1 << " | ";
            for(int j = 0; j < 8; j++)
            {
                debugLog << int(sensorStates[j][i]) << "   ";
            }
            debugLog << endl;
        }
        
        if(intersection())
        {
            debugLog << "At Intersection" << endl;
            wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
        }
        else
        {
            error = calculateError();
            debugLog << "Error Calculated : " << error << endl;
            
            if(error < 0)
            {
                wheelSpeed1 = wheelSpeed3 = (1.0 - abs(error)*CORRECTION_SCALE)*MOTOR_SCALE;
                wheelSpeed2 = wheelSpeed4 = MOTOR_SCALE;
            }
            else
            {
                wheelSpeed1 = wheelSpeed3 = MOTOR_SCALE;
                wheelSpeed2 = wheelSpeed4 = (1.0 - error*CORRECTION_SCALE)*MOTOR_SCALE;
            }
            
            atLine = false;
        }       
    } while(!intersection() || atLine);
    
    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
    debugLog << "Move Complete" << endl;
    debugLog << "=====================================" << endl;
}

int DriveController::calculateError()
{
    int error;
    int bin = 0;
    
    for(int i = 7; i >= 0; i--)
    {
            bin += sensorStates[i][1]<<(7-i);
    }
    
    switch(bin)
    {
        case 1: error = 7;
            break;
        case 3: error = 6;
            break;
        case 2: error = 5;
            break;
        case 6: error = 4;
            break;
        case 4: error = 3;
            break;
        case 12: error = 2;
            break;
        case 8: error = 1;
            break;
        case 24: error = 0;
            break;
        case 16: error = -1;
            break;
        case 48: error = -2;
            break;  
        case 32: error = -3;
            break;
        case 96: error = -4;
            break;
        case 64: error = -5;
            break;
        case 192: error = -6;
            break;
        case 128: error = -7;
    }
                
    return error;
}

bool DriveController::intersection()
{
    
    // add speed bump check later
    
    return (sensorStates[0][1]&&sensorStates[1][1]&&sensorStates[2][1]&&sensorStates[3][1]) 
            || (sensorStates[4][1]&&sensorStates[5][1]&&sensorStates[6][1]&&sensorStates[7][1]);
}

void DriveController::rotate(char direction)
{
    bool atLine = true;
    
    if(direction == 'L')
    {
        wheelDirection1 = wheelDirection3 = 0;
        wheelDirection2 = wheelDirection4 = 1;
        debugLog << "Rotating Left" << endl;
    }
    else
    {
        wheelDirection1 = wheelDirection3 = 1;
        wheelDirection2 = wheelDirection3 = 0;
        debugLog << "Rotating Right" << endl;
    }
    
    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = MOTOR_SCALE;
    
    do
    {
        sensors.read();
        sensors.lineDetect(sensorStates);
        
        debugLog << "Updating Sensors" << endl;
        debugLog << "    1   2   3   4   5   6   7   8" << endl;
        debugLog << "----------------------------------" << endl;
        for(int i = 0; i < 3; i++)
        {
            debugLog << i + 1 << " | ";
            for(int j = 0; j < 8; j++)
            {
                debugLog << int(sensorStates[j][i]) << "   ";
            }
            debugLog << endl;
        }
        
        if(!intersection())
            atLine = false;
            
    } while(!intersection() || atLine);
    
    wheelSpeed1 = wheelSpeed2 = wheelSpeed3 = wheelSpeed4 = 0;
    
    debugLog << "Rotate Complete" << endl;
    debugLog << "=====================================" << endl;

}

void DriveController::getCommand()
{
    bool received = false;
    int status;
    int msg;
    
    while(!received)
    {
        status = i2c.receive();
        
        if(status == 2) //if status is WriteGeneral
        {
            msg = i2c.read();
            received = true;
        }
    }
    
    command = msg & 7;
    edge = msg & 24;
        
}

void DriveController::sendComplete()
{
    i2c.write(1);
}