// Testprogram for testing av Mr. Abot etter oppbygging
// Programmet tester:
// Høyre og venstre Servo
// Fire analoge reflektans-sensorer
// En digital avstandssensor
// Nils Kr. Rossing Vitensenteret 10.04.2014

#include <Servo.h>             // Inkluderer servobibliotek

int analog_sensorPin_0 = A0;      //
int analog_sensorPin_1 = A1;
int analog_sensorPin_2 = A2;
int analog_sensorPin_3 = A3;

int analog_sensor_0_read = 0;
int analog_sensor_1_read = 0;
int analog_sensor_2_read = 0;
int analog_sensor_3_read = 0;

int digital_sensorPin = 10;

int digital_sensor_read = 0;

Servo servo_left;    // Definerer venste servo

Servo servo_right;   // Definerer høyre servo

void setup()
{

  //#################### Definerer utganger ####################
  
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);  
  
  //#################### definerer innganger #####################
  
  pinMode(digital_sensorPin, INPUT);
  
  pinMode(analog_sensorPin_0, INPUT);
  pinMode(analog_sensorPin_1, INPUT);
  pinMode(analog_sensorPin_2, INPUT);
  pinMode(analog_sensorPin_3, INPUT);

 //#################### definerer servoer #####################
  
  servo_left.attach(11);    // Venstre servo er koblet til port nr. 12
  servo_right.attach(12);   // Høyre servo er koblet til port nr. 13
    
 //#################### setter opp kommunikasjon til monitor ##################### 

  Serial.begin(9600);
  
}

void loop()
{

 //#################### Start høyre motor #####################
 
 servo_right.writeMicroseconds(1700); // Start høyre motor
 Serial.println("Hoyre motor gaar i 2 sek.");
 delay(2000);
 servo_right.writeMicroseconds(1500);     // Stopp høyre motor
 Serial.println("Hoyre motor stoppes");
 delay(1000);
 
 //#################### Start venstre motor #####################
 
 servo_left.writeMicroseconds(1300);
 Serial.println("Venstre motor gaar i 2 sek.");
 delay(2000);
 servo_left.writeMicroseconds(1500);
 Serial.println("Venstre motor stoppes");
 delay(1000);

 while(true)
   {
   //#################### Leser sensorer #####################
 
   analog_sensor_0_read = analogRead(analog_sensorPin_0);
   delay(100);
   analog_sensor_1_read = analogRead(analog_sensorPin_1); 
   delay(100);
   analog_sensor_2_read = analogRead(analog_sensorPin_2);
   delay(100);
   analog_sensor_3_read = analogRead(analog_sensorPin_3);
   delay(100);
   digital_sensor_read = digitalRead(digital_sensorPin);
  
   //#################### Rapporterer sensor verdier #####################
 
   Serial.print("A0 = ");
   Serial.print(analog_sensor_0_read);
   Serial.print(", A1 = ");
   Serial.print(analog_sensor_1_read); 
   Serial.print(", A2 = ");
   Serial.print(analog_sensor_2_read); 
   Serial.print(", A3 = ");
   Serial.print(analog_sensor_3_read); 
   Serial.print(", D0 = ");
   Serial.println(digital_sensor_read); 
   
   }
   
}