main.cpp

00001 #include "mbed.h"
00002 #include "tsi_sensor.h"
00003 #include <string>
00004 #include "HCSR04.h"
00005 
00006 /* This defines will be replaced by PinNames soon
00007 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
00008 #define ELEC0 9
00009 #define ELEC1 10
00010 #elif defined (TARGET_KL05Z)
00011 #define ELEC0 9
00012 #define ELEC1 8
00013 #else
00014 #error TARGET NOT DEFINED
00015 #endif*/
00016 
00017 PwmOut LED(LED1);
00018 PwmOut leftForward(PTD5);
00019 PwmOut leftBackward(PTA13);
00020 PwmOut rightForward(PTA5);
00021 PwmOut rightBackward(PTA4);
00022 
00023 PwmOut grabIn(PTE30);
00024 PwmOut grabOut(PTC1);
00025 PwmOut heightUp(PTD4);
00026 PwmOut heightDown(PTA12);
00027 
00028 // Defines the sensors (Left and Right)
00029 HCSR04 sensorLEFT(PTC12, PTC13);
00030 HCSR04 sensorRIGHT(PTC16, PTC17);
00031 
00032 Serial pc(USBTX, USBRX);
00033 Serial BT(PTC4, PTC3);
00034 char command = 0;
00035 char store;
00036 float frontSpeed = 0;
00037 float backSpeed = 0;
00038 float rightSpeed = 0;
00039 float leftSpeed = 0;
00040 bool pilot = false;
00041 
00042 
00043 int distLeft(int dLEFT){
00044     return dLEFT;    
00045 }
00046 
00047 // Get the right distance variable
00048 int distRight(int dRIGHT){
00049     return dRIGHT;    
00050 }
00051 
00052 //SELVKØRENDE I MØRKET!
00053 void selfDrive(){
00054     
00055     int dLEFT = sensorLEFT.distance(CM);
00056     int dRIGHT = sensorRIGHT.distance(CM);
00057     /*
00058     // Writes the left and right distance variable
00059        pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT));
00060        pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT));
00061     */
00062         char diller = BT.getc();
00063         BT.putc(diller);
00064        // Break out of loop if 'p'
00065        if(diller == 'p' && pilot == true){
00066             pilot = false;      
00067         }
00068         
00069     //Drej til venstre for at rette op    
00070     if(dRIGHT > 35){
00071       leftForward = 0.75;
00072       leftBackward = 0.25;
00073       rightForward = 0.25;
00074       rightBackward = 0.75;
00075     }
00076     
00077     else if(dLEFT > 35){
00078       leftForward = 0.25;
00079       leftBackward = 0.75;
00080       rightForward = 0.75;
00081       rightBackward = 0.25;
00082     }
00083     
00084     else if(dRIGHT > 50){
00085       leftForward = 0.1;
00086       leftBackward = 0.9;
00087       rightForward = 0.9;
00088       rightBackward = 0.1;
00089     }     
00090     
00091     else if(dLEFT > 50){
00092       leftForward = 0.1;
00093       leftBackward = 0.9;
00094       rightForward = 0.9;
00095       rightBackward = 0.1;
00096       }else{
00097       leftForward = 1;
00098       leftBackward = 0;
00099       rightForward = 1;
00100       rightBackward = 0;
00101     }         
00102 }     
00103 
00104 void motorControl(char input) {
00105   switch (input) {
00106     case 'w':
00107     rightSpeed = 0;
00108     leftSpeed = 0;
00109     BT.printf("FORWARDS!");
00110       if (frontSpeed < 0.99) {
00111         frontSpeed = frontSpeed + 0.25f;
00112 }
00113       if (backSpeed > 0.1) {
00114         backSpeed = backSpeed - 0.25f;
00115       }
00116       leftForward = frontSpeed;
00117       leftBackward = backSpeed;
00118       rightForward = frontSpeed;
00119       rightBackward = backSpeed;
00120       
00121       break;
00122 
00123     case 's':
00124     rightSpeed = 0;
00125     leftSpeed = 0;
00126     if (frontSpeed > 0.1) {
00127         frontSpeed = frontSpeed - 0.25f;
00128       }
00129     if (backSpeed < 0.99) {
00130         backSpeed = backSpeed + 0.25f;
00131       }
00132       leftForward = frontSpeed;
00133       leftBackward = backSpeed;
00134       rightForward = frontSpeed;
00135       rightBackward = backSpeed;
00136       break;
00137 
00138     case 'a':
00139       if (leftSpeed > 0.1) {
00140         leftSpeed = leftSpeed - 0.10f;
00141       }
00142       if (rightSpeed < 0.99) {
00143         rightSpeed = rightSpeed + 0.10f;
00144       }
00145       leftForward = rightSpeed;
00146       leftBackward = leftSpeed;
00147       rightForward = leftSpeed;
00148       rightBackward = rightSpeed;
00149       break;
00150 
00151     case 'd':
00152       if (leftSpeed < 0.99) {
00153         leftSpeed = leftSpeed + 0.10f;
00154       }
00155       if (rightSpeed > 0.1) {
00156         rightSpeed = rightSpeed - 0.10f;
00157       }
00158       leftForward = rightSpeed;
00159       leftBackward = leftSpeed;
00160       rightForward = leftSpeed;
00161       rightBackward = rightSpeed;
00162       break;
00163       
00164     case 'r':
00165       // Go up
00166       heightDown = 0;
00167       heightUp.write(0.5);
00168       wait(0.25);
00169       heightUp = 0;
00170       break;
00171       
00172     case 'f':
00173       // Down
00174       heightUp = 0;
00175       heightDown.write(0.5);
00176       wait(0.25);
00177       heightDown = 0;
00178       break;
00179       
00180     case 'g':
00181       // Close the grab
00182       grabOut = 0;
00183       grabIn.write(0.5);
00184       wait(0.3);
00185       grabIn = 0;
00186       break;
00187     case 'h': 
00188       // Open the grab
00189       grabIn = 0;
00190       grabOut.write(0.5);
00191       wait(0.3);
00192       grabOut = 0;
00193       break;
00194       
00195       
00196     case 'x':
00197       frontSpeed = 0;
00198       backSpeed = 0;
00199       rightSpeed = 0;
00200       leftSpeed = 0;
00201       leftForward = 0;
00202       rightForward = 0;
00203       leftBackward = 0;
00204       rightBackward = 0;
00205       grabIn = 0;
00206       grabOut = 0;
00207       heightUp = 0;
00208       heightDown = 0;
00209       break;
00210       
00211     case 'p':
00212         if(pilot == false){
00213             pilot = true;
00214             while(1){ 
00215                 selfDrive();
00216             }
00217         }
00218     break;
00219     
00220   }
00221 }
00222 int main(void) {
00223   LED = 0.1;
00224   leftForward = 0;
00225   rightForward = 0;
00226   leftBackward = 0;
00227   rightBackward = 0;
00228   while (1) {
00229 
00230         command = BT.getc();
00231         BT.putc(command);
00232         motorControl(command);
00233     }
00234   }