/*
  VEX Remote - Test 1
  Vex output to input pin 10 on Arduino
  
  Displays the channels and their corresponding values
 
  Dan Kohn
  10/12/2010
  
*/

unsigned long duration;
unsigned long q[7];

void setup() {                
  // initialize the digital pin as an output.
  // Pin 13 has an LED connected on most Arduino boards:
  pinMode(10, INPUT); 
  pinMode(13, OUTPUT);
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  Serial.begin(9600);    
}

void loop() {
  digitalWrite(13, LOW);           // set the LED on
  
  do                               // wait for long sync pulse
  {
    duration = pulseIn(10,LOW);    
  } while(duration < 7000);
   
  for (int x=1; x<=6; x++)        // read 6 valid readings and store in q[1]..q[6]
  {
    duration=pulseIn(10,LOW);
    q[x]=duration;

  }

  for (int x=1; x<=6; x++)        //display readings
  {
    Serial.print(x);
    Serial.print(":\t");
    Serial.print(q[x]);
    Serial.print("\t");
  }
  Serial.print("\t");
  
  if ((q[2]>1200)&&(q[1]>800)&&(q[1]<1200))
  {
      Serial.print ("forward");
      digitalWrite(3,HIGH);
      digitalWrite(4,LOW);
      digitalWrite(6,HIGH);
      digitalWrite(7,LOW);
      digitalWrite(2,HIGH);
      digitalWrite(5,HIGH);
  }
  
  else if ((q[2]<800)&&(q[1]>800)&&(q[1]<1200))
  {
      Serial.print ("reverse");
      digitalWrite(3,LOW);
      digitalWrite(4,HIGH);
      digitalWrite(6,LOW);
      digitalWrite(7,HIGH);
      digitalWrite(2,HIGH);
      digitalWrite(5,HIGH);
  }
  else if (q[1]<800)
  {
      Serial.print ("turn1");
      digitalWrite(3,HIGH);
      digitalWrite(4,LOW);
      digitalWrite(6,LOW);
      digitalWrite(7,HIGH);
      digitalWrite(2,HIGH);
      digitalWrite(5,HIGH);
  }
  else if(q[1]>1200)
  {
      Serial.print ("turn2");
      digitalWrite(3,LOW);
      digitalWrite(4,HIGH);
      digitalWrite(6,HIGH);
      digitalWrite(7,LOW);
      digitalWrite(2,HIGH);
      digitalWrite(5,HIGH);
  }
  
  else
  {
      Serial.print ("stop");
      digitalWrite(2,LOW);
      digitalWrite(5,LOW);
  }
  Serial.print("\n");
  
  digitalWrite(13, HIGH);    // set the LED off

}