File content as of revision 22:8f726dc175cd:

#include "DriveController.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), orient(NORTH)
{
    sensors.setThreshold();
    wait(10);
    treadSpeed1.period_us(50);  //setting pwm period for all wheels to 20khz
    treadSpeed2.period_us(50);
}

void DriveController::move(typename ::abs_direction dir)
{
    int diff;
    bool atLine = true; //tracks whether the bot is still at the starting intersection
    double error = 0;
    
    diff=direction-orient;
    
    if(abs(diff)==3){
        diff/=-3;
    
    if(diff == 2)//back
    {  
        treadDirection1 = treadDirection2 = 0;
    }
    else if(direction == 0)//forward
    {
        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(typename ::abs_direction dir)
{
    int diff;
    bool atLine = true;
    
    diff=direction-orient
    
    if(abs(diff)==3){
        diff/=-3;
    }
    do{
        if(diff < 0)
        {
            treadDirection1 = 0;
            treadDirection2 = 1;
            diff++;
        }
        else if(diff > 0)
        {
            treadDirection1 = 0;
            treadDirection2 = 1;
            diff--;
        }
        
        treadSpeed1 = treadSpeed2 = MOTOR_SCALE;
    }while(diff!=0);
    
    /*do
    {
        sensors.lineDetect(sensorStates);
        
        if(!intersection())
            atLine = false;
            
    } 
    while(!intersection() || atLine);*/
    
    wait(0.9);
    
    treadSpeed1 = treadSpeed2 = 0;
}

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

typename ::abs_direction getOrient()
{
    return orient;
}