// lab1_tester.pde
// August 2010 Matthew_Keeter@hmc.edu
// Updated 5 September 2014 David_Harris@hmc.edu to match Mudduino 1.31
//
// Test Mudduino board

#define MOTOR_R_PLUS 7
#define MOTOR_R_MINUS 12
#define MOTOR_R_EN 11
#define MOTOR_L_PLUS 9
#define MOTOR_L_MINUS 8
#define MOTOR_L_EN 6

#define NOTE_A3  220
#define NOTE_B3  247
#define NOTE_CS4 277
#define NOTE_D4  294
#define NOTE_E4  330

int digitalPins[] = {MOTOR_L_MINUS, MOTOR_L_PLUS, 2, 4, 5, 10, 13, 
                    MOTOR_R_MINUS, MOTOR_R_PLUS}; 

void setup()
{
  // test serial 
  Serial.begin(9600);
  Serial.println("Hello, world!");
  delay(100);
  Serial.end();
  
  for(int i=0; i<14; i++) pinMode(i, OUTPUT);

  //Serial.println("\nTesting speaker");
  int notes[] = {
    NOTE_A3, NOTE_B3, NOTE_CS4, NOTE_D4, NOTE_E4      };
  int timing[] = {
    100, 100, 100, 100, 100      };
  for(int i=0; i<5; i++) {
    tone(4, notes[i], timing[i]);
    delay(timing[i]*1.3);
  }
  
  for(int i=0; i<9; i++) {
    digitalWrite(digitalPins[i], LOW);
  }

  Serial.begin(9600);
  int expectedvalue = 1023;
  int readvalue;
  for(int i=1; i<=5; i++) {
    readvalue = analogRead(i);
    if(abs(expectedvalue - readvalue) < 15) {
      Serial.print("Analog input ");
      Serial.print(i); 
      Serial.println(" is working.");
    }
    else {
      Serial.print("Analog input ");
      Serial.print(i); 
      Serial.print(" is NOT working: expected ");
      Serial.print(expectedvalue);
      Serial.print(", found ");
      Serial.println(readvalue);
    }
    expectedvalue -= 256;
  }

/*
  if(abs(analogRead(0) - 512) < 15) {
    Serial.println("Distance sensor plug is working.");
  }
  else {
    Serial.print("Distance sensor plug is NOT working: expected ");
    Serial.print(expectedvalue);
    Serial.print(", found ");
    Serial.println(readvalue);
  }
*/
  Serial.end();

  digitalWrite(MOTOR_L_EN, HIGH);
  digitalWrite(MOTOR_R_EN, HIGH);
}


void loop() 
{
  // Test digital pins
  //Serial.println("\nTesting digital pins");
  for(int i=0; i<9; i++) {
    digitalWrite(digitalPins[i], HIGH);
    delay(200);
    digitalWrite(digitalPins[i], LOW);
  }
}