/*
  Robot
  Ver 1.0
  Dan Kohn
  11/18/2010
  
*/

//temp variables
int temp;
long temp2;

//vex RC variables
long duration;
long center=1040;
long span=350;
boolean chn5;
boolean chn6;
long q[7];
long last[7];
long diff[7];
long maxerr;

//sonar varialbes 
unsigned long starttime;
unsigned long endtime;
unsigned long ttime;
float distance;

//compass variable
unsigned long compass;
float bearing; 

void setup() 
{
  // initialize the digital pin as an output.
  //vex remote control reciever
  pinMode(10, INPUT);
  
  //H-bridge drive motor control
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  
  //sonar pins
  pinMode (12, INPUT);
  pinMode (13, OUTPUT);
  
  //compass pin
  pinMode (9, INPUT);
  
  Serial.begin(9600);
}

void loop() {
  // read vex remote control reciever and store values in q[chn#]
  
  do // wait for long sync pulse
  {
    duration = pulseIn(10,LOW);
  } while(duration < 7000);
  
  maxerr=0;
  for (int x=1; x<=6; x++) // read 6 valid readings and store in q[1]..q[6]
  {
    duration=pulseIn(10,LOW);
    last[x]=last[x]*0.30+q[x]*0.70;
    q[x]=duration;
    
    diff[x]=q[x]-last[x];
    temp2=abs(diff[x]);
    
    if ((temp2>maxerr)&&(x<5))
    {
      maxerr=temp2;
    }      
  }
  // handle chn5 and chn6 buttons (set variables chn5 and chn6 (boolean) to true or false
  if (q[5]>1500)
  {
    chn5=LOW;
    mstop();
  }
  else if (q[5]<500)
  {
    chn5=HIGH;
    mstop();
  }  
  
  if (q[6]>1500)
  {
    chn6=LOW;
    mstop();
  }
  else if (q[6]<600)
  {
    chn6=HIGH;
    mstop();
  }  
  
  
  //display reading from vex remote control
  for (int x=1; x<=6; x++)
  {
    Serial.print(x);
    Serial.print(":\t");
    Serial.print(q[x]);
    Serial.print("\t");
    Serial.print(diff[x]);
    Serial.print("\t");
  }
  Serial.print("*");
  Serial.print(maxerr);
  Serial.print("\t");
  Serial.print(chn5,BIN);
  Serial.print("\t");
  Serial.print(chn6,BIN);
  Serial.print("\t");
  
  
  //sonar read and display
    digitalWrite(13,HIGH);
    starttime=micros();
    delayMicroseconds(10);
    digitalWrite(13,LOW);
    do
    {
    }while(digitalRead(12)!=HIGH);
    do
    {
    }while(digitalRead(12)!=LOW);
    endtime=micros();
    ttime=endtime-starttime;
    distance=ttime*pow(10,-6)*1087*12/2;
    Serial.print(ttime);
    Serial.print(" ");
    Serial.print(distance);
    Serial.print("\t");
    //read and display compass
    compass = pulseIn(9,HIGH);
    Serial.print(compass);
    Serial.print("\t");
    bearing = (compass-1000)/100;
    Serial.print(bearing);
    Serial.print("\t");
  if (chn5==LOW && chn6==LOW)
  {
     if (maxerr>150)
     {
       mstop();
       delay(200);
     }
     //remote control drive
     if (q[1]<200 || q[1]>3000 || q[2]<200 || q[2]>3000 || q[3]<200 || q[3]>3000 || q[4]<200 || q[4]>3000 || q[5]<200 || q[5]>3000 || q[6]<200 || q[6]>3000) //check for dead remote and stop
     {
        Serial.print ("no remote");
        digitalWrite(2,LOW);
        digitalWrite(5,LOW);
     }
     else if ((q[2]>center+span)&&(q[1]>center-span)&&(q[1]center-span)&&(q[1]center+span)
     {
        right();
     }
     else
     {
        mstop();
     }
  } 
  else if (chn5==LOW and chn6==HIGH)
  {
    if (distance>20)
    {
      forward();
    } 
    else
    {
      temp=random(0,99);
      if (temp <50)
      {
        right();
        delay(200);
        mstop();
        delay(200);
      }
      else
      {
        left();
        delay(300);
        mstop();
        delay(200);
      }     
    }
    
  }
  else if (chn5==HIGH and chn6==LOW)
  {
  }
  else if (chn5==HIGH and chn6==HIGH)
  {
  }
  
  Serial.print("\n");
}

void forward(void) 
{
        Serial.print ("forward");
        digitalWrite(3,LOW);
        digitalWrite(4,HIGH);
        digitalWrite(6,LOW);
        digitalWrite(7,HIGH);
        digitalWrite(2,HIGH);
        digitalWrite(5,HIGH);
}

void reverse(void) 
{
        Serial.print ("reverse");
        digitalWrite(3,HIGH);
        digitalWrite(4,LOW);
        digitalWrite(6,HIGH);
        digitalWrite(7,LOW);
        digitalWrite(2,HIGH);
        digitalWrite(5,HIGH);
}

void left(void) 
{
        Serial.print ("left");
    
        digitalWrite(3,HIGH);
        digitalWrite(4,LOW);
        digitalWrite(6,LOW);
        digitalWrite(7,HIGH);
        digitalWrite(2,HIGH);
        digitalWrite(5,HIGH);
}

void right(void) 
{
        Serial.print ("right");
       
        digitalWrite(3,LOW);
        digitalWrite(4,HIGH);
        digitalWrite(6,HIGH);
        digitalWrite(7,LOW);
        digitalWrite(2,HIGH);
        digitalWrite(5,HIGH);
}

void mstop(void) 
{
        Serial.print ("stop");
        digitalWrite(2,LOW);
        digitalWrite(5,LOW);
}