#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#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               !!*//

/* The code above this comment was automatically generated by ROBOTC based on the Motors
 * and Sensors Setup command under the Robot menu. */

/*
 * RC_Dual --- Put either autonomous or teleop code in this program using ROBOTC
 *
 *  This program is written for ROBOTC version 1.46 and to be used for the FIRST
 *  Tech Challenge tournaments.  It is a dual-mode program where either the autonomous
 *  or tele-op portions of the code are in this program.  You should make copies of this
 *  program for your teleop code and another one for your autonomous code and add to it
 *
 *
 *  This program is protected by United States copyright law and may not be
 *  reproduced, distributed, transmitted, displayed or published without the
 *  prior written permission of FIRST, NPO Services, LLC or Mannie Lowe.  You
 *  may not alter or remove any copyright or other notice from copies of the
 *  content.  However, you are free to use this source code for your personal,
 *  non-commercial use.
 *  Copyright ©2008 NPO Services, LLC - Mannie Lowe.  All rights are reserved.
 *
 *
 */

#include "JoystickDriver.c"

void stopMotors()
{
  /* This subroutine only stops the 12V motors defined in the Motors and Sensors setup area.
   * If you add more motors and or servos, please update this routine to stop their movement. */

  motor[motorB] = 0;
  motor[motorA] = 0;

  /* If you have servos, a way to stop their movement is with the following code:
  servoTarget[servo1] = ServoValue[servo1];
  */
}

task main()
{
  while (true)
  {
    getJoystickSettings(joystick);

    // has the FMS stopped the program
    if (joystick.StopPgm)
    {
      nxtDisplayTextLine(2, "Disabled");
      stopMotors();
      wait1Msec(50);
      continue;   // go back into top of while loop
    }

    nxtDisplayTextLine(2, "Enabled");

    /* put the rest of your code here */

    nMotorEncoder[motorB] = 0;
    nMotorEncoder[motorA] = 0;

    nMotorEncoderTarget[motorB] = 3000;
    nMotorEncoderTarget[motorA] = 3000;

    motor[motorB] = 50;
    motor[motorA] = 50;

    while(nMotorRunState[motorB] != runStateIdle || nMotorRunState[motorA] != runStateIdle)
    {
      //Idle Loop
    }
  }
}