/*
  Robot 
  Ver 1.33
  Dan Kohn
  05/19/2011
*/

//debug
#define debug LOW           //set HIGH for debug output

//Define Pins

#define LED 13              //if LED on solid - no remote control signal

#define vex 10

#define sonar_init 52
#define sonar_echo 8

#define compass_in 9

#define m1_I1 7
#define m1_I2 6
#define m1_E 5

#define m2_I3 4
#define m2_I4 3
#define m2_E 2

#define m1_encoder 18
#define m2_encoder 19

#define m1_int 5
#define m2_int 4

//temp variables
int temp;
long temp2;

//motor speed variables
int m1_speed;
int m2_speed;
int speed;

//motor encoder
volatile unsigned long m1_count;
volatile unsigned long m2_count;
 
//vex RC variables
long duration;
long center=1040;
//long span=350;
long span=50;
boolean chn5;
boolean chn6;
long q[7];

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

//compass variable
unsigned long compass;
float bearing;

//mode 3
int direction;

void setup() {                
  // initialize the digital pin as an output.

  pinMode(LED, OUTPUT);
  
  //vex remote control reciever
  pinMode(vex, INPUT); 
  
  //H-bridge drive motor control
  pinMode(m2_E, OUTPUT);
  pinMode(m2_I4, OUTPUT);
  pinMode(m2_I3, OUTPUT);
  pinMode(m1_E, OUTPUT);
  pinMode(m1_I2, OUTPUT);
  pinMode(m1_I1, OUTPUT);
  
  //sonar pins
  pinMode (sonar_echo, INPUT);
  pinMode (sonar_init, OUTPUT);
  
  //compass pin
  pinMode (compass_in, INPUT);
  
  //encoder
  
  m1_count=0;
  m2_count=0;
  
  attachInterrupt(m1_int,encode_m1,RISING);
  attachInterrupt(m2_int,encode_m2,RISING);
  
  direction=0;
  
  Serial.begin(9600);    

  
}

void loop() {
  
  digitalWrite(LED,LOW);
  // read vex remote control reciever and store values in q[chn#]
  noInterrupts();
  do                               // wait for long sync pulse
  {
    duration = pulseIn(vex,LOW);   
    if (duration ==0)              //if no remote stop everything!
    {
       digitalWrite(LED,HIGH);
       digitalWrite(m2_E,LOW);
       digitalWrite(m1_E,LOW); 
    }  
  } while(duration < 7000);      
  

  for (int x=1; x<=6; x++)        // read 6 valid readings and store in q[1]..q[6]
  {
    duration=pulseIn(vex,LOW);
    q[x]=duration;
  }

  // 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();
  }  
  interrupts();
  
  //display reading from vex remote control
  if (debug)
  {
    for (int x=1; x<=6; x++)
    {
      Serial.print(x);
      Serial.print(":\t");
      Serial.print(q[x]);
      Serial.print("\t");    
    }
    Serial.print("*");
    Serial.print(chn5,BIN);
    Serial.print("\t");
    Serial.print(chn6,BIN);
    Serial.print("\t");  
  }
  
  //sonar read and display
    digitalWrite(sonar_init,HIGH);
    starttime=micros();
    delayMicroseconds(10);
    digitalWrite(sonar_init,LOW);
    do 
    {
    }while(digitalRead(sonar_echo)!=HIGH);
    do 
    {
    }while(digitalRead(sonar_echo)!=LOW);
    endtime=micros();
    ttime=endtime-starttime;
    distance=ttime*pow(10,-6)*1087*12/2;
    if (debug)
    {
      Serial.print(ttime);
      Serial.print("  ");
      Serial.print(distance);
      Serial.print("\t");
    }
    
    //read and display compass
    compass = pulseIn(compass_in,HIGH);
    bearing = (compass-1000)/100;
    if (debug)
    {
      Serial.print(compass);
      Serial.print("\t");
      Serial.print(bearing);
      Serial.print("\t");    
    }
    
//  //stop if no com with vex controller 
//  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(m2_E,LOW);
//    digitalWrite(m1_E,LOW);
//  }

    if (chn5==LOW && chn6==LOW)  //Remote control drive
    { 
      
       if (debug)
       {
         Serial.print("->");
         Serial.print(speed); 
         Serial.print("\t");      
       }
       
       if ((q[2]>center+span)&&(q[1]>center-span)&&(q[1]<center+span))
       {
         speed=abs(1040-q[2])*0.2561+150;
         if (speed>255) 
           speed=255;
         m1_speed=speed;
         m2_speed=speed;
         reverse();
       }
    
       else if ((q[2]<center-span)&&(q[1]>center-span)&&(q[1]<center+span))
       {
         speed=abs(1040-q[2])*0.2561+150;
         if (speed>255) 
           speed=255;
         m1_speed=speed;
         m2_speed=speed;
         forward();
       }
       else if (q[1]<center-span)
       {
         speed=abs(1040-q[1])*0.2561+150;
         if (speed>255) 
           speed=255;
         m1_speed=speed;
         m2_speed=speed;
         left();
       }
       else if(q[1]>center+span)
       {
         speed=abs(1040-q[1])*0.2561+150;
         if (speed>255) 
           speed=255;
         m1_speed=speed;
         m2_speed=speed;
         right();
       }
       else 
       {
          mstop();
       }
    } 
    else if (chn5==LOW and chn6==HIGH)  //sonar collision avoidance
    {
      m1_speed=255;
      m2_speed=255;
      
      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)  //clear counters
    {
      m1_count=0;
      m2_count=0;
      chn5=LOW;
    }
    else if (chn5==HIGH and chn6==HIGH)  //preprogrammed cycle
    {
        m1_speed=200;
        m2_speed=200;
        if ((direction==0)&&(m1_count<4000))    
        {
           forward();
        }
        else if ((direction==0)&&(m1_count>4000))
        {
           m1_count=0;
           direction=1;
        }
        else if ((direction==1)&&(m1_count<5200))    // aprox 90-100 deg turn
        {
           right();
        }
        else if ((direction==1)&&(m1_count>5200))
        {
           m1_count=0;
           direction=0;
        }
    }    

 
  if (debug)
  {
        Serial.print("\t**\t");
        Serial.print(m1_count);
        Serial.print("\t");
        Serial.print(m2_count);
    	Serial.print("\n");
  }

}


void forward()
{
  if (debug)
    Serial.print ("forward");

  digitalWrite(m2_I4,LOW);
  digitalWrite(m2_I3,HIGH);
  digitalWrite(m1_I2,LOW);
  digitalWrite(m1_I1,HIGH);
  analogWrite(m2_E,m2_speed);  //digitalWrite(m2_E,HIGH);
  analogWrite(m1_E,m1_speed);  //digitalWrite(m1_E,HIGH);
}

void reverse()
{
  if (debug)
    Serial.print ("reverse");
        
  digitalWrite(m2_I4,HIGH);
  digitalWrite(m2_I3,LOW);
  digitalWrite(m1_I2,HIGH);
  digitalWrite(m1_I1,LOW);
  analogWrite(m2_E,m2_speed);  //digitalWrite(m2_E,HIGH);
  analogWrite(m1_E,m1_speed);  //digitalWrite(m1_E,HIGH);

}

void left()
{
  if (debug)
    Serial.print ("left");

  digitalWrite(m2_I4,HIGH);
  digitalWrite(m2_I3,LOW);
  digitalWrite(m1_I2,LOW);
  digitalWrite(m1_I1,HIGH);
  analogWrite(m2_E,m2_speed);  //digitalWrite(m2_E,HIGH);
  analogWrite(m1_E,m1_speed);  //digitalWrite(m1_E,HIGH);
}

void right()
{
  if (debug)
    Serial.print ("right");

  digitalWrite(m2_I4,LOW);
  digitalWrite(m2_I3,HIGH);
  digitalWrite(m1_I2,HIGH);
  digitalWrite(m1_I1,LOW);
  analogWrite(m2_E,m2_speed);  //digitalWrite(m2_E,HIGH);
  analogWrite(m1_E,m1_speed);  //digitalWrite(m1_E,HIGH);

}

void mstop()
{
  if (debug)
    Serial.print ("stop");

  digitalWrite(m2_E,LOW);
  digitalWrite(m1_E,LOW);  
}

void encode_m1()
{
  m1_count++;
}

void encode_m2()
{
  m2_count++;
}