main.cpp

00001 #include "mbed.h"
00002 #include "HCSR04.h"
00003 
00004 // Set's the Serial port
00005 Serial pc(USBTX, USBRX);
00006 
00007 // Defines the LED colors
00008 DigitalOut led(LED_RED);
00009 DigitalOut led2(LED_GREEN);
00010 
00011 // Defines the sensors (Left and Right)
00012 HCSR04 sensorLEFT(PTA12, PTD4);
00013 HCSR04 sensorRIGHT(PTA4, PTA5);
00014 
00015 // Get the left distance variable
00016 int distLeft(int dLEFT){
00017     return dLEFT;    
00018 }
00019 
00020 // Get the right distance variable
00021 int distRight(int dRIGHT){
00022     return dRIGHT;    
00023 }
00024 
00025 
00026 //SELVKØRENDE I MØRKET!
00027 void selfDrive(){
00028     
00029     int dLEFT = sensorLEFT.distance(CM);
00030     int dRIGHT = sensorRIGHT.distance(CM);
00031     
00032     // Writes the left and right distance variable
00033        pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT));
00034        pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT));
00035     
00036         
00037     //Drej til venstre for at rette op
00038     if(dRIGHT > 60 && dLEFT > 60 || BT.getc()=='p'){
00039       control = 0;
00040     }
00041     
00042     else if(dRIGHT > 35){
00043       leftForward = 0.75;
00044       leftBackward = 0.25;
00045       rightForward = 0.25;
00046       rightBackward = 0.75;
00047     }
00048     
00049     else if(dLEFT > 35){
00050       leftForward = 0.25;
00051       leftBackward = 0.75;
00052       rightForward = 0.75;
00053       rightBackward = 0.25;
00054     }
00055     
00056     else if(dRIGHT > 50){
00057       leftForward = 0.1;
00058       leftBackward = 0.9;
00059       rightForward = 0.9;
00060       rightBackward = 0.1;
00061     }     
00062     
00063     else if(dLEFT > 50){
00064       leftForward = 0.1;
00065       leftBackward = 0.9;
00066       rightForward = 0.9;
00067       rightBackward = 0.1;
00068       
00069     else{
00070       leftForward = 1;
00071       leftBackward = 0;
00072       rightForward = 1;
00073       rightBackward = 0;
00074     }         
00075 }     
00076 // The main() function
00077 int main()
00078 {
00079     
00080     
00081     // The loop() function
00082     while(1) {
00083         //left and right distance variables
00084                 selfControl(dRIGHT, dLEFT);    
00085         
00086         
00087         // Delay
00088         wait(0.5);
00089     }
00090     
00091 }
00092 
00093  
00094 
00095 
00096