File content as of revision 0:bcad524c1856:

//by Austin Saunders
//FA2013 - SP2014
#include "PowerControl/EthernetPowerControl.h"
#include "mbed.h"
#include "ReceiverIR.h"
#include "TransmitterIR.h"
#include "USBHost.h"
#include "Utils.h"
#include "Motor.h"
#include "Wiimote.h"
#include "mbed_logo.h"
#include "ou_ece_logo.h"
#include "countdown.h"
#include "battery.h"
#include "SSD1308.h"
#include "MCP3008.h"
#include "QTR_8A.h"
#include "PinDetect.h"
#include "SRF05.h"

//Define Pins
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

SRF05 srf(p30, p29);

ReceiverIR ir_rx_L(p15);//Left TSOP324 38Khz IR Receiver
ReceiverIR ir_rx_R(p16);//Right TSOP324 38Khz IR Receiver

Motor RightMotor(p22,p25,p26);//Right Motor Driven with SN754410 H-Bridge Motor Driver Chip
Motor LeftMotor(p21,p23,p24);//Left Motor Driven with SN754410 H-Bridge Motor Driver Chip

AnalogIn VBatt(p17);//100k-100k voltage divider that measures (Battery Voltage)/2

AnalogIn CDS_L(p19);//Left Cadmium Sulfide Cell voltage divider to detect light levels
AnalogIn CDS_R(p20);//Right Cadmium Sulfide Cell voltage divider to detect light levels


PinDetect sw3(p7);//top momentary switch
PinDetect sw2(p6);//bottom momentary switch
PinDetect sw1(p5);//middle momentary switch

//DigitalOut LED_ON(p8);//control signal for QTR sensor LEDs

I2C i2c(p9, p10);//used for Seeed 128x64 OLED
SSD1308 oled = SSD1308(i2c, SSD1308_SA0);//Seeed 128x64 OLED

QTR_8A qtra(p8);

// PID terms
#define P_TERM 3
#define I_TERM 0
#define D_TERM 500

#define MAX .5
#define MIN 0

Ticker ticker;
Ticker ticker2;

unsigned int sensorValues[8];//used for QTR

volatile int mode;
volatile int selection;
volatile int count;
volatile float speed;
Timeout timeout;

volatile bool calibrated;
volatile bool checked;
volatile bool pressed;

volatile bool programmed;
volatile bool prog_fwd;
volatile bool prog_rev;
volatile bool prog_left;
volatile bool prog_right;
volatile bool prog_up;
volatile bool prog_down;
volatile bool flipped;
volatile bool set;

volatile float cds_l_max;
volatile float cds_r_max;
volatile float cds_l_min;
volatile float cds_r_min;

volatile float right;
volatile float left;
volatile float derivative,proportional,integral;
volatile float power;
volatile float current_position;
volatile float previous_position;

void set_motors(float l, float r)
{
    LeftMotor.speed(l);
    RightMotor.speed(r);
}


// this is called by the USB infrastructure when a wii message comes in
void wii_data(char * data)
{

    Wiimote wii;
    wii.decode(data);
    if( wii.left ) {
        set_motors(-speed,speed);
    } else if( wii.right ) {
        set_motors(speed,-speed);
    } else if( wii.up ) {
        set_motors(speed,speed);
    } else if( wii.down ) {
        set_motors(-speed,-speed);
    } else if (wii.plus) {
        if(speed < .99) {
            speed = speed + 0.01;
            oled.writeString(7,0,"speed: ");
            oled.printf("%.2f",speed);
        }
    } else if (wii.minus) {
        if(speed >.26) {
            speed = speed - 0.01;
            oled.writeString(7,0,"speed: ");
            oled.printf("%.2f",speed);
        }
    } else {
        set_motors(0,0);
    }
    float factor = wii.wheel*.015f;

    float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
    float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;

    if( wii.two ) {
        set_motors(left_factor*speed,right_factor*speed);

    }
    if( wii.one) {
        set_motors(-left_factor*speed,-right_factor*speed);
    }

}

float get_batt_volt()//reads in battery voltage from voltage divider (1/2 Vbatt) and multiplies by 2*Vbatt*3.3 (3.3 since ain is fp 0-1)
{
    float batt_volt = VBatt.read();
    batt_volt = batt_volt*6.6;
    return batt_volt;
}

float get_batt_perc()
{
    float batt_perc = get_batt_volt();
    batt_perc = batt_perc-4;
    batt_perc = batt_perc/2;
    batt_perc = batt_perc*100;
    if (batt_perc<0) {
        batt_perc = 0;
    } else if (batt_perc>100) {
        batt_perc = 100;
    }
    return batt_perc;
}

//display battery power on OLED
void display_batt()
{
    float batt_perc = get_batt_perc();
    float batt_volt = get_batt_volt();
    if(batt_perc >= 100) {
        oled.writeBitmap((uint8_t*) battery_100p);
    } else if(batt_perc >= 87.5) {
        oled.writeBitmap((uint8_t*) battery_87_5p);
    } else if(batt_perc >= 75) {
        oled.writeBitmap((uint8_t*) battery_75p);
    } else if(batt_perc >= 62.5) {
        oled.writeBitmap((uint8_t*) battery_62_5p);
    } else if(batt_perc >= 50) {
        oled.writeBitmap((uint8_t*) battery_50p);
    } else if(batt_perc >= 37.5) {
        oled.writeBitmap((uint8_t*) battery_37_5p);
    } else if(batt_perc >= 25) {
        oled.writeBitmap((uint8_t*) battery_25p);
    } else if(batt_perc >= 12.5) {
        oled.writeBitmap((uint8_t*) battery_12_5p);
    } else {
        oled.writeBitmap((uint8_t*) battery_0p);
    }
    oled.writeString(0,0,"");
    oled.printf("%.0f%%",batt_perc);
    oled.writeString(0,12,"");
    oled.printf("%1.1fV",batt_volt);

}



void set_leds(int l1, int l2, int l3, int l4)
{
    led1 = l1;
    led2 = l2;
    led3 = l3;
    led4 = l4;
}

void led_chase()
{
    set_leds(1,0,0,0);
    wait(.05);
    set_leds(0,1,0,0);
    wait(.05);
    set_leds(0,0,1,0);
    wait(.05);
    set_leds(0,0,0,1);
    wait(.05);
    set_leds(0,0,0,0);
}

void display_ou()
{
    oled.writeBitmap((uint8_t*) OU);
}

void display_mbed()
{
    oled.writeBitmap((uint8_t*) mbed_logo);
}

void display_off()
{
    oled.setDisplayOff();
}

void clear_display()
{
    oled.clearDisplay();
}

void display_mode()
{
    if(mode == 0) { //main menu
        if(selection == 1) {
            oled.setInverted(true);
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.setInverted(false);
            oled.writeString(1,0,"Wiimote Ctrl");
            oled.writeString(2,0,"Flash Light Ctrl");
            oled.writeString(3,0,"Line Following");
            oled.writeString(4,0,"Object Avoidance");
            oled.writeString(5,0,"Object Seeking");
            oled.writeString(6,0,"Display Battery");
        } else if(selection == 2) {
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.setInverted(true);
            oled.writeString(1,0,"Wiimote Ctrl");
            oled.setInverted(false);
            oled.writeString(2,0,"Flash Light Ctrl");
            oled.writeString(3,0,"Line Following");
            oled.writeString(4,0,"Object Avoidance");
            oled.writeString(5,0,"Object Seeking");
            oled.writeString(6,0,"Display Battery");
        } else if(selection == 3) {
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(1,0,"Wiimote Ctrl");
            oled.setInverted(true);
            oled.writeString(2,0,"Flash Light Ctrl");
            oled.setInverted(false);
            oled.writeString(3,0,"Line Following");
            oled.writeString(4,0,"Object Avoidance");
            oled.writeString(5,0,"Object Seeking");
            oled.writeString(6,0,"Display Battery");
        } else if(selection == 4) {
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(1,0,"Wiimote Ctrl");
            oled.writeString(2,0,"Flash Light Ctrl");
            oled.setInverted(true);
            oled.writeString(3,0,"Line Following");
            oled.setInverted(false);
            oled.writeString(4,0,"Object Avoidance");
            oled.writeString(5,0,"Object Seeking");
            oled.writeString(6,0,"Display Battery");
        } else if(selection == 5) {
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(1,0,"Wiimote Ctrl");
            oled.writeString(2,0,"Flash Light Ctrl");
            oled.writeString(3,0,"Line Following");
            oled.setInverted(true);
            oled.writeString(4,0,"Object Avoidance");
            oled.setInverted(false);
            oled.writeString(5,0,"Object Seeking");
            oled.writeString(6,0,"Display Battery");
        } else if(selection == 6) {
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(1,0,"Wiimote Ctrl");
            oled.writeString(2,0,"Flash Light Ctrl");
            oled.writeString(3,0,"Line Following");
            oled.writeString(4,0,"Object Avoidance");
            oled.setInverted(true);
            oled.writeString(5,0,"Object Seeking");
            oled.setInverted(false);
            oled.writeString(6,0,"Display Battery");
        } else {
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(1,0,"Wiimote Ctrl");
            oled.writeString(2,0,"Flash Light Ctrl");
            oled.writeString(3,0,"Line Following");
            oled.writeString(4,0,"Object Avoidance");
            oled.writeString(5,0,"Object Seeking");
            oled.setInverted(true);
            oled.writeString(6,0,"Display Battery");
            oled.setInverted(false);
        }
    } else if(mode == 1) {

        if(!programmed) {
            oled.writeString(0,0,"Program");
            oled.writeString(2,0,"Forward:");
            oled.writeString(3,0,"Reverse:");
            oled.writeString(4,0,"Left:");
            oled.writeString(5,0,"Right:");
            oled.writeString(6,0,"Speed Up:");
            oled.writeString(7,0,"Speed Dn:");
            oled.writeString(0,12,"next");
            oled.writeString(7,12,"back");
        } else {
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(7,12,"back");
            oled.writeBigChar(3,55,'3');
            wait(1);
            oled.writeBigChar(3,55,'2');
            wait(1);
            oled.writeBigChar(3,55,'1');
            wait(1);
            oled.writeBitmap((uint8_t*) big_GO);
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(7,12,"back");
            led_chase();
            led_chase();
            led_chase();
            clear_display();
            oled.writeString(0,0,"TV Remote Ctrl");
            oled.writeString(7,12,"back");
            oled.writeString(7,0,"speed: ");
            oled.printf("%.2f",speed);
        }
    } else if(mode == 2) {
        oled.writeString(0,0,"Wiimote Ctrl");
        oled.writeString(7,12,"back");

    } else if(mode == 3) {
        if(!calibrated) {
            oled.writeString(0,0,"Calibrate");
            oled.writeString(1,0,"Sensors");
            oled.writeString(0,12,"next");
            oled.writeString(7,12,"back");
        } else {
            oled.writeString(0,0,"Flash Light Ctrl");
            oled.writeString(7,12,"back");
            oled.writeBigChar(3,55,'3');
            wait(1);
            oled.writeBigChar(3,55,'2');
            wait(1);
            oled.writeBigChar(3,55,'1');
            wait(1);
            oled.writeBitmap((uint8_t*) big_GO);
            oled.writeString(0,0,"Flash Light Ctrl!");
            oled.writeString(7,12,"back");
            led_chase();
            led_chase();
            led_chase();
            clear_display();
            oled.writeString(0,0,"Flash Light Ctrl!");
            oled.writeString(7,12,"back");
        }
    } else if(mode == 4) {
        if(!calibrated) {
            oled.writeString(0,0,"Calibrate");
            oled.writeString(1,0,"Sensors");
            oled.writeString(0,12,"next");
            oled.writeString(7,12,"back");
        } else if (!checked) {
            oled.writeString(0,0,"Check");
            oled.writeString(1,0,"Calibration");
            oled.writeString(0,12,"next");
            oled.writeString(7,12,"back");
        } else {
            oled.writeString(0,0,"Line Following!");
            oled.writeString(7,12,"back");
            oled.writeBigChar(3,55,'3');
            wait(1);
            oled.writeBigChar(3,55,'2');
            wait(1);
            oled.writeBigChar(3,55,'1');
            wait(1);
            oled.writeBitmap((uint8_t*) big_GO);
            oled.writeString(0,0,"Line Following!");
            oled.writeString(7,12,"back");
            led_chase();
            led_chase();
            led_chase();
        }
    } else if(mode == 5) {
        oled.writeString(0,0,"Object Avoidance");
        oled.writeString(7,12,"back");
    } else if(mode == 6) {
        oled.writeString(0,0,"Object Seeking");
        oled.writeString(7,12,"back");
    } else if(mode == 7) {
        display_batt();
        oled.writeString(7,12,"back");
    }

}




void sw3_pressed()//top switch
{
    if(mode == 0) {
        if(selection>1) {
            selection = selection-1;
        } else {
            selection = 1;
        }
        display_mode();
    } else if(mode == 1) {
        if (!programmed) {
            programmed = true;
        }
        pressed = true;
    } else if((mode == 4)||(mode == 3)) {
        if (!calibrated) {
            calibrated = true;
        } else if (!checked) {
            checked = true;
        } else {

        }
        //clear_display();
        //display_mode();
        pressed = true;

    }
}


void sw1_pressed()//middle switch
{
    if(mode==0) {

        mode = selection;
        selection = 1;
        clear_display();
        display_mode();
    }
}

void sw2_pressed()//bottom switch
{
    if(mode ==0) {
        if(selection<7) {
            selection = selection+1;
        } else {
            selection = 7;
        }
        display_mode();
    } else if(mode == 1) {
        if (programmed) {
            programmed = false;
            prog_fwd = false;
            prog_rev = false;
            prog_left = false;
            prog_right = false;
            prog_up = false;
            prog_down = false;
        } else {
            mode = 0;
        }
        pressed = true;
    } else if(mode == 2) {
        pressed = true;
        mode = 0;
    } else if((mode == 4)||(mode ==3)) {
        if (checked) {
            checked = false;
            set_motors(0,0);
        } else if (calibrated) {
            calibrated = false;
            set_motors(0,0);
            if (mode ==3) {
                qtra.resetCalibration();
            } else {
                cds_r_max = 0;
                cds_r_min = 1;
                cds_l_max = 0;
                cds_l_min = 1;
            }

        } else {
            mode = 0;
            if (mode ==3) {
                qtra.resetCalibration();
            } else {
                cds_r_max = 0;
                cds_r_min = 1;
                cds_l_max = 0;
                cds_l_min = 1;
            }


        }
        //clear_display();
        //display_mode();
        pressed = true;

    } else if( (mode==5)||(mode==6)) {
        if (set) {
            set = false;
        }
        mode = 0;
        pressed = true;

    } else {

        mode = 0;
        clear_display();
        display_mode();
        pressed = true;

    }
}

void flash()
{
    led1 = !led1;
    led2 = !led2;
    led3 = !led3;
    led4 = !led4;
}

void flip()
{
    flipped = !flipped;
}

void connect()
{
    oled.writeString(3,0,"Connect Wiimote");
}

void connecting()
{
    oled.writeString(3,0,"Connecting.....");
    ticker.attach(&flash,.125);
}

void connected()
{
    oled.writeString(3,0,"Connected!     ");
    oled.writeString(7,0,"speed: ");
    oled.printf("%.2f",speed);
    set_leds(0,0,0,1);
    ticker.detach();
}


void Init()
{
    PHY_PowerDown();
    i2c.frequency(400000);//max freq
    USBInit();
    set_leds(0,0,0,0);
    set_motors(0,0);
    selection = 1;
    mode = 0;
    count = 0;
    speed = MAX;
    pressed = 0;

    cds_l_max = 0;
    cds_r_max = 0;
    cds_l_min = 1;
    cds_r_min = 1;

    calibrated = false;
    checked = false;
    programmed = false;
    prog_fwd = false;
    prog_rev = false;
    prog_left = false;
    prog_right = false;
    prog_up = false;
    prog_down = false;
    flipped = false;
    set = false;

    right = 0;
    left = 0;
    derivative = 0;
    proportional = 0;
    integral = 0;
    power = 0;
    current_position = 0;
    previous_position = 0;

    sw1.mode( PullUp );
    sw1.attach_deasserted( &sw1_pressed);
    sw1.setSampleFrequency();

    sw2.mode( PullUp );
    sw2.attach_deasserted( &sw2_pressed);
    sw2.setSampleFrequency();

    sw3.mode( PullUp );
    sw3.attach_deasserted( &sw3_pressed);
    sw3.setSampleFrequency();
    oled.setContrastControl(0);
    display_mbed();
    led_chase();
    led_chase();
    led_chase();
    display_ou();
    led_chase();
    led_chase();
    led_chase();
    display_batt();
    led_chase();
    led_chase();
    led_chase();
    wait(.5);
    clear_display();
    display_mode();

}


void time_motors()
{
    set_motors(0,0);
}

int main ()
{

    Init();
    //set_motors(1,1);
    RemoteIR::Format format;
    uint8_t buf[32];
    int bitcount = 0;
    uint8_t fwd= 0;
    uint8_t rev= 0;
    uint8_t lft= 0;
    uint8_t rgt= 0;
    uint8_t up= 0;
    uint8_t down= 0;

    while(1) {

        if(mode == 1) {//tv remote mode
            if(!programmed) {
                if(!prog_fwd) {
                    if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
                        bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
                        if(buf[3]!=0) {
                            fwd = buf[3];
                            oled.writeString(2,9,"");
                            oled.printf("%X",fwd);
                            prog_fwd = true;
                        }
                    }

                } else if(!prog_rev) {
                    if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
                        bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
                        if((buf[3]!=0)&&(buf[3]!=fwd)) {
                            rev = buf[3];
                            oled.writeString(3,9,"");
                            oled.printf("%X",rev);
                            prog_rev = true;
                        }
                    }
                } else if(!prog_left) {
                    if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
                        bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
                        if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)) {
                            lft = buf[3];
                            oled.writeString(4,9,"");
                            oled.printf("%X",lft);
                            prog_left = true;
                        }
                    }
                } else if(!prog_right) {
                    if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
                        bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
                        if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)) {
                            rgt = buf[3];
                            oled.writeString(5,9,"");
                            oled.printf("%X",rgt);
                            prog_right = true;
                        }
                    }
                } else if(!prog_up) {
                    if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
                        bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
                        if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)) {
                            up = buf[3];
                            oled.writeString(6,9,"");
                            oled.printf("%X",up);
                            prog_up = true;
                        }
                    }
                } else if(!prog_down) {
                    if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
                        bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
                        if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)&&(buf[3]!=up)) {
                            down = buf[3];
                            oled.writeString(7,9,"");
                            oled.printf("%X",down);
                            prog_down = true;
                        }
                    }
                }
            } else {

                if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
                    timeout.attach(&time_motors, .1);
                    bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
                    if(buf[3]==fwd) {//forward
                        set_motors(speed,speed);

                    } else if(buf[3]==rev) { //reverse
                        set_motors(-speed,-speed);

                    } else if(buf[3]==rgt) { //right
                        set_motors(speed*.75,-speed*.75);

                    } else if(buf[3]==lft) { //left
                        set_motors(-speed*.75,speed*.75);

                    } else if(buf[3]==down) {
                        if(speed > 0.3) {
                            speed = speed-.05;
                            oled.writeString(7,0,"speed: ");
                            oled.printf("%.2f",speed);
                        }

                    } else if(buf[3]==up) {
                        if(speed <1) {
                            speed = speed+.05;
                            oled.writeString(7,0,"speed: ");
                            oled.printf("%.2f",speed);
                        }
                    }
                }

            }
            if(pressed) {
                clear_display();
                display_mode();
                pressed = false;
            }
        } else if(mode == 2) {//wiimote mode
            USBLoop();//wait for commands
            if(pressed) {
                clear_display();
                display_mode();
                pressed = false;
            }
        } else if (mode == 3) {//cds
            //speed = 0.5;

            //if(!calibrated) {
            uint32_t h_sensor = 0;
            float cds_l = CDS_L.read();
            if (cds_l > cds_l_max) {
                cds_l_max = cds_l;
            } else if (cds_l < cds_l_min) {
                cds_l_min = cds_l;
            }

            float cds_r = CDS_R.read();
            if (cds_r > cds_r_max) {
                cds_r_max = cds_r;
            } else if (cds_r < cds_r_min) {
                cds_r_min = cds_r;
            }

            float den_l = cds_l_max - cds_l_min;
            float den_r = cds_r_max - cds_r_min;

            if(den_l != 0) {
                cds_l = ((cds_l - cds_l_min)/ den_l);
            }

            if(den_r != 0) {
                cds_r = ((cds_r - cds_r_min)/ den_r);
            }

            int n_loop = (1-cds_l)*32;//normalize to 32 or less (max value of sensor values is 1000)
            for(int j = 0; j < n_loop; j++) {//create 32 bit integer where 1's represent height of bar (max value -> black seen, min value -> white seen)
                h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
            }
            oled.fillDisplay((0xFF&h_sensor), 3, 3, 30, 60);//break up that 32 bit integer and look at top 8 bits
            oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 30, 60);//break up that 32 bit integer and look at next 8 bits
            oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 30, 60);//break up that 32 bit integer and look at next 8 bits
            oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 30, 60);//break up that 32 bit integer and look at bottom 8 bits
            h_sensor = 0;//reset height of sensor bar

            n_loop = (1-cds_r)*32;//normalize to 32 or less (max value of sensor values is 1000)
            for(int j = 0; j < n_loop; j++) {//create 32 bit integer where 1's represent height of bar (max value -> black seen, min value -> white seen)
                h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
            }
            oled.fillDisplay((0xFF&h_sensor), 3, 3, 70, 100);//break up that 32 bit integer and look at top 8 bits
            oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 70, 100);//break up that 32 bit integer and look at next 8 bits
            oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 70, 100);//break up that 32 bit integer and look at next 8 bits
            oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 70, 100);//break up that 32 bit integer and look at bottom 8 bits
            h_sensor = 0;//reset height of sensor bar
            //}
            if(calibrated) {
                float l_speed = (1 - cds_l)*0.5;
                if(l_speed < 0.25) {
                    l_speed = 0;
                }
                float r_speed = (1 - cds_r)*0.5;
                if(r_speed < 0.25) {
                    r_speed = 0;
                }

                LeftMotor.speed(l_speed);
                RightMotor.speed(r_speed);
            }
            if(pressed) {
                //mode = 0;
                clear_display();
                display_mode();
                pressed = false;
            }

        } else if (mode == 4) {
            if(!calibrated) {//calibrate
                qtra.calibrate();
                qtra.readCalibrated(sensorValues);
                uint32_t h_sensor = 0;

                //stupid method to make qtra sensitivity bar indication since this oled doesn't fucking have direct pixel indexing (it has page indexing where there are 8 pages(0-7) and each page is made up of 8 rows of pixels)
                for(int i = 0; i<8; i++) {//loop through 8 sensors
                    int n_loop = sensorValues[i]/32;//normalize to 32 or less (max value of sensor values is 1000)
                    for(int j = 0; j < n_loop; j++) {//create 32 bit integer where 1's represent height of bar (max value -> black seen, min value -> white seen)
                        h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
                    }
                    oled.fillDisplay((0xFF&h_sensor), 3, 3, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at top 8 bits
                    oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at next 8 bits
                    oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at next 8 bits
                    oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, (16*i), (16*(i+1)-2));//break up that 32 bit integer and look at bottom 8 bits
                    h_sensor = 0;//reset height of sensor bar
                }
            } else if(!checked) {//check line position
                int position = qtra.readLine(sensorValues);
                int pos = (position)/66;
                oled.writeString(7,0,"pos: ");
                oled.printf("%i",position);
                oled.fillDisplay(0xFF,3,6,pos,(pos+20));
                if(pos>0) {
                    oled.fillDisplay(0x00,3,6,0,pos-1);
                }
                if((pos+20)<127) {
                    oled.fillDisplay(0x00,3,6,(pos+20)+1,127);
                }
            } else {//start

                int position = qtra.readLine(sensorValues);
                //printf("%i\n",position);
                current_position = ((float)position-3500)/3500;
                proportional = current_position;

                // Compute the derivative
                derivative = current_position - previous_position;

                // Compute the integral
                integral += proportional;



                // Compute the power
                power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;


                // Compute new speeds
                right = speed-power;
                left  = speed+power;

                // limit checks
                if (right < MIN)
                    right = MIN;
                else if (right > MAX)
                    right = MAX;

                if (left < MIN)
                    left = MIN;
                else if (left > MAX)
                    left = MAX;

                // set speed
                set_motors(left,right);

                previous_position = current_position;

            }
            if(pressed) {
                clear_display();
                display_mode();
                pressed = false;
                set_motors(0,0);
            }

        } else if (mode == 6) {
            if(!set) {
                set = true;
                ticker2.attach(&flip,1);
            }
            float dist = srf.read();
            if(dist < 20) {
                set_motors(.15,.15);
            } else {
                if(flipped) {
                    set_motors(.15,-.15);
                } else {
                    set_motors(0,0);
                }
            }
            wait(.1);
            if(pressed) {
                ticker2.detach();
                clear_display();
                display_mode();
                pressed = false;
                set_motors(0,0);
            }

        } else if (mode == 5) {

            float dist = srf.read();
            if(dist > 20) {
                set_motors(.15,.15);
            } else {
                set_motors(-.15,.15);
            }
            wait(.1);
            if(pressed) {
                clear_display();
                display_mode();
                pressed = false;
                set_motors(0,0);
            }
        }

    }
}