int switchPin= 3; //  bump switch on pin 3
int swval; // Variable for reading switch status
int ultraSoundSignal = 2; // Ultrasound signal pin
int val = 0;
int ultrasoundValue = 0;
int timecount = 0; // Echo counter
//int ledPin = 13; // LED connected to digital pin 13

//left motor
int InA1 = 7;
int InB1 = 6;
int PWM1 = 5;  //PWM1 connects to pin 5

//right motor
int InA2 = 8;
int InB2 = 9;
int PWM2 = 10;  //PWM1 connects to pin 10

//pwm values
int PWM1_val = 255; //(25% = 64; 50% = 127; 75% = 191; 100% = 255)
int PWM2_val = 191; //(25% = 64; 50% = 127; 75% = 191; 100% = 255)
int PWM3_val = 127; //(25% = 64; 50% = 127; 75% = 191; 100% = 255)
int PWM4_val = 64;



void setup() {
  pinMode(switchPin, INPUT); // Sets the digital pin as input
  //pinMode(ledPin, OUTPUT); // Sets the digital pin as output

  pinMode(InA1, OUTPUT);
  pinMode(InB1, OUTPUT);
  pinMode(PWM1, OUTPUT);

  pinMode(InA2, OUTPUT);
  pinMode(InB2, OUTPUT);
  pinMode(PWM2, OUTPUT);
}

void loop() {



/* Start Scan
* --------------------------------------------------
*/{
timecount = 0;
val = 0;
pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output

/* Send low-high-low pulse to activate the trigger pulse of the sensor
* -------------------------------------------------------------------
*/

digitalWrite(ultraSoundSignal, LOW); // Send low pulse
delayMicroseconds(2); // Wait for 2 microseconds
digitalWrite(ultraSoundSignal, HIGH); // Send high pulse
delayMicroseconds(5); // Wait for 5 microseconds
digitalWrite(ultraSoundSignal, LOW); // Holdoff

/* Listening for echo pulse
* -------------------------------------------------------------------
*/

pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input
val = digitalRead(ultraSoundSignal); // Append signal value to val
while(val == LOW) { // Loop until pin reads a high value
val = digitalRead(ultraSoundSignal);
}

while(val == HIGH) { // Loop until pin reads a high value
val = digitalRead(ultraSoundSignal);
timecount = timecount +1; // Count echo pulse time
}

/* Lite up LED if any value is passed by the echo pulse
* -------------------------------------------------------------------
*/

//if(timecount > 0){
//digitalWrite(ledPin, HIGH);
//delay(50); //LED on for 50 microseconds
//digitalWrite(ledPin, LOW);
{

/* Delay of program
* -------------------------------------------------------------------
*/

delay(100);

}
/* Action based on data
* -------------------------------------------------------------------
*/

{
ultrasoundValue = timecount; // Append echo pulse time to ultrasoundValue


}
if (ultrasoundValue > 1100) // set distance to objects, 1200/pwm3 for outside, 1000/pwm4 for inside


{
/* Drive straight forward
*-----------------------------------------------
*/
digitalWrite(InA1, LOW);
digitalWrite(InB1, HIGH);
analogWrite(PWM1, PWM3_val);

digitalWrite(InA2, LOW);
digitalWrite(InB2, HIGH);
analogWrite(PWM2, PWM3_val);

}
/*------------------------------------------------
*/
else
/* Turn hard right
*---------------------------------------------
*/
{
digitalWrite(InA1, HIGH);
digitalWrite(InB1, HIGH);
analogWrite(PWM1, PWM3_val);
digitalWrite(InA2, HIGH);
digitalWrite(InB2, HIGH);
analogWrite(PWM2, PWM3_val);
delay(250);

digitalWrite(InA1, LOW);
digitalWrite(InB1, HIGH);
analogWrite(PWM1, PWM3_val);

digitalWrite(InA2, HIGH);
digitalWrite(InB2, LOW);
analogWrite(PWM2, PWM3_val);
delay(350);

digitalWrite(InA1, HIGH);
digitalWrite(InB1, HIGH);
analogWrite(PWM1, PWM3_val);
digitalWrite(InA2, HIGH);
digitalWrite(InB2, HIGH);
analogWrite(PWM2, PWM3_val);
delay(250);



/*----------------------------------------------
*/
}

/* Backup and turn right when switch gets bumped and closes circuit to ground
*---------------------------------------------------
*/
/*
*/

digitalWrite(switchPin, HIGH); // Sets the pin to high
swval = digitalRead(switchPin); // Read input value and store it
if (swval == LOW) {

digitalWrite(InA1, HIGH);
digitalWrite(InB1, HIGH);
digitalWrite(InA2, HIGH);
digitalWrite(InB2, HIGH);


delay(100);

digitalWrite(InA1, HIGH);
digitalWrite(InB1, LOW);
analogWrite(PWM1, PWM3_val);

 digitalWrite(InA2, HIGH);
 digitalWrite(InB2, LOW);
 analogWrite(PWM2, PWM3_val);

 delay(500);

  digitalWrite(InA1, HIGH);
digitalWrite(InB1, HIGH);
analogWrite(PWM1, PWM1_val);

digitalWrite(InA2, HIGH);
digitalWrite(InB2, HIGH);
analogWrite(PWM2, PWM2_val);

delay(250);

 digitalWrite(InA1, HIGH);
digitalWrite(InB1, LOW);
analogWrite(PWM1, PWM1_val);

digitalWrite(InA2, LOW);
digitalWrite(InB2, HIGH);
analogWrite(PWM2, PWM2_val);
delay(200);

   digitalWrite(InA1, HIGH);
digitalWrite(InB1, HIGH);
analogWrite(PWM1, PWM1_val);

digitalWrite(InA2, HIGH);
digitalWrite(InB2, HIGH);
analogWrite(PWM2, PWM2_val);

delay(250);


}

/*------------------------------------------------
*/


}
}