/*
  Robot 
  Ver 1.51
  Dan Kohn
  05/28/2011
*/

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

//Define Pins

#define LED 13              //if LED on solid - no remote control signal
#define LED1 31
#define LED2 33
#define LED4 35

#define LEDE1 22
#define LEDE2 24

#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=100;
long q[7];
int mode;
boolean button[4];
long ch5_last;
long ch6_last;

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

//compass variable
unsigned long compass;
float bearing;

//heading
float bearing_target;
float bearing_db;

float bearing_h;
float bearing_l;

//mode 3
int direction;

//autocalibration mode 4
int step;
float point[100];

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

  pinMode(LED, OUTPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED4, OUTPUT);
  pinMode(LEDE1, OUTPUT);
  pinMode(LEDE2, OUTPUT);
  
  //vex remote control reciever
  pinMode(vex, INPUT); 
  mode=0;
  
  //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);
  bearing_db=5;
  
  //encoder
  
  m1_count=0;
  m2_count=0;
  
  attachInterrupt(m1_int,encode_m1,RISING);
  attachInterrupt(m2_int,encode_m2,RISING);
  
  direction=0;
  step=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 variable button[] to true or false 
  // and creates a mode number based on the binary value.
  // 2^3 is used as a clear bit for the other 3 bits.

  temp2=q[5]-ch5_last;
  if ((q[5]>1500)&&(abs(temp2)>50))
  {
    button[2]=!button[2];
  }
  else if ((q[5]<500)&&(abs(q[5]-ch5_last)>50))
  {
    // button[3]=!button[3];
    mode=0;
    step=0;
    button[0]=LOW;
    button[1]=LOW;
    button[2]=LOW;
    button[3]=LOW;
  }  
  
  temp2=q[6]-ch6_last;
  if ((q[6]>1500)&&(abs(temp2)>50))
  {
    button[1]=!button[1];
  }
  else if ((q[6]<600)&&(abs(temp2)>50))
  {
    button[0]=!button[0];
  }  

  mode=button[0]*1+button[1]*2+button[2]*4;

  digitalWrite(LED1,button[0]);
  digitalWrite(LED2,button[1]);
  digitalWrite(LED4,button[2]);
       
  ch5_last=q[5];
  ch6_last=q[6];

  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(mode);
    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)&&(micros()<=starttime+24000));       //wait for echo OR 24ms - max range
        
    endtime=micros();
    ttime=endtime-starttime;
    delay((25000-ttime)/1000);            //make the sonar routine take a constant time to run
    distance=ttime*pow(10,-6)*1087*12/2;

    if (distance>156)                    //Max distance out of range light
          digitalWrite(LEDE1,HIGH);
    else
          digitalWrite(LEDE1,LOW);
    
    if (debug)
    {
      Serial.print(ttime);
      Serial.print("  ");
      Serial.print(distance);
      Serial.print("\t");
    }
  
    //read and display compass
    compass = pulseIn(compass_in,HIGH);
    delay((36990-compass)/1000);                //make this routine run in a constant time
    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 (mode==0)  //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 (mode==1)  //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 (mode==2)  //find bearing using campass
    {
      if (q[3]<660)
         bearing_target=0;
      else if (q[4]>1420)
         bearing_target=90;
      else if (q[3]>1420)
         bearing_target=180;
      else if (q[4]<660)
         bearing_target=270;
      else 
         bearing_target=bearing;
      
      bearing_l = bearing_target-bearing_db/2;
      bearing_h = bearing_target+bearing_db/2;
      
      if (((bearing_target!=0)&&((bearing<bearing_l)||(bearing>bearing_h)))||((bearing_target==0)&&((bearing<bearing_l+360)&&(bearing>bearing_h))))
      {
         m1_speed=200;
         m2_speed=200;
         right();
         delay(75);
         mstop();
      }
      else
         mstop();
    }
    else if (mode==3)  //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;
        }
    } 
    else if (mode==4)  //autocalibration  place robot down facing a wall about two feet away
    {
        if (step==0)    
        {
           m1_speed=200;
           m2_speed=200;
           mstop();
           step++;
           delay(1000);
        }
        else if (step==1)
        {
           point[0]=distance;      //distance from wall to start
           m1_count=0;
           m2_count=0;
           step++;
        }
        else if (step==2)
        {
           forward();
           delay(2000);
           mstop();
           delay(1000);
           step++;
        }
        else if (step==3)
        {
           point[1]=distance;   //distance from wall after move
           point[2]=m1_count;   //number of counts m1 after move
           point[3]=m2_count;   //number of counts m2 after move
           m1_count=0;
           m2_count=0;
           step++;           
        }
        else if (step==4)
        {
           reverse();
           delay(2000);
           mstop();
           delay(1000);
           step++;
        }
        else if (step==5)
        {
           point[4]=distance;  //after move back to start - distance from wall
           point[5]=m1_count;  //number of counts m1 after move
           point[6]=m2_count;  //number of counts m2 after move
           step++;
        }
        else if (step==6)
        {
           point[7]=bearing;   //bearing before turn
           m1_count=0;
           m2_count=0;
           step++;
        }
        else if (step==7)
        {
           right();
           delay(2000);
           mstop();
           delay(1000);
           step++;
        }
        else if (step==8)
        {
           point[8]=bearing;   //bearing after turn
           point[9]=m1_count;  //number of counts m1 after turn  
           point[10]=m2_count; //number of counts m2 after turn
           m1_count=0;
           m2_count=0;
           step++;           
        }
        else if (step==9)
        {
           left();
           delay(2000);
           mstop();
           delay(1000);
           step++;
        }
        else if (step==10)
        {
           point[11]=bearing;  //bearing after turn back to start
           point[12]=m1_count; //number of counts m1 after turn 
           point[13]=m2_count; //number of counts m2 after turn
           step++;
        }
        else
        {
          Serial.print(step);
          Serial.print("\t");
          for(temp=0;temp<=13;temp++)
          {
              Serial.print(point[temp]);
              Serial.print("\t");
          }
          Serial.print("\n");
        }
    } 
    else 
    {
        digitalWrite(m2_E,LOW);
        digitalWrite(m1_E,LOW); 
        digitalWrite(LED,HIGH);
        delay(500);
        digitalWrite(LED,LOW);
        delay(500);
        step=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);  
  analogWrite(m1_E,m1_speed);  
}

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);  
  analogWrite(m1_E,m1_speed);  

}

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); 
  analogWrite(m1_E,m1_speed);  
}

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);  
  analogWrite(m1_E,m1_speed); 

}

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++;
}