![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is the project for the Old Model Robots for OU's Dr. Davis's Configurable Robots Research. This is being published so future robots can be set up easily.
Dependencies: FatFileSystem MCP3008 Motor PinDetect QTR_8A SRF05 SSD1308_128x64_I2C mbed
Diff: main.cpp
- Revision:
- 0:bcad524c1856
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Nov 01 15:57:59 2017 +0000 @@ -0,0 +1,966 @@ +//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); + } + } + + } +} + + +