#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S3,     Light,               sensorLightActive)
#pragma config(Sensor, S4,     Sonar,               sensorSONAR)
#pragma config(Motor,  motorB,          ,              tmotorNormal, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int autotimer;
int mode2;
int mode3;

#include "JoystickDriver.c"

task main()

{
  autotimer = 0;
  mode2 = 1;
  mode3 = 1;

  while (autotimer < 300)
  {

     if (mode2 == 1)
     {
       if (SensorValue(Sonar) < 30)
       {
          mode2 = 2;
      }
    }
     if (mode2 == 1)
     {
       motor[motorD] = 100;
       motor[motorE] = 100;
    }
    if (mode2 == 2)
    {
       motor[motorD] = 75;
       motor[motorE] = - 75;
       wait10Msec (85);
       mode2 = 3;
       autotimer = autotimer + 10;
    }
    if (mode2 == 3)
    {
       motor[motorD] = 100;
       motor[motorE] = 100;
       if (SensorValue(Light) < 25)
       {
         wait10Msec (300);
         motor[motorD] = 75;
         motor[motorE] = - 75;
         wait10Msec (85);
         autotimer = autotimer + 40;
         motor[motorD] = 100;
         motor[motorE] = 100;
         wait10Msec (100);
         autotimer = autotimer + 10;
         if (SensorValue(Sonar) < 5);
         {
           mode3 = 2;
         }
      }
    }
    if (mode2 == 3 && mode3 == 2)
    {
       motor[motorD] = 0;
       motor[motorE] = 0;
       motor[motorA] = 100;
       wait10Msec (100);
       motor[motorA] = -100;
       motor[motorA] = 0;
       autotimer = 300;
    }
    wait10Msec (9);
    ++autotimer;
  }


  while(true)
  {
    while (true)
    {
      getJoystickSettings(joystick);
      motor[motorD] = joystick.joy1_y1;
      motor[motorE] = joystick.joy2_y1;

      if(joy1Btn(1))
      {
         motor[motorA] = 128;
      }
      else
      {
          motor[motorA] = ~ 128;
         motor[motorA] = 0;
      }
      if(joy2Btn(1))
      {
         motor[motorB] = 100;
      }
      else
      {
         motor[motorB] = 0;
      }
    }
  }
}

#pragma config(Motor,  mtr_S1_C1_1,     motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motor2,        tmotorNormal, openLoop)