![](/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
main.cpp
- Committer:
- DrewSchaef
- Date:
- 2017-11-01
- Revision:
- 0:bcad524c1856
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); } } } }