#pragma config(Sensor, in1,    touchSensor,         sensorTouch)
#pragma config(Motor,  port1,           motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  port2,           motor2,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           motor3,        tmotorNormal, openLoop)
#pragma config(Motor,  port4,           motor4,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void bump();         //Function declaration for bump
void move(int portOne,int portTwo,int portThree,int portFour,int waitTime); //Function declaration for move, which takes three integer parameters
task main()
{
  wait1Msec(2000);                  //Robot waits for 2000 milliseconds before executing program

    while(1 == 1) //Creates an infinite loop
    {

      //code to move robot forward here

motor[port1] = -63; //run at half power
motor[port2] = -63;
motor[port3] = 63;
motor[port4] = 63;
wait1Msec(2800);

motor[port1] = 0; //stops
motor[port2] = 0;
motor[port3] = 0;
motor[port4] = 0;
wait1Msec(2000);


motor[port1] = 63; //right turn
motor[port2] = 63;
motor[port3] = 63;
motor[port4] = 63;
wait1Msec(650);

motor[port1] = 0; //stops
motor[port2] = 0;
motor[port3] = 0;
motor[port4] = 0;
wait1Msec(2000);


//Robot movement stops, unless bumper swtich interupts

      if(SensorValue(touchSensor) == 0)
      {
        bump();   //Call and run the bump function

      }//end if

    }//end while



}//end main

void bump()      //Function definition for bump
{
   if(SensorValue[touchSensor] == 1)   //If bumper is pressed
   {
      move(0, 0, 0, 0, 3000);   //Call and run the move function, with portThree = -63, portTwo = -63, waitTime = 500
      move(0, 0, 0, 0, 6000);   //Call and run the move function, with portThree = 63, portTwo = -63, waitTime = 1000
   }
}


void move(int portOne,int portTwo,int portThree,int portFour,int waitTime)      //Function definition for move
{
   motor[port1] = portOne;
   motor[port2] = portTwo;
   motor[port3] = portThree;
   motor[port4] = portFour;
   wait1Msec(waitTime);            //Waits the number of milliseconds specified in the waitTime parameter
}

task bumpPress(){
  if(SensorValue[bump] == 1){
    hogCPU();
    //whatever you want the robot to do when the switch is pressed
    //If you want to stop the program, add StopAllTasks();
    //If you Don't want to resume the main task when the bumper is released, you will need
    //to insert a boolean flag or something
  }
}

task main(){
  StartTask(bumpPress);
    while(true){
      motor[port1] = -63; //run at half power
      motor[port2] = -63;
      motor[port3] = 63;
      motor[port4] = 63;
      wait1Msec(2800);

      motor[port1] = 0; //stops
      motor[port2] = 0;
      motor[port3] = 0;
      motor[port4] = 0;
      wait1Msec(2000);

      motor[port1] = 63; //right turn
      motor[port2] = 63;
      motor[port3] = 63;
      motor[port4] = 63;
      wait1Msec(650);

      motor[port1] = 0; //stops
      motor[port2] = 0;
      motor[port3] = 0;
      motor[port4] = 0;
      wait1Msec(2000);
  }
}