const int servoPin = 6;
const int escPin = 7;
const int trig = 10; // attach pin 3 to Trig
const int echo = 9; //attach pin 4 to Echo
Servo steeringServo;
Servo esc;

void drive(int speed) {
speed = max(speed, 0);
speed = min(speed, 100);
speed = map(speed, 0, 100, 91, 0);
esc.write(speed);
}

void start() {
drive(5);
}

void stop() {
drive(0);
}

int avstand() {
long duration, cm;
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig, HIGH);
delayMicroseconds(5);
digitalWrite(trig, LOW);
duration = pulseIn(echo, HIGH);
cm = microsecondsToCentimeters(duration);
Serial.print(cm);
Serial.print("cm");
Serial.println();
delay(100);
return cm;
}

long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}

void setup() {
pinMode(servoPin, OUTPUT);
pinMode(escPin, OUTPUT);
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
steeringServo.attach(servoPin);
steeringServo.write(90);
esc.attach(escPin);
delay(2000);

Serial.begin(9600);
}

void loop(){
//her skal dere skrive kode