/*
* 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() {
// create mutexes
irDetectorMutex = mutexCreate();
piSetpointMutex = mutexCreate();
//initialize IMEs
imeInitializeAll();
}
void imeReinitialize(){
print("Starting IMEs....\n\r");
imeShutdown();
imeReset(0);
imeReset(1);
imeInitializeAll();
print("Done!\n\r");
}
/*
* 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() {
imecount=imeInitializeAll();
r_encoder = encoderInit(ENC_RIGHT_TOP,ENC_RIGHT_BOT,1);
l_encoder = encoderInit(ENC_LEFT_TOP,ENC_LEFT_BOT,0);
pinMode(CON_SWITCH,INPUT);
//lcdinit(LCD_PORT);
}
/**
* 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() {
imeInitializeAll();
robotDrive = initDrive(initPantherMotor(8,0), initPantherMotor(1,1),
initPantherMotor(9,0), initPantherMotor(2,1),
initPantherMotor(10,0), initPantherMotor(3,1),
encoderInit(1, 2, 0), encoderInit(3,4,0));
PIDController shooterPID = initPIDController(.0125, 0, 0, .028, 0, 100);
robotShooter = initShooter(shooterPID, initPantherMotor(4,0), initPantherMotor(7,0), 2800, 0, 0);
}
/*
* Call this from the default Initialize function.
* After, be sure to reinitialize each motor you will be using on your robot.
*/
void riceBotInitialize() {
Motor *MOTDTFrontRight = newMotor();
Motor *MOTDTFrontMidRight = newMotor();
Motor *MOTDTMidRight = newMotor();
Motor *MOTDTBackRight = newMotor();
Motor *MOTDTFrontLeft = newMotor();
Motor *MOTDTFrontMidLeft = newMotor();
Motor *MOTDTMidLeft = newMotor();
Motor *MOTDTBackLeft = newMotor();
Motor *MOTARMFront = newMotor();
Motor *MOTARMRight = newMotor();
Motor *MOTARMLeft = newMotor();
Motor *MOTARMTopRight = newMotor();
Motor *MOTARMBottomRight = newMotor();
Motor *MOTARMTopLeft = newMotor();
Motor *MOTARMBottomLeft = newMotor();
Motor *MOTCOL = newMotor();
printf("%d\n\r", imeInitializeAll());
imeReset(IMEARMLEFT);
imeReset(IMEARMRIGHT);
encDTLeft = encoderInit(0, 0, false);
encDTRight = encoderInit(0, 0, false);
encARMLeft = encoderInit(0, 0, false);
encARMRight = encoderInit(0, 0, false);
Ricencoder *ENCDT = newEncoder();
Ricencoder *ENCARM = newEncoder();
// gyro = gyroInit(0, 196);
// gyroVal = gyroGet(gyro);
analogCalibrate(POTARMLeft);
analogCalibrate(POTARMRight);
analogCalibrate(POTARMFront);
Pid *PidARMLeft = newPid();
Pid *PidARMRight = newPid();
Pid *PidARMFront = newPid();
}
/**
* Runs pre-initialization code.
*
* This function will be started in kernel mode one time while the VEX Cortex is starting up. As the scheduler is still paused, most API functions will fail.
*
* The purpose of this function is solely to set the default pin modes (pinMode()) and port states (digitalWrite()) of limit switches, push buttons, and solenoids. It can also safely configure a UART port (usartOpen()) but cannot set up an LCD (lcdInit()).
*/
void initializeIO() {
imeInitializeAll();
lcdInit(uart1);
}
void integratedEncoderPrimaryInitialization() {
connectedIntegratedMotorEncoders = imeInitializeAll();
}
/*
* 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()
{
imeInitializeAll();
shooter = initSingleFlyWheelShooter(0.1,0.0000002,0.01,0.015, 100, 2, 0, 0, 0);
}
