main.cpp

00001 //by Austin Saunders
00002 //FA2013 - SP2014
00003 #include "PowerControl/EthernetPowerControl.h"
00004 #include "mbed.h"
00005 #include "ReceiverIR.h"
00006 #include "TransmitterIR.h"
00007 #include "USBHost.h"
00008 #include "Utils.h"
00009 #include "Motor.h"
00010 #include "Wiimote.h"
00011 #include "mbed_logo.h"
00012 #include "ou_ece_logo.h"
00013 #include "countdown.h"
00014 #include "battery.h"
00015 #include "SSD1308.h"
00016 #include "MCP3008.h"
00017 #include "QTR_8A.h"
00018 #include "PinDetect.h"
00019 #include "SRF05.h"
00020 
00021 //Define Pins
00022 DigitalOut led1(LED1);
00023 DigitalOut led2(LED2);
00024 DigitalOut led3(LED3);
00025 DigitalOut led4(LED4);
00026 
00027 SRF05 srf(p30, p29);
00028 
00029 ReceiverIR ir_rx_L(p15);//Left TSOP324 38Khz IR Receiver
00030 ReceiverIR ir_rx_R(p16);//Right TSOP324 38Khz IR Receiver
00031 
00032 Motor RightMotor(p22,p25,p26);//Right Motor Driven with SN754410 H-Bridge Motor Driver Chip
00033 Motor LeftMotor(p21,p23,p24);//Left Motor Driven with SN754410 H-Bridge Motor Driver Chip
00034 
00035 AnalogIn VBatt(p17);//100k-100k voltage divider that measures (Battery Voltage)/2
00036 
00037 AnalogIn CDS_L(p19);//Left Cadmium Sulfide Cell voltage divider to detect light levels
00038 AnalogIn CDS_R(p20);//Right Cadmium Sulfide Cell voltage divider to detect light levels
00039 
00040 
00041 PinDetect sw3(p7);//top momentary switch
00042 PinDetect sw2(p6);//bottom momentary switch
00043 PinDetect sw1(p5);//middle momentary switch
00044 
00045 //DigitalOut LED_ON(p8);//control signal for QTR sensor LEDs
00046 
00047 I2C i2c(p9, p10);//used for Seeed 128x64 OLED
00048 SSD1308 oled = SSD1308(i2c, SSD1308_SA0);//Seeed 128x64 OLED
00049 
00050 QTR_8A qtra(p8);
00051 
00052 // PID terms
00053 #define P_TERM 3
00054 #define I_TERM 0
00055 #define D_TERM 500
00056 
00057 #define MAX .5
00058 #define MIN 0
00059 
00060 Ticker ticker;
00061 Ticker ticker2;
00062 
00063 unsigned int sensorValues[8];//used for QTR
00064 
00065 volatile int mode;
00066 volatile int selection;
00067 volatile int count;
00068 volatile float speed;
00069 Timeout timeout;
00070 
00071 volatile bool calibrated;
00072 volatile bool checked;
00073 volatile bool pressed;
00074 
00075 volatile bool programmed;
00076 volatile bool prog_fwd;
00077 volatile bool prog_rev;
00078 volatile bool prog_left;
00079 volatile bool prog_right;
00080 volatile bool prog_up;
00081 volatile bool prog_down;
00082 volatile bool flipped;
00083 volatile bool set;
00084 
00085 volatile float cds_l_max;
00086 volatile float cds_r_max;
00087 volatile float cds_l_min;
00088 volatile float cds_r_min;
00089 
00090 volatile float right;
00091 volatile float left;
00092 volatile float derivative,proportional,integral;
00093 volatile float power;
00094 volatile float current_position;
00095 volatile float previous_position;
00096 
00097 void set_motors(float l, float r)
00098 {
00099     LeftMotor.speed(l);
00100     RightMotor.speed(r);
00101 }
00102 
00103 
00104 // this is called by the USB infrastructure when a wii message comes in
00105 void wii_data(char * data)
00106 {
00107 
00108     Wiimote wii;
00109     wii.decode(data);
00110     if( wii.left ) {
00111         set_motors(-speed,speed);
00112     } else if( wii.right ) {
00113         set_motors(speed,-speed);
00114     } else if( wii.up ) {
00115         set_motors(speed,speed);
00116     } else if( wii.down ) {
00117         set_motors(-speed,-speed);
00118     } else if (wii.plus) {
00119         if(speed < .99) {
00120             speed = speed + 0.01;
00121             oled.writeString(7,0,"speed: ");
00122             oled.printf("%.2f",speed);
00123         }
00124     } else if (wii.minus) {
00125         if(speed >.26) {
00126             speed = speed - 0.01;
00127             oled.writeString(7,0,"speed: ");
00128             oled.printf("%.2f",speed);
00129         }
00130     } else {
00131         set_motors(0,0);
00132     }
00133     float factor = wii.wheel*.015f;
00134 
00135     float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
00136     float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
00137 
00138     if( wii.two ) {
00139         set_motors(left_factor*speed,right_factor*speed);
00140 
00141     }
00142     if( wii.one) {
00143         set_motors(-left_factor*speed,-right_factor*speed);
00144     }
00145 
00146 }
00147 
00148 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)
00149 {
00150     float batt_volt = VBatt.read();
00151     batt_volt = batt_volt*6.6;
00152     return batt_volt;
00153 }
00154 
00155 float get_batt_perc()
00156 {
00157     float batt_perc = get_batt_volt();
00158     batt_perc = batt_perc-4;
00159     batt_perc = batt_perc/2;
00160     batt_perc = batt_perc*100;
00161     if (batt_perc<0) {
00162         batt_perc = 0;
00163     } else if (batt_perc>100) {
00164         batt_perc = 100;
00165     }
00166     return batt_perc;
00167 }
00168 
00169 //display battery power on OLED
00170 void display_batt()
00171 {
00172     float batt_perc = get_batt_perc();
00173     float batt_volt = get_batt_volt();
00174     if(batt_perc >= 100) {
00175         oled.writeBitmap((uint8_t*) battery_100p);
00176     } else if(batt_perc >= 87.5) {
00177         oled.writeBitmap((uint8_t*) battery_87_5p);
00178     } else if(batt_perc >= 75) {
00179         oled.writeBitmap((uint8_t*) battery_75p);
00180     } else if(batt_perc >= 62.5) {
00181         oled.writeBitmap((uint8_t*) battery_62_5p);
00182     } else if(batt_perc >= 50) {
00183         oled.writeBitmap((uint8_t*) battery_50p);
00184     } else if(batt_perc >= 37.5) {
00185         oled.writeBitmap((uint8_t*) battery_37_5p);
00186     } else if(batt_perc >= 25) {
00187         oled.writeBitmap((uint8_t*) battery_25p);
00188     } else if(batt_perc >= 12.5) {
00189         oled.writeBitmap((uint8_t*) battery_12_5p);
00190     } else {
00191         oled.writeBitmap((uint8_t*) battery_0p);
00192     }
00193     oled.writeString(0,0,"");
00194     oled.printf("%.0f%%",batt_perc);
00195     oled.writeString(0,12,"");
00196     oled.printf("%1.1fV",batt_volt);
00197 
00198 }
00199 
00200 
00201 
00202 void set_leds(int l1, int l2, int l3, int l4)
00203 {
00204     led1 = l1;
00205     led2 = l2;
00206     led3 = l3;
00207     led4 = l4;
00208 }
00209 
00210 void led_chase()
00211 {
00212     set_leds(1,0,0,0);
00213     wait(.05);
00214     set_leds(0,1,0,0);
00215     wait(.05);
00216     set_leds(0,0,1,0);
00217     wait(.05);
00218     set_leds(0,0,0,1);
00219     wait(.05);
00220     set_leds(0,0,0,0);
00221 }
00222 
00223 void display_ou()
00224 {
00225     oled.writeBitmap((uint8_t*) OU);
00226 }
00227 
00228 void display_mbed()
00229 {
00230     oled.writeBitmap((uint8_t*) mbed_logo);
00231 }
00232 
00233 void display_off()
00234 {
00235     oled.setDisplayOff();
00236 }
00237 
00238 void clear_display()
00239 {
00240     oled.clearDisplay();
00241 }
00242 
00243 void display_mode()
00244 {
00245     if(mode == 0) { //main menu
00246         if(selection == 1) {
00247             oled.setInverted(true);
00248             oled.writeString(0,0,"TV Remote Ctrl");
00249             oled.setInverted(false);
00250             oled.writeString(1,0,"Wiimote Ctrl");
00251             oled.writeString(2,0,"Flash Light Ctrl");
00252             oled.writeString(3,0,"Line Following");
00253             oled.writeString(4,0,"Object Avoidance");
00254             oled.writeString(5,0,"Object Seeking");
00255             oled.writeString(6,0,"Display Battery");
00256         } else if(selection == 2) {
00257             oled.writeString(0,0,"TV Remote Ctrl");
00258             oled.setInverted(true);
00259             oled.writeString(1,0,"Wiimote Ctrl");
00260             oled.setInverted(false);
00261             oled.writeString(2,0,"Flash Light Ctrl");
00262             oled.writeString(3,0,"Line Following");
00263             oled.writeString(4,0,"Object Avoidance");
00264             oled.writeString(5,0,"Object Seeking");
00265             oled.writeString(6,0,"Display Battery");
00266         } else if(selection == 3) {
00267             oled.writeString(0,0,"TV Remote Ctrl");
00268             oled.writeString(1,0,"Wiimote Ctrl");
00269             oled.setInverted(true);
00270             oled.writeString(2,0,"Flash Light Ctrl");
00271             oled.setInverted(false);
00272             oled.writeString(3,0,"Line Following");
00273             oled.writeString(4,0,"Object Avoidance");
00274             oled.writeString(5,0,"Object Seeking");
00275             oled.writeString(6,0,"Display Battery");
00276         } else if(selection == 4) {
00277             oled.writeString(0,0,"TV Remote Ctrl");
00278             oled.writeString(1,0,"Wiimote Ctrl");
00279             oled.writeString(2,0,"Flash Light Ctrl");
00280             oled.setInverted(true);
00281             oled.writeString(3,0,"Line Following");
00282             oled.setInverted(false);
00283             oled.writeString(4,0,"Object Avoidance");
00284             oled.writeString(5,0,"Object Seeking");
00285             oled.writeString(6,0,"Display Battery");
00286         } else if(selection == 5) {
00287             oled.writeString(0,0,"TV Remote Ctrl");
00288             oled.writeString(1,0,"Wiimote Ctrl");
00289             oled.writeString(2,0,"Flash Light Ctrl");
00290             oled.writeString(3,0,"Line Following");
00291             oled.setInverted(true);
00292             oled.writeString(4,0,"Object Avoidance");
00293             oled.setInverted(false);
00294             oled.writeString(5,0,"Object Seeking");
00295             oled.writeString(6,0,"Display Battery");
00296         } else if(selection == 6) {
00297             oled.writeString(0,0,"TV Remote Ctrl");
00298             oled.writeString(1,0,"Wiimote Ctrl");
00299             oled.writeString(2,0,"Flash Light Ctrl");
00300             oled.writeString(3,0,"Line Following");
00301             oled.writeString(4,0,"Object Avoidance");
00302             oled.setInverted(true);
00303             oled.writeString(5,0,"Object Seeking");
00304             oled.setInverted(false);
00305             oled.writeString(6,0,"Display Battery");
00306         } else {
00307             oled.writeString(0,0,"TV Remote Ctrl");
00308             oled.writeString(1,0,"Wiimote Ctrl");
00309             oled.writeString(2,0,"Flash Light Ctrl");
00310             oled.writeString(3,0,"Line Following");
00311             oled.writeString(4,0,"Object Avoidance");
00312             oled.writeString(5,0,"Object Seeking");
00313             oled.setInverted(true);
00314             oled.writeString(6,0,"Display Battery");
00315             oled.setInverted(false);
00316         }
00317     } else if(mode == 1) {
00318 
00319         if(!programmed) {
00320             oled.writeString(0,0,"Program");
00321             oled.writeString(2,0,"Forward:");
00322             oled.writeString(3,0,"Reverse:");
00323             oled.writeString(4,0,"Left:");
00324             oled.writeString(5,0,"Right:");
00325             oled.writeString(6,0,"Speed Up:");
00326             oled.writeString(7,0,"Speed Dn:");
00327             oled.writeString(0,12,"next");
00328             oled.writeString(7,12,"back");
00329         } else {
00330             oled.writeString(0,0,"TV Remote Ctrl");
00331             oled.writeString(7,12,"back");
00332             oled.writeBigChar(3,55,'3');
00333             wait(1);
00334             oled.writeBigChar(3,55,'2');
00335             wait(1);
00336             oled.writeBigChar(3,55,'1');
00337             wait(1);
00338             oled.writeBitmap((uint8_t*) big_GO);
00339             oled.writeString(0,0,"TV Remote Ctrl");
00340             oled.writeString(7,12,"back");
00341             led_chase();
00342             led_chase();
00343             led_chase();
00344             clear_display();
00345             oled.writeString(0,0,"TV Remote Ctrl");
00346             oled.writeString(7,12,"back");
00347             oled.writeString(7,0,"speed: ");
00348             oled.printf("%.2f",speed);
00349         }
00350     } else if(mode == 2) {
00351         oled.writeString(0,0,"Wiimote Ctrl");
00352         oled.writeString(7,12,"back");
00353 
00354     } else if(mode == 3) {
00355         if(!calibrated) {
00356             oled.writeString(0,0,"Calibrate");
00357             oled.writeString(1,0,"Sensors");
00358             oled.writeString(0,12,"next");
00359             oled.writeString(7,12,"back");
00360         } else {
00361             oled.writeString(0,0,"Flash Light Ctrl");
00362             oled.writeString(7,12,"back");
00363             oled.writeBigChar(3,55,'3');
00364             wait(1);
00365             oled.writeBigChar(3,55,'2');
00366             wait(1);
00367             oled.writeBigChar(3,55,'1');
00368             wait(1);
00369             oled.writeBitmap((uint8_t*) big_GO);
00370             oled.writeString(0,0,"Flash Light Ctrl!");
00371             oled.writeString(7,12,"back");
00372             led_chase();
00373             led_chase();
00374             led_chase();
00375             clear_display();
00376             oled.writeString(0,0,"Flash Light Ctrl!");
00377             oled.writeString(7,12,"back");
00378         }
00379     } else if(mode == 4) {
00380         if(!calibrated) {
00381             oled.writeString(0,0,"Calibrate");
00382             oled.writeString(1,0,"Sensors");
00383             oled.writeString(0,12,"next");
00384             oled.writeString(7,12,"back");
00385         } else if (!checked) {
00386             oled.writeString(0,0,"Check");
00387             oled.writeString(1,0,"Calibration");
00388             oled.writeString(0,12,"next");
00389             oled.writeString(7,12,"back");
00390         } else {
00391             oled.writeString(0,0,"Line Following!");
00392             oled.writeString(7,12,"back");
00393             oled.writeBigChar(3,55,'3');
00394             wait(1);
00395             oled.writeBigChar(3,55,'2');
00396             wait(1);
00397             oled.writeBigChar(3,55,'1');
00398             wait(1);
00399             oled.writeBitmap((uint8_t*) big_GO);
00400             oled.writeString(0,0,"Line Following!");
00401             oled.writeString(7,12,"back");
00402             led_chase();
00403             led_chase();
00404             led_chase();
00405         }
00406     } else if(mode == 5) {
00407         oled.writeString(0,0,"Object Avoidance");
00408         oled.writeString(7,12,"back");
00409     } else if(mode == 6) {
00410         oled.writeString(0,0,"Object Seeking");
00411         oled.writeString(7,12,"back");
00412     } else if(mode == 7) {
00413         display_batt();
00414         oled.writeString(7,12,"back");
00415     }
00416 
00417 }
00418 
00419 
00420 
00421 
00422 void sw3_pressed()//top switch
00423 {
00424     if(mode == 0) {
00425         if(selection>1) {
00426             selection = selection-1;
00427         } else {
00428             selection = 1;
00429         }
00430         display_mode();
00431     } else if(mode == 1) {
00432         if (!programmed) {
00433             programmed = true;
00434         }
00435         pressed = true;
00436     } else if((mode == 4)||(mode == 3)) {
00437         if (!calibrated) {
00438             calibrated = true;
00439         } else if (!checked) {
00440             checked = true;
00441         } else {
00442 
00443         }
00444         //clear_display();
00445         //display_mode();
00446         pressed = true;
00447 
00448     }
00449 }
00450 
00451 
00452 void sw1_pressed()//middle switch
00453 {
00454     if(mode==0) {
00455 
00456         mode = selection;
00457         selection = 1;
00458         clear_display();
00459         display_mode();
00460     }
00461 }
00462 
00463 void sw2_pressed()//bottom switch
00464 {
00465     if(mode ==0) {
00466         if(selection<7) {
00467             selection = selection+1;
00468         } else {
00469             selection = 7;
00470         }
00471         display_mode();
00472     } else if(mode == 1) {
00473         if (programmed) {
00474             programmed = false;
00475             prog_fwd = false;
00476             prog_rev = false;
00477             prog_left = false;
00478             prog_right = false;
00479             prog_up = false;
00480             prog_down = false;
00481         } else {
00482             mode = 0;
00483         }
00484         pressed = true;
00485     } else if(mode == 2) {
00486         pressed = true;
00487         mode = 0;
00488     } else if((mode == 4)||(mode ==3)) {
00489         if (checked) {
00490             checked = false;
00491             set_motors(0,0);
00492         } else if (calibrated) {
00493             calibrated = false;
00494             set_motors(0,0);
00495             if (mode ==3) {
00496                 qtra.resetCalibration();
00497             } else {
00498                 cds_r_max = 0;
00499                 cds_r_min = 1;
00500                 cds_l_max = 0;
00501                 cds_l_min = 1;
00502             }
00503 
00504         } else {
00505             mode = 0;
00506             if (mode ==3) {
00507                 qtra.resetCalibration();
00508             } else {
00509                 cds_r_max = 0;
00510                 cds_r_min = 1;
00511                 cds_l_max = 0;
00512                 cds_l_min = 1;
00513             }
00514 
00515 
00516         }
00517         //clear_display();
00518         //display_mode();
00519         pressed = true;
00520 
00521     } else if( (mode==5)||(mode==6)) {
00522         if (set) {
00523             set = false;
00524         }
00525         mode = 0;
00526         pressed = true;
00527 
00528     } else {
00529 
00530         mode = 0;
00531         clear_display();
00532         display_mode();
00533         pressed = true;
00534 
00535     }
00536 }
00537 
00538 void flash()
00539 {
00540     led1 = !led1;
00541     led2 = !led2;
00542     led3 = !led3;
00543     led4 = !led4;
00544 }
00545 
00546 void flip()
00547 {
00548     flipped = !flipped;
00549 }
00550 
00551 void connect()
00552 {
00553     oled.writeString(3,0,"Connect Wiimote");
00554 }
00555 
00556 void connecting()
00557 {
00558     oled.writeString(3,0,"Connecting.....");
00559     ticker.attach(&flash,.125);
00560 }
00561 
00562 void connected()
00563 {
00564     oled.writeString(3,0,"Connected!     ");
00565     oled.writeString(7,0,"speed: ");
00566     oled.printf("%.2f",speed);
00567     set_leds(0,0,0,1);
00568     ticker.detach();
00569 }
00570 
00571 
00572 void Init()
00573 {
00574     PHY_PowerDown();
00575     i2c.frequency(400000);//max freq
00576     USBInit();
00577     set_leds(0,0,0,0);
00578     set_motors(0,0);
00579     selection = 1;
00580     mode = 0;
00581     count = 0;
00582     speed = MAX;
00583     pressed = 0;
00584 
00585     cds_l_max = 0;
00586     cds_r_max = 0;
00587     cds_l_min = 1;
00588     cds_r_min = 1;
00589 
00590     calibrated = false;
00591     checked = false;
00592     programmed = false;
00593     prog_fwd = false;
00594     prog_rev = false;
00595     prog_left = false;
00596     prog_right = false;
00597     prog_up = false;
00598     prog_down = false;
00599     flipped = false;
00600     set = false;
00601 
00602     right = 0;
00603     left = 0;
00604     derivative = 0;
00605     proportional = 0;
00606     integral = 0;
00607     power = 0;
00608     current_position = 0;
00609     previous_position = 0;
00610 
00611     sw1.mode( PullUp );
00612     sw1.attach_deasserted( &sw1_pressed);
00613     sw1.setSampleFrequency();
00614 
00615     sw2.mode( PullUp );
00616     sw2.attach_deasserted( &sw2_pressed);
00617     sw2.setSampleFrequency();
00618 
00619     sw3.mode( PullUp );
00620     sw3.attach_deasserted( &sw3_pressed);
00621     sw3.setSampleFrequency();
00622     oled.setContrastControl(0);
00623     display_mbed();
00624     led_chase();
00625     led_chase();
00626     led_chase();
00627     display_ou();
00628     led_chase();
00629     led_chase();
00630     led_chase();
00631     display_batt();
00632     led_chase();
00633     led_chase();
00634     led_chase();
00635     wait(.5);
00636     clear_display();
00637     display_mode();
00638 
00639 }
00640 
00641 
00642 void time_motors()
00643 {
00644     set_motors(0,0);
00645 }
00646 
00647 int main ()
00648 {
00649 
00650     Init();
00651     //set_motors(1,1);
00652     RemoteIR::Format format;
00653     uint8_t buf[32];
00654     int bitcount = 0;
00655     uint8_t fwd= 0;
00656     uint8_t rev= 0;
00657     uint8_t lft= 0;
00658     uint8_t rgt= 0;
00659     uint8_t up= 0;
00660     uint8_t down= 0;
00661 
00662     while(1) {
00663 
00664         if(mode == 1) {//tv remote mode
00665             if(!programmed) {
00666                 if(!prog_fwd) {
00667                     if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
00668                         bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
00669                         if(buf[3]!=0) {
00670                             fwd = buf[3];
00671                             oled.writeString(2,9,"");
00672                             oled.printf("%X",fwd);
00673                             prog_fwd = true;
00674                         }
00675                     }
00676 
00677                 } else if(!prog_rev) {
00678                     if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
00679                         bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
00680                         if((buf[3]!=0)&&(buf[3]!=fwd)) {
00681                             rev = buf[3];
00682                             oled.writeString(3,9,"");
00683                             oled.printf("%X",rev);
00684                             prog_rev = true;
00685                         }
00686                     }
00687                 } else if(!prog_left) {
00688                     if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
00689                         bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
00690                         if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)) {
00691                             lft = buf[3];
00692                             oled.writeString(4,9,"");
00693                             oled.printf("%X",lft);
00694                             prog_left = true;
00695                         }
00696                     }
00697                 } else if(!prog_right) {
00698                     if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
00699                         bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
00700                         if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)) {
00701                             rgt = buf[3];
00702                             oled.writeString(5,9,"");
00703                             oled.printf("%X",rgt);
00704                             prog_right = true;
00705                         }
00706                     }
00707                 } else if(!prog_up) {
00708                     if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
00709                         bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
00710                         if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)) {
00711                             up = buf[3];
00712                             oled.writeString(6,9,"");
00713                             oled.printf("%X",up);
00714                             prog_up = true;
00715                         }
00716                     }
00717                 } else if(!prog_down) {
00718                     if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
00719                         bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
00720                         if((buf[3]!=0)&&(buf[3]!=fwd)&&(buf[3]!=rev)&&(buf[3]!=lft)&&(buf[3]!=rgt)&&(buf[3]!=up)) {
00721                             down = buf[3];
00722                             oled.writeString(7,9,"");
00723                             oled.printf("%X",down);
00724                             prog_down = true;
00725                         }
00726                     }
00727                 }
00728             } else {
00729 
00730                 if (ir_rx_L.getState() == ReceiverIR::Received) {//if ir received, decode commands
00731                     timeout.attach(&time_motors, .1);
00732                     bitcount = ir_rx_L.getData(&format, buf, sizeof(buf) * 8);
00733                     if(buf[3]==fwd) {//forward
00734                         set_motors(speed,speed);
00735 
00736                     } else if(buf[3]==rev) { //reverse
00737                         set_motors(-speed,-speed);
00738 
00739                     } else if(buf[3]==rgt) { //right
00740                         set_motors(speed*.75,-speed*.75);
00741 
00742                     } else if(buf[3]==lft) { //left
00743                         set_motors(-speed*.75,speed*.75);
00744 
00745                     } else if(buf[3]==down) {
00746                         if(speed > 0.3) {
00747                             speed = speed-.05;
00748                             oled.writeString(7,0,"speed: ");
00749                             oled.printf("%.2f",speed);
00750                         }
00751 
00752                     } else if(buf[3]==up) {
00753                         if(speed <1) {
00754                             speed = speed+.05;
00755                             oled.writeString(7,0,"speed: ");
00756                             oled.printf("%.2f",speed);
00757                         }
00758                     }
00759                 }
00760 
00761             }
00762             if(pressed) {
00763                 clear_display();
00764                 display_mode();
00765                 pressed = false;
00766             }
00767         } else if(mode == 2) {//wiimote mode
00768             USBLoop();//wait for commands
00769             if(pressed) {
00770                 clear_display();
00771                 display_mode();
00772                 pressed = false;
00773             }
00774         } else if (mode == 3) {//cds
00775             //speed = 0.5;
00776 
00777             //if(!calibrated) {
00778             uint32_t h_sensor = 0;
00779             float cds_l = CDS_L.read();
00780             if (cds_l > cds_l_max) {
00781                 cds_l_max = cds_l;
00782             } else if (cds_l < cds_l_min) {
00783                 cds_l_min = cds_l;
00784             }
00785 
00786             float cds_r = CDS_R.read();
00787             if (cds_r > cds_r_max) {
00788                 cds_r_max = cds_r;
00789             } else if (cds_r < cds_r_min) {
00790                 cds_r_min = cds_r;
00791             }
00792 
00793             float den_l = cds_l_max - cds_l_min;
00794             float den_r = cds_r_max - cds_r_min;
00795 
00796             if(den_l != 0) {
00797                 cds_l = ((cds_l - cds_l_min)/ den_l);
00798             }
00799 
00800             if(den_r != 0) {
00801                 cds_r = ((cds_r - cds_r_min)/ den_r);
00802             }
00803 
00804             int n_loop = (1-cds_l)*32;//normalize to 32 or less (max value of sensor values is 1000)
00805             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)
00806                 h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
00807             }
00808             oled.fillDisplay((0xFF&h_sensor), 3, 3, 30, 60);//break up that 32 bit integer and look at top 8 bits
00809             oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 30, 60);//break up that 32 bit integer and look at next 8 bits
00810             oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 30, 60);//break up that 32 bit integer and look at next 8 bits
00811             oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 30, 60);//break up that 32 bit integer and look at bottom 8 bits
00812             h_sensor = 0;//reset height of sensor bar
00813 
00814             n_loop = (1-cds_r)*32;//normalize to 32 or less (max value of sensor values is 1000)
00815             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)
00816                 h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
00817             }
00818             oled.fillDisplay((0xFF&h_sensor), 3, 3, 70, 100);//break up that 32 bit integer and look at top 8 bits
00819             oled.fillDisplay((0xFF&(h_sensor>>8)), 4, 4, 70, 100);//break up that 32 bit integer and look at next 8 bits
00820             oled.fillDisplay((0xFF&(h_sensor>>16)), 5, 5, 70, 100);//break up that 32 bit integer and look at next 8 bits
00821             oled.fillDisplay((0xFF&(h_sensor>>24)), 6, 6, 70, 100);//break up that 32 bit integer and look at bottom 8 bits
00822             h_sensor = 0;//reset height of sensor bar
00823             //}
00824             if(calibrated) {
00825                 float l_speed = (1 - cds_l)*0.5;
00826                 if(l_speed < 0.25) {
00827                     l_speed = 0;
00828                 }
00829                 float r_speed = (1 - cds_r)*0.5;
00830                 if(r_speed < 0.25) {
00831                     r_speed = 0;
00832                 }
00833 
00834                 LeftMotor.speed(l_speed);
00835                 RightMotor.speed(r_speed);
00836             }
00837             if(pressed) {
00838                 //mode = 0;
00839                 clear_display();
00840                 display_mode();
00841                 pressed = false;
00842             }
00843 
00844         } else if (mode == 4) {
00845             if(!calibrated) {//calibrate
00846                 qtra.calibrate();
00847                 qtra.readCalibrated(sensorValues);
00848                 uint32_t h_sensor = 0;
00849 
00850                 //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)
00851                 for(int i = 0; i<8; i++) {//loop through 8 sensors
00852                     int n_loop = sensorValues[i]/32;//normalize to 32 or less (max value of sensor values is 1000)
00853                     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)
00854                         h_sensor = (h_sensor >> 1)|0x80000000;//shift in 1's from the right until you represent height of the bar
00855                     }
00856                     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
00857                     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
00858                     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
00859                     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
00860                     h_sensor = 0;//reset height of sensor bar
00861                 }
00862             } else if(!checked) {//check line position
00863                 int position = qtra.readLine(sensorValues);
00864                 int pos = (position)/66;
00865                 oled.writeString(7,0,"pos: ");
00866                 oled.printf("%i",position);
00867                 oled.fillDisplay(0xFF,3,6,pos,(pos+20));
00868                 if(pos>0) {
00869                     oled.fillDisplay(0x00,3,6,0,pos-1);
00870                 }
00871                 if((pos+20)<127) {
00872                     oled.fillDisplay(0x00,3,6,(pos+20)+1,127);
00873                 }
00874             } else {//start
00875 
00876                 int position = qtra.readLine(sensorValues);
00877                 //printf("%i\n",position);
00878                 current_position = ((float)position-3500)/3500;
00879                 proportional = current_position;
00880 
00881                 // Compute the derivative
00882                 derivative = current_position - previous_position;
00883 
00884                 // Compute the integral
00885                 integral += proportional;
00886 
00887 
00888 
00889                 // Compute the power
00890                 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
00891 
00892 
00893                 // Compute new speeds
00894                 right = speed-power;
00895                 left  = speed+power;
00896 
00897                 // limit checks
00898                 if (right < MIN)
00899                     right = MIN;
00900                 else if (right > MAX)
00901                     right = MAX;
00902 
00903                 if (left < MIN)
00904                     left = MIN;
00905                 else if (left > MAX)
00906                     left = MAX;
00907 
00908                 // set speed
00909                 set_motors(left,right);
00910 
00911                 previous_position = current_position;
00912 
00913             }
00914             if(pressed) {
00915                 clear_display();
00916                 display_mode();
00917                 pressed = false;
00918                 set_motors(0,0);
00919             }
00920 
00921         } else if (mode == 6) {
00922             if(!set) {
00923                 set = true;
00924                 ticker2.attach(&flip,1);
00925             }
00926             float dist = srf.read();
00927             if(dist < 20) {
00928                 set_motors(.15,.15);
00929             } else {
00930                 if(flipped) {
00931                     set_motors(.15,-.15);
00932                 } else {
00933                     set_motors(0,0);
00934                 }
00935             }
00936             wait(.1);
00937             if(pressed) {
00938                 ticker2.detach();
00939                 clear_display();
00940                 display_mode();
00941                 pressed = false;
00942                 set_motors(0,0);
00943             }
00944 
00945         } else if (mode == 5) {
00946 
00947             float dist = srf.read();
00948             if(dist > 20) {
00949                 set_motors(.15,.15);
00950             } else {
00951                 set_motors(-.15,.15);
00952             }
00953             wait(.1);
00954             if(pressed) {
00955                 clear_display();
00956                 display_mode();
00957                 pressed = false;
00958                 set_motors(0,0);
00959             }
00960         }
00961 
00962     }
00963 }
00964 
00965 
00966