File content as of revision 0:47165eb4e54d:

#include "mbed.h"
#include "HCSR04.h"
#include "SG90.h"
#include "PwmOut.h"


Serial pc(USBTX, USBRX);
servo myservo;

 int i = 0;
int G0 = 0;
int G15 = 0;
int G30 = 0;
int G45 = 0;
int G60 = 0;
int G75 = 0;
int G90 = 0;
int G105 = 0;
int G120 = 0;
int G135 = 0;
int G150 = 0;
int G165 = 0;
int G180 = 0;

float m = 0.5;

PwmOut left(PTE23 );
PwmOut right(PTE22 );
int delay = 200; // Delay mellem målingerne
double B = 0; // Vinklen B Tegning 1
int A = 179; // Vinkel mellem de to målinger



int main(){
    pc.baud(9600);
    HCSR04 sensor(PTA13, PTD5);
    left.period(0.0066);
    right.period(0.0066);
    while(1){
        myservo.position(1, 0);
        wait_ms(delay);
        G0 = sensor.distance(CM);
        i = i + A;
        myservo.position(1, 180);
        G180 = sensor.distance(CM);
        wait_ms(delay);
        i = i + A;
        
        pc.printf("G0: %ld  G15: %ld G30: %ld G45: %ld G60: %ld G75: %ld G90: %ld G105: %ld G120: %ld G135: %ld G150: %ld G165: %ld G180: %ld \r\n", G0, G15, G30, G45, G60, G75, G90, G105, G120, G135, G150, G165, G180);
        
        
        if(G0 > G180){
            left.write(0.4);
            right.write(0.2);
            }
        else if(G180 > G0){
            left.write(0.2);
            right.write(0.4);
            }

        
        
        
        
        
        if(i >= 180 || i+20 >= 180){
            i=0;
            }//if
    } // while
} // main