#include <Servo.h>

 int E1 = 3; //M1 Speed Control
int E2 = 11; //M2 Speed Control
int M1 = 12; //M1 Direction Control
int M2 = 13; //M2 Direction Control

 //Declare Servos
 Servo leftservo;     //Left wheel servo
 Servo rightservo;    //Right wheel servo
 Servo scanservo;     //Ping Sensor Servo
 Servo armservo;       //gripper servo
 
 const int turntime = 1500;  //Number of milliseconds to turn when turning 
 const int pingPin = 10;     //Pin that the Ping sensor is attached to. 
 const int trigPin = -1;
 const int echoPin = -1;
 int URPWM = -1; // PWM Output 0?25000US?Every 50US represent 1cm
 int URTRIG=-1; // PWM trigger pin
 int distance;
 int cm;
 
 uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01}; // distance measure command

 int leftspeed = 250; //255 is maximum speed
 int rightspeed = 250;
 const int sharpin = 5;
 const int armpin = 4;
 
 const int leftservopin = -1; //Pin number for left servo
 const int rightservopin = -1; // Pin number for right servo
 const int scanservopin = 9;   // Pin number for scan servo
 const int distancelimit = 15;   //If something gets this many inched from 
 String app="mapping";
                                 // the robot it stops and looks for where to go. 
 //Setup function. Runs once when Arduino is turned on or restarted
 void setup()
 {
  int i;
  pinMode(3, OUTPUT);
  for(i=11;i<=13;i++)
  pinMode(i, OUTPUT);
  Serial.begin(9600);
  //leftservo.attach(leftservopin); //Attach left servo to its pin.
  //rightservo.attach(rightservopin); // Attch the right servo
  scanservo.attach(scanservopin); // Attach the scan servo
  armservo.attach(armpin);   //attach the gripper servo
  if(URPWM>-1) 
        PWM_Mode_Setup();

  delay(2000);        // wait two seconds
  Serial.println('f');
 }
 
 
 void loop(){
  //go(leftspeed,rightspeed);  
  
  if(app=="pulling")
  {
  sharp();
  }
 
 
 
 if (app=="mapping")
  {
    sharp2();
    scan2();
    go(leftspeed,rightspeed);
  }

  if(pingPin>-1)
  {
  delay(800);
  distance = ping(); // us the ping() function to see if anything is ahead. 
  if (distance < distancelimit){
    stopmotors(); // If something is ahead, stop the motors. 
    Serial.println('s');
    char turndirection = scan(); //Decide which direction to turn. 
    
     switch (turndirection){
      case 'l':
        turnleft(turntime,leftspeed,rightspeed);
        break;
      case 'r':
        turnright(turntime,leftspeed,rightspeed);
        break;
      case 'x': 
        backward(turntime,leftspeed,rightspeed);
        turnleft(turntime,leftspeed,rightspeed);
        break;
    }
  Serial.println('f');
  }
  }
 if(URPWM>-1)
  {
  
  distance = PWM_Mode(); // us the ping() function to see if anything is ahead. 
  if (distance < distancelimit){
    stopmotors(); // If something is ahead, stop the motors. 
    Serial.println('s');
    char turndirection = Pwmscan(); //Decide which direction to turn. 
    
     switch (turndirection){
      case 'l':
        turnleft(turntime,leftspeed,rightspeed);
        break;
      case 'r':
        turnright(turntime,leftspeed,rightspeed);
        break;
      case 'x': 
        backward(turntime,leftspeed,rightspeed);
        turnleft(turntime,leftspeed,rightspeed);
        break;
    }
   Serial.println('f');
  }
  
  }
 } 
 int PWM_Mode()
 { // a low pull on pin COMP/TRIG triggering a sensor reading
 digitalWrite(URTRIG, LOW);
 digitalWrite(URTRIG, HIGH); // reading Pin PWM will output pulses
 unsigned long DistanceMeasured=pulseIn(URPWM,LOW);
 if(DistanceMeasured==50000)
 { // the reading is invalid.
   Serial.print("Invalid");
    }
 else{
    distance=DistanceMeasured/50; // every 50us low level stands for 1cm
     }
 //Serial.print("Distance=");
 //Serial.println(distance);
 //Serial.println("cm");

  return (distance);
 }

 void PWM_Mode_Setup()
 {
 pinMode(URTRIG,OUTPUT); // A low pull on pin COMP/TRIG
 digitalWrite(URTRIG,HIGH); // Set to HIGH

 pinMode(URPWM, INPUT); // Sending Enable PWM mode command
 for(int i=0;i<4;i++){
 //Serial.write(EnPwmCmd[i]);
 }
 }

 void sharp()
 {
   pinMode(sharpin,OUTPUT);
   cm=analogRead(sharpin);
   cm=(6762/(cm-9))-4;
   Serial.print("sharp= ");
   Serial.println(cm);
   if (cm <15)
    {
      stopmotors();
      armservo.write(160);
     
     forward(1000,leftspeed,rightspeed);
     stopmotors();
     delay(1000);
     armservo.write(80);
     delay(1000);
     backward(2000,leftspeed,rightspeed);
     turnright(1000,leftspeed,rightspeed);
     
          
   }
 } 

 void sharp2()
 {
   pinMode(sharpin,OUTPUT);
   cm=analogRead(sharpin);
   cm=(6762/(cm-9))-4;
   Serial.print("sharp= ");
   Serial.println(cm);
   if (cm <21)
    {
     turnleft(turntime,leftspeed,rightspeed);
     
   }
 }   
 
 int ping(){
  long duration, inches, cm;
  //Send Pulse
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(8);
  digitalWrite(pingPin, LOW);
  //Read Echo
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

 // convert the time into a distance
  inches = microsecondsToInches(duration);
  cm = microsecondsToCentimeters(duration);
  Serial.print("Ping:  ");
  Serial.println(cm);
 return round(cm);
 }
 void go(char a, char b){

  analogWrite (E1,a);
  digitalWrite(M1,LOW);
  analogWrite (E2,b);
  digitalWrite(M2,LOW);
  //Serial.println('f');
 }

 void turnleft(int t,char a, char b){

  analogWrite (E1,a);
  digitalWrite(M1,HIGH);
  analogWrite (E2,b);
  digitalWrite(M2,LOW);

  delay(t);
  
 }
 void turnright(int t,char a, char b){
  analogWrite (E1,a);
  digitalWrite(M1,LOW);
  analogWrite (E2,b);
  digitalWrite(M2,HIGH);
  delay(t);
 }      
 void forward(int t, char a, char b){
  analogWrite (E1,a);
  digitalWrite(M1,LOW);
  analogWrite (E2,b);
  digitalWrite(M2,LOW);
  Serial.println('D');
  delay(t);
 }
 void backward(int t, char a, char b){

  analogWrite (E1,a);
  digitalWrite(M1,HIGH);
  analogWrite (E2,b);
  digitalWrite(M2,HIGH);
  delay(t);
 }
 void stopmotors(){
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);
  //Serial.println('S');
 }      
 char scan(){
  unsigned int leftscanval, centerscanval, rightscanval;
  char choice;
  //Look left
  delay(800);
  scanservo.write(1);
  delay(800);
  leftscanval = ping();
  delay(100);
  //Look right
  scanservo.write(180);
  delay(800);
  rightscanval = ping();
  delay(100);
  //center scan servo
  scanservo.write(90);
  
  /*Serial.print("left = ");
  Serial.println(leftscanval);
  Serial.print("right = ");
  Serial.println(rightscanval);*/

  if (leftscanval< 15 && rightscanval >15){
    choice = 'r';
  }
  else if (rightscanval < 15 && leftscanval > 15){
    choice = 'l';
  }
  else{
    choice = 'x';
  }
  //Serial.print("Choice:  ");
  Serial.println(choice);
  return choice;
 }

 char Pwmscan(){
  int leftscanval, centerscanval, rightscanval;
  char choice;
  //Look left
  scanservo.write(15);
  delay(800);
  leftscanval = PWM_Mode();
  //Look right
  scanservo.write(150);
  delay(800);
  rightscanval = PWM_Mode();
  //center scan servo
  scanservo.write(80);


  if (leftscanval< 15 && rightscanval >15){
    choice = 'r';
  }
  else if (rightscanval < 15 && leftscanval > 15){
    choice = 'l';
  }
  else{
    choice = 'x';
  }
  //Serial.print("Choice:  ");
  Serial.println(choice);
  return choice;
 }


 void scan2()
 {
   int right,left,center;
  
  forward(1000,leftspeed,rightspeed);
  stopmotors();
  scanservo.write(1);
  delay(900);
  if (pingPin==-1)
  right = PWM_Mode();
  else
  right= ping();
  delay(100);
  scanservo.write(90);
  delay(900);
  if (pingPin==-1)
  center = PWM_Mode();
  else
  center = ping();
  delay(100);
  //center
  scanservo.write(180);
  delay(900);
  if(pingPin==-1)
  left = PWM_Mode();
  else
  left =ping();
  delay(100);
  scanservo.write(90);
  Serial.print("room width is ");
  Serial.println(right+left+center);
 }
  

 long microsecondsToInches(long microseconds){
  return microseconds / 74 / 2;
 }
 long microsecondsToCentimeters(long microseconds){
  return microseconds / 29 / 2;
 }