/*
* 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() {
//Init Drive Motors
frontLeftWheel = createMotor(5, false);
frontRightWheel = createMotor(6, true);
backLeftWheel = createMotor(7, false);
backRightWheel = createMotor(8, true);
//Init Lift Motors
frontLeftLift = createMotor(1, false);
backLeftLift = createMotor(3, false);
frontRightLift = createMotor(2, true);
backRightLift = createMotor(4, false);
//Init Controller Buttons
liftUp = controlButtonInit(5, JOY_UP);
liftDown = controlButtonInit(5, JOY_DOWN);
forward_backward_drive = controlStickInit(3);
left_right_drive = controlStickInit(4);
forward_backward_strafe = controlStickInit(2);
left_right_strafe = controlStickInit(1);
}
FocusMotorTwoPolys::FocusMotorTwoPolys(const RigConfig &rigConfig) {
motor = createMotor(rigConfig, "");
motor->connect();
lastDistance = 0.0;
createTransferFunctions(rigConfig);
}
