File content as of revision 15:150ec9448efc:

#include "DriveController.h"
#include "navcontroller.h"

#define MOTOR_SCALE 0.5
#define CORRECTION_SCALE 0.15
#define Kp 0.066666666667
#define Ki 0.006666666667


DriveController::DriveController() : i2c(PTC2, PTC1), treadSpeed1(PTB0), treadSpeed2(PTB1), treadDirection1(PTE20), treadDirection2(PTE21),
    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), com1(PTC13), com2(PTC12)
{
 
}

void DriveController::go()
{
    sensors.setThreshold();
    
    // wait(10);
    
    treadSpeed1.period_us(50);  //setting pwm period for all wheels to 20khz
    treadSpeed2.period_us(50);
    
    //spinTest();
    
    /*while(true) //test loop
    {
        move();
        wait(0.5);
        move();
        wait(0.5);
        rotate('R');
        wait(0.5);
        rotate('R');
        wait(0.5);
    }*/
    
    while(true)
    {
        getCommand();
        
        switch(command)
        {
            case 'F' : move(); //move forward
                break;
            case 'B' : move('B'); //move backward
                break;
            case 'R' : rotate('R'); //rotate right
                break;
            case 'L' : rotate('L'); //rotate left
        }
            
        sendComplete();
    }  
}

void DriveController::move(char direction)
{
    bool atLine = true; //tracks whether the bot is still at the starting intersection
    double error = 0;
    
    if(direction == 'B')
    {  
        treadDirection1 = treadDirection2 = 0;
    }
    else
    {
        treadDirection1 = treadDirection2 = 1;
    }
        
    do
    {
        sensors.lineDetect(sensorStates);
        
        if(intersection())
        {
            treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
        }
        else
        {
            error = calculateError();
            
            if(error < 0)
            {
                treadSpeed1 = (1.0 - fabs(error))*MOTOR_SCALE;
                treadSpeed2 = MOTOR_SCALE;
            }
            else
            {
                treadSpeed1 = MOTOR_SCALE;
                treadSpeed2 = (1.0 - error)*MOTOR_SCALE;
            }
            
            atLine = false;
        }       
    } while(!intersection() || atLine);
    
    treadSpeed1 = treadSpeed2 = 0;
}

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

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

void DriveController::rotate(char direction)
{
    bool atLine = true;
    
    if(direction == 'L')
    {
        treadDirection1 = 0;
        treadDirection2 = 1;
    }
    else
    {
        treadDirection1 = 0;
        treadDirection2 = 1;
    }
    
    treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
    
    /*do
    {
        sensors.lineDetect(sensorStates);
        
        if(!intersection())
            atLine = false;
            
    } 
    while(!intersection() || atLine);*/
    
    wait(0.9);
    
    treadSpeed1 = treadSpeed2 = 0;
}

void DriveController::getCommand()
{
    bool received = false;
    int status;
    
    while(!received)
    {
        status = i2c.receive();
        
        if(status == I2CSlave::WriteAddressed) //if status is WriteAddressed
        {
            command = i2c.read();
            received = true;
        }
    }
}

void DriveController::spinTest()
{
    treadDirection1 = 1;
    treadDirection2 = 0;
    
    treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
    
    while(true)
        wait(1);
}