File content as of revision 21:ee2b617cc0e6:

//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);

//Sonar
DigitalOut trig(A1);
AnalogIn echo(A2);

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

//Motors
Motor RightMotor(D6, D5, D4, 1); // pwm, fwd, rev, can break
Motor LeftMotor(D3, D7, D2, 1); // pwm, fwd, rev, can break

//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 = 200;
    //set cds to 0
    for(int i=0; i<8; i++)
        _cds[i] = 0;
    trig.write(0);
    speedLeft = 0;
    speedRight = 0;
    
    wait_ms(100);
    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]>_threshold ? 1 : 0;
}

//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;
}

void RobotModel::StopMotors(void)
{
    RightMotor.stop(1);
    LeftMotor.stop(1);
}

void RobotModel::TurnRight()
{
    RightMotor.speed(1);
    LeftMotor.stop(1);
    wait(.3875);
}
void RobotModel::TurnLeft()
{
    LeftMotor.speed(1);
    RightMotor.stop(1);
    wait(.3875);
}
void RobotModel::DriveStraight()
{
    RightMotor.speed(.5); //Right
    LeftMotor.speed(.572); //Left
}
void RobotModel::DriveStraightMode(int ModeSetting)
{
    switch(ModeSetting) {
        default:
        //Medium is default
        case 0:
            RightMotor.speed(.5); //Right
            LeftMotor.speed(.572); //Left
        //Slow
        case 1:
            RightMotor.speed(.25); //Right
            LeftMotor.speed(.2672); //Left        
        //Fast
        case 2:
            RightMotor.speed(.75); //Right
            LeftMotor.speed(.8172); //Left
        //Max
        case 3:
            RightMotor.speed(1); //Right
            LeftMotor.speed(1); //Left
        }
}

void TurnDegrees(int Degrees,int TurnDir) //Degrees to turn, TurnDir = 1 for left, 0 for right
{
    //Roughly .004 seconds turns 1 degree
    float DegreesFactor = .0043055556; //in seconds
    float TurnTime = (float)DegreesFactor*Degrees;
    if(TurnDir == 0) {
        RightMotor.stop(1);
        LeftMotor.speed(1);
        wait(TurnTime);
    } else {
        LeftMotor.stop(1);
        RightMotor.speed(1);
        wait(TurnTime);
    }
}
void RobotModel::DriveSquare()
{
    DriveStraight();
    wait(1);
    TurnLeft();
    DriveStraight();
    wait(1);
    TurnLeft();
    DriveStraight();
    wait(1);
    TurnLeft();
    DriveStraight();
    wait(1);
    TurnLeft();
}

float RobotModel::LeftSpeed(){
    return speedLeft;
}

float RobotModel::RightSpeed(){
    return speedRight;
}

//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:
            //Read line sensors
            for(int i=0; i<8; i++)
            {
                scan(i);
                _larray[i] = scan(i);
            }
        
            //Calculate Line Location
            float p = 0;
            float Gp = 0.5f;
            int width = 0;
            for(int i=0; i<8; i++)
            {
                int temp = checkLine(i);
                p += temp*i;
                width += temp>0 ? 1 : 0;
            }
            p = Gp*(p/width)/7;
            float speedLeft = 0;
            float speedRight = 0;
            if((width == 8) || (width == 7)){
                StopMotors();
                return -1;
            }else if(width == 0){
                LeftMotor.speed(Gp*.5172);
                RightMotor.speed(Gp*.5);
                return 0;
            }else{
                speedLeft = Gp-p;
                speedRight = p*Gp;
            }
            LeftMotor.speed(speedRight);
            RightMotor.speed(speedLeft*0.5f);
            return (int)(p*100);
            //OF
        case 3:
            //sonar_t.reset();
//            //ping sensor
//            trig.write(1);
//            wait_us(10);
//            trig.write(0);
//            while(echo){}
//            sonar_t.start();
//            while(!echo){}
//            sonar_t.stop();
//            float time_us = sonar_t.read_us();
            float volts = echo.read();
            if(volts <= .4){
                LeftMotor.speed(.15);
                RightMotor.speed(-.15);
            }else{
                DriveStraight();
            }
            return (int)(volts * 100);
            //LA
        case 4:
            //Scan sensors
            scan(10);
            _cds[0] = scan(10);
            scan(8);
            _cds[1] = scan(8);
            
            //Convert readings
            speedLeft = (float)(_cds[0]-200)/200;
            speedRight = (float)(_cds[1]-200)/200;
            
            //Check light levels 
            if ((speedLeft < 0) && (speedRight < 0)) {
                LeftMotor.speed(speedLeft);
                RightMotor.speed(speedRight);
            }else if(speedRight < 0){
                LeftMotor.stop(1);
                RightMotor.speed(speedRight);
            }else if (speedLeft < 0) {
                LeftMotor.speed(speedLeft);
                RightMotor.stop(1);
            }else{
                StopMotors();
            }
            return 4;
            //TRC
        case 5:
            DriveStraight();
            break;
            //WiiC
        case 6:
            DriveSquare();
            break;
    }
    return  (int)(ain.read()*100);
}