/*
* Runs user initialization code. This function will be started in its own task
* with the default
* priority and stack size once when the robot is starting up. It is possible
* that the VEXnet
* communication link may not be fully established at this time, so reading from
* the VEX
* Joystick may fail.
*
* This function should initialize most sensors (gyro, encoders, ultrasonics),
* LCDs, global
* variables, and IMEs.
*
* This function must exit relatively promptly, or the operatorControl() and
* autonomous() tasks
* will not start. An autonomous mode selection menu like the pre_auton() in
* other environments
* can be implemented in this task if desired.
*/
void initialize() {
print("[Init] Initializing the robot\n");
// Set up the LCD and start it
print("[Init] Setting up the LCD\n");
lcdInitMenu(1, 3, 1); // Min 1, max 3, home 1
lcdSetUpdater(implUpdateLCD);
lcdSetMenuBack(implMenuBack);
lcdSetMenuNext(implMenuNext);
lcdSetCycles(true);
// Set up our drive
print("[Init] Setting up drive motors\n");
lcdSetText(uart2, 1, "Init drive...");
driveInit(MOTOR_DRIVE_FL, MOTOR_DRIVE_BL, MOTOR_DRIVE_FR, MOTOR_DRIVE_BR);
driveSetReverse(MOTOR_DRIVE_FL_REV, MOTOR_DRIVE_BL_REV, MOTOR_DRIVE_FR_REV,
MOTOR_DRIVE_BR_REV);
// enableSlew(15); // Set slew rate to 15
// Set up our autonomous to these modes
print("[Init] Setting up autonomous modes\n");
lcdSetText(uart2, 1, "Init auton...");
autonInit(4); // 3 auton modes
// Set up our gyroscope
print("[Init] Setting gyroscope\n");
lcdSetText(uart2, 1, "Init gyro...");
// To tune: 196*((360*rotations)/gyroValue)
gyro = gyroInit(ANALOG_GYRO, 190); // default is 196, this is after tune
// Set up our encoders
print("[Init] Setting up encoders\n");
lcdSetText(uart2, 1, "Init Encs...");
initDriveEncoders();
// initPidControl();
// Sets communication port for JINX data and start task to parse incoming
// messages.
print("[Init] Setting up JINX\n");
lcdSetText(uart2, 1, "Init JINX...");
initJINX(stdout);
delay(100);
taskCreate(JINXRun, TASK_DEFAULT_STACK_SIZE, NULL, (TASK_PRIORITY_DEFAULT));
delay(100);
// Done init
print("[Init] Finished, starting LCD menu\n");
lcdSetText(uart2, 1, "Init menu...");
lcdStartMenu();
// TODO Remove
hc05Init(1, false);
}
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
char * sFileName = "debug.txt";
short nFileSize =10000;
eraseDisplay();
nxtDisplayTextLine(0, "Autonomous Period");
driveInit();
armInit(tas);
//Does armInit need 5 sec wait????
wait1Msec(10);
return;
}
