File content as of revision 4:6a4a59e4e3dd:

#include "hack_motor.h" 
#include "auto_pilot.h"
#include "mbed.h"
#include "HCSR04.h"
#include "PwmOut.h"


Serial pc(USBTX, USBRX);
HCSR04 sensor(PTA13, PTD5, PTD0, PTD2);
    
Wheel robot;
int control = 0;
extern double L1 = 0;
extern double L2 = 0;
int main(){
    

    pc.baud(9600);
    pc.printf(" speed 1, 2 & 3, frem w, bagud s, venstre a & hoejre d"); 
   
    while(control == 0){                
        robot.init(); 
        char c; 
        
          
            { 
            c = pc.getc();  
            switch (c) 
                {  
                    case 0x31: 
                        robot.speed = 1.0; 
                        pc.printf("1"); 
                        break; 
                          
                    case 0x32: 
                        robot.speed = 0.5; 
                        pc.printf("2"); 
                        break; 
                          
                    case 0x33:  
                        robot.speed = 0.25; 
                        pc.printf("3"); 
                        break; 
                          
                    case 's': 
                        robot.FW(); 
                        pc.printf("s"); 
                        break; 
                      
                    case 'w': 
                        robot.BW(); 
                        pc.printf("w"); 
                        break; 
                          
                    case 'a': 
                        robot.right();  
                        pc.printf("a"); 
                        wait(0.1);
                        robot.stop();
                        
                        break; 
                          
                    case 'd': 
                        robot.left(); 
                        pc.printf("d");
                        wait(0.1);
                        robot.stop(); 
                        break; 
                        
                    case 'n':
                        robot.open();
                        pc.printf("n");
                        break;
                        
                        case 'm':
                        robot.close();
                        pc.printf("m");
                        break;
                        
                        case 'h':
                        robot.hejs();
                        pc.printf("h");
                        break;
                        
                        case 'b':
                        robot.saenk();
                        pc.printf("b");
                        break;
                        
                        case 'p':
                        control = 1;
                        break;                        
                        
                      
                    default : 
                        robot.stop(); 
                        break; 
                }
    }
        }
        {
       
    
    
        
        
        
    } // while
    
    while(control == 1){                
        
        L1 = sensor.Distance(CM); // Højre
        L2 = sensor.distance(CM); // Venstre
        
        pc.printf("L1: %ld  L2: %ld \r\n", L1, L2);
        
        
        if(L1 >= L2 && L1 > 50){
        robot.venSelv3(); // Højre skarpt
        }
        
        if(L2 >= L1 && L2 > 50){
        robot.venSelv4(); // Venstre skarpt
        }
        
        if(L1 >= L2){
        robot.venSelv1(); // Højre
        }
        
        
        
        if(L2 > L1){
        robot.venSelv2(); // Venstre
        }
        
        if(L1 > 65){
            control = 0;
            }
            }
    
    
    
    
    
} // main