#include <Servo.h>

const int MotorA_IA = 2;
const int MotorA_IB = 3; //PWM
const int MotorB_IA = 4;
const int MotorB_IB = 5; //PWM

const int Motor2A_IA = 6;
const int Motor2A_IB = 7; //PWM
const int Motor2B_IA = 8;
const int Motor2B_IB = 9;//PWM

const int KOMMUNIKASJON= 12;

const int MOTOR1 = 1;
const int MOTOR2 = 2;
const int MOTOR3 = 3;
const int MOTOR4 = 4;

const int ServoPin = 10;
Servo MYSERVO;

const int TRIGPIN = 11;
const int ECHOPIN = 12;

void drive(int Motor,int Speed)
{
int MotorPinA = MotorA_IA;
int MotorPinB = MotorA_IB;

if (Speed > 255 || Speed < -255)
Speed = 0;

Speed *= -1;

switch (Motor)
{
case MOTOR1:
MotorPinA = MotorA_IA;
MotorPinB = MotorA_IB;
break;
case MOTOR2:
MotorPinA = MotorB_IA;
MotorPinB = MotorB_IB;
break;
case MOTOR3:
MotorPinA = Motor2A_IA;
MotorPinB = Motor2A_IB;
break;
case MOTOR4:
MotorPinA = Motor2B_IA;
MotorPinB = Motor2B_IB;
break;
}

if (Speed >= 0)
{
digitalWrite(MotorPinA, 0);
analogWrite(MotorPinB, Speed);
}
else
{
digitalWrite(MotorPinA, 1);
analogWrite(MotorPinB, 255+Speed);
}
}

long ping(){
long duration;
digitalWrite(TRIGPIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIGPIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIGPIN, LOW);
duration = pulseIn(ECHOPIN, HIGH);
return duration;
}

void direction(int pos){
MYSERVO.write(pos);
}

void setup()
{
Serial.begin(9600);
pinMode(MotorA_IA, OUTPUT);
pinMode(MotorA_IB, OUTPUT);
pinMode(MotorB_IA, OUTPUT);
pinMode(MotorB_IB, OUTPUT);
pinMode(Motor2A_IA, OUTPUT);
pinMode(Motor2A_IB, OUTPUT);
pinMode(Motor2B_IA, OUTPUT);
pinMode(Motor2B_IA, OUTPUT);

pinMode(KOMMUNIKASJON, OUTPUT);

pinMode(ServoPin, OUTPUT);
MYSERVO.attach(ServoPin);

pinMode(TRIGPIN, OUTPUT);
pinMode(ECHOPIN, INPUT);
}

void loop()
{
//Her skal koden deres stå
}