void arcadeDrive(Drive drive, int mag, int rot)
{
int leftSpeed = limit(mag + rot, 127, -127);
int rightSpeed = limit(mag - rot, 127, -127);
tankDrive(drive, leftSpeed, rightSpeed);
}
task usercontrol()
{
while(true)
{
tankDrive();
TreadDrive();
PIDArmControl();
}
}
task usercontrol()
{
while (true)
{
tankDrive();
armDrive();
TreadDrive();
}
}
/*
* main
* main function of the robot
*/
task main(){
while(true){
//Check if we should drive with slow sleeds.
slow = 1.0 - (vexRT[JOY_BTN_SLOW] * SLOW_SPEED_MULTIPLIER);
//Call to the drive methods
tankDrive();
moveBtns();
//Call the Arm control method
runArm();
//Call the Calw control method
runClaw();
//If we are pushing our special button
//Set the arm motor to move to the pre-defined position
if(vexRT[JOY_BTN_MOVE_POINT]) startTask(task_move_to_height, kDefaultTaskPriority);
}
}
/**
* Runs the user operator control code.
*
* This function will be started in its own task with the default priority and stack size whenever the robot is enabled via the Field Management System or the VEX Competition Switch in the operator control mode. If the robot is disabled or communications is lost, the operator control task will be stopped by the kernel. Re-enabling the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will run the operator control task. Be warned that this will also occur if the VEX Cortex is tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl()
{
int lastIncrement = 0;
int lastDecrement = 0;
int lastIntake1InButton = 0;
int intake1RunningIn = 0;
int lastIntake1OutButton = 0;
int intake1RunningOut = 0;
while (true)
{
tankDrive(robotDrive, OIGetDriveLeft(), OIGetDriveRight());
if(OIGetIntake1In() && !lastIntake1InButton)
{
intake1RunningIn = !intake1RunningIn;
intake1RunningOut = 0;
if(intake1RunningIn)
{
intake1In(robotIntake);
}
else
{
intake1Stop(robotIntake);
}
}
else if(OIGetIntake1Out() && !lastIntake1OutButton)
{
intake1RunningIn = 0;
intake1RunningOut = !intake1RunningOut;
if(intake1RunningOut)
{
intake1Out(robotIntake);
}
else
{
intake1Stop(robotIntake);
}
}
lastIntake1InButton = OIGetIntake1In();
lastIntake1OutButton = OIGetIntake1Out();
if(OIGetIntake2In())
{
intake2In(robotIntake);
}
else if(OIGetIntake2Out())
{
intake2Out(robotIntake);
}
else
{
intake2Stop(robotIntake);
}
if(OIShooterOn())
{
turnShooterOn(&robotShooter);
}
else if(OIShooterOff())
{
turnShooterOff(&robotShooter);
}
if(OIShooterUp() && !lastIncrement)
{
incrementShooterSP(&robotShooter, 100);
}
else if(OIShooterDown() && !lastDecrement)
{
incrementShooterSP(&robotShooter, -100);
}
lastIncrement = OIShooterUp();
lastDecrement = OIShooterDown();
updateShooter(&robotShooter);
runShooter(&robotShooter);
lcdPrint(uart1, 2, "SP: %d", robotShooter.SP);
puts("6");
lcdPrint(uart1, 1, "PV: %d", robotShooter.processVariable);
if(isShooterUpToSpeed(&robotShooter))
{
lcdSetBacklight(uart1, true);
}
else
{
lcdSetBacklight(uart1, false);
}
delay(25);
puts("hi");
}
}
void DriveTrainSubsystem::arcadeDrive(float drive, float turn)
{
tankDrive((-turn) - drive, (-turn) + drive);
}
