File content as of revision 17:df86b0e2bb40:

//OU Configurable Robot Project
//Spring 2017
//William Bonner

#include "RobotModel.h"

//Pin definitions
//Onboard LEDs
DigitalOut led_green(LED1);
DigitalOut led_orange(LED2);
DigitalOut led_red(LED3);
DigitalOut led_blue(LED4);

//Input Pins
AnalogIn ain(A0);

//Serial
//Line Detector
SPI spi(D11,D12,D13);//SPI interface
DigitalOut cs(D10);// chip select

//Constructors/Destructors
RobotModel::~RobotModel()
{
    //#TODO
}
RobotModel::RobotModel()
{
    _mode = 0;
}

//initialize the robot's hardware
void RobotModel::init()
{
    led_green = 0;
    led_orange = 0;
    led_red = 0;
    led_blue = 0;

    //Line Detector Setup
    // Chip must be deselected
    cs = 1;
    // Setup the spi for 8 bit data, high steady state clock,
    // second edge capture, with a 1MHz clock rate
    spi.format(8,0);
    spi.frequency(1000000);
    //set line array to 0
    for(int i=0; i<8; i++)
        _larray[i] = 0;
    _threshold = 120;
    //set cds to 0
    for(int i=0; i<8; i++)
        _cds[i] = 0;
    //cs = 0;
//    //Averaging
//    spi.write(0x3C);
//    cs = 1;

    wait_ms(200);
    led_orange = 1;
    led_red = 1;
    led_blue = 1;
}

//read the indicated light sensor
int RobotModel::checkLight(int x)
{
    return _cds[x];
}

//read bit b of the line array
int RobotModel::checkLine(int b)
{
    return _larray[b];
}

//scan the ADC on channel n
int RobotModel::scan(int n)
{
    // Select the device by seting chip select low
    cs = 0;
    spi.write(0x18);
    //Scan channel N
    //Command bits -- 1 CHS3 CHS2 CHS1     CHS0 1 1 0
    //CHSX is the binary for N
    switch(n) {
        default:
        case 0:
            spi.write(0x84);
            break;
        case 1:
            spi.write(0x8c);
            break;
        case 2:
            spi.write(0x94);
            break;
        case 3:
            spi.write(0x9C);
            break;
        case 4:
            spi.write(0xA4);
            break;
        case 5:
            spi.write(0xAC);
            break;
        case 6:
            spi.write(0xB4);
            break;
        case 7:
            spi.write(0xBC);
            break;
        case 8:
            spi.write(0xC4);
            break;
        case 9:
            spi.write(0xCC);
            break;
        case 10:
            spi.write(0xD4);
            break;
    }
    int temp = spi.write(0x00) << 4;
    temp |= spi.write(0x00) >> 4;
    spi.write(0x18);
    // Deselect the device
    cs = 1;
    return temp;
}

//Motor Control
#include "motordriver.h"


Motor A(D6, D5, D4, 1); // pwm, fwd, rev, can break
Motor B(D3, D7, D2, 1); // pwm, fwd, rev, can break

void StopMotors(void)
{
    A.stop(1);
    B.stop(1);
}

void TurnRight()
{
    A.speed(1);
    B.stop(1);
    wait(.3);
}
void TurnLeft()
{
    B.speed(1);
    A.stop(1);
    wait(.3);
}
void DriveStraight()
{
    A.speed(1); //Right
    B.speed(1); //Left
}
void DriveSquare()
{
    DriveStraight();
    wait(1);
    TurnLeft();
    DriveStraight();
    wait(1);
    TurnLeft();
    DriveStraight();
    wait(1);
    TurnLeft();
    DriveStraight();
    wait(1);
    TurnLeft();
}

//update the model based on the mode
int RobotModel::update()
{
    if(_mode == 0)
        led_green = 0;
    else
        led_green = !led_green;

    switch(_mode) {
        default:
            //Reset
        case 0:
            StopMotors();
            break;
            //LF
        case 2:
            for(int i=0; i<8; i++) {
                scan(i);
                _larray[i] = scan(i);
                //LF track using m and n
                //Forget which ones work exactly
                int SpeedOnLine = .75;
                int SpeedOffLine = .5;
                int m = 1;
                int n = 2;
                int OnLineLeft = _larray[m];
                int OnLineRight = _larray[n];
                if((OnLineLeft && OnLineRight) > 200){
                    A.speed(SpeedOnLine);
                    B.speed(SpeedOnLine);
                    }
                if((OnLineLeft > 200) && (OnLineRight < 200)){
                    A.speed(SpeedOffLine);
                    B.speed(SpeedOnLine);
                    }
                if((OnLineRight > 200) && (OnLineLeft < 200)){
                    A.speed(SpeedOnLine);
                    B.speed(SpeedOffLine);
                    }
            }
            break;
            //OA
        case 3:
            break;
            //LA
        case 4:
            scan(10);
            _cds[0] = scan(10);
            scan(8);
            _cds[1] = scan(8);
            float speedLeft = (_cds[0]-200)/200;
            float speedRight = (_cds[1]-200)/200;
            if (speedLeft < 0) {
                B.speed(speedLeft);
            }
            if (speedRight < 0) {
                A.speed(speedRight);
            }
            if ((speedRight && speedLeft) >= 0) {
                StopMotors();
            }
                break;
            //TRC
        case 5:
            DriveStraight();
            break;
            //WiiC
        case 6:
            DriveSquare();
            break;
    }
    return  (int)(ain.read()*100);
}