void operatorControl() {
int speed = 4000;
int lastLCDState = 0;
while (1)
{
if(lcdReadButtons(uart1) != lastLCDState)
{
if(lcdReadButtons(uart1) == 1) speed -= 100;
else if(lcdReadButtons(uart1) == 4) speed += 100;
}
lastLCDState = lcdReadButtons(uart1);
setSetPoint(&shooter, speed);
updateShooter(&shooter);
runShooter(&shooter);
if(isShooterUpToSpeed(&shooter))
{
lcdSetText(uart1, 1, "YES :)");
}
else
{
lcdSetText(uart1, 1, "NO :(");
}
delay(20);
}
}
bool PewPewBot::shootAllBalls(double targetTime = -1)
{
updateShooter(true);
collector->setAutomatic(true);
if (System::currentTimeMillis() >= targetTime)
{
if (!collector->isShooting() && shooter->isStableReady())
{
stableCount = 0;
collector->requestShot();
hasResetItem = true;
}
if (!collector->isCollecting() && !collector->isShooting())
{
if (collector->automatic)
{
for (int slot = 0; slot<3; slot++)
{
if (collector->getSense(slot))
{
return false;
}
}
updateShooter(false);
hasResetItem = false;
return true;
} else if (hasResetItem)
{
stableCount++;
if (stableCount > 250)
{
updateShooter(false);
stableCount = 0;
hasResetItem = false;
return true;
}
}
}
}
return false;
}
void shootingModes(){
if (manShootMode == 0){
updateShooter();
}
if (manShootMode == 1){
if (stick2.GetRawAxis(triggerR) > 0){
t_motor.Set(scaler(-1*(stick2.GetRawAxis(triggerR))));
}
else if (stick2.GetRawAxis(triggerL) > 0){
t_motor.Set(scaler(stick2.GetRawAxis(triggerL)));
}
else if (stick2.GetRawAxis(triggerL) == 0 and stick2.GetRawAxis(triggerR) == 0){
t_motor.Set(0);
}
if (stick2.GetRawButton(buttonX) == true){
myServo->SetAngle(175);
}
if (stick2.GetRawButton(buttonX) == false){
myServo->SetAngle(5);
}
}
}
void PewPewBot::Autonomous()
{
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(0.2);
drive->shift(false);
drive->resetEncoders();
autonomousMode = kCollect;
stableCount = 0;
//Cleaning
collector->clean();
double startTime = System::currentTimeMillis();
//Spinup shooter
updateShooter(true);
drive->setLight(true);
hasResetItem = false;
while (IsAutonomous() && !IsDisabled())
{
if (PID_SLIDER> .2095)
{
shooter->setPIDAdjust(0.0762 * log(PID_SLIDER) + 0.0407);
} else
{
shooter->setPIDAdjust(-0.15);
}
shooter->update();
#if KINECT
if (kinect->getKinectMode())
{
autonomousMode = kKinect;
}
#endif
drive->updateCompressor();
collector->update();
#if KINECT
kinect->update();
#endif
updateDriverStation();
switch (autonomousMode)
{
case kDoYawAlign:
break;
case kDoDepthAlign:
if (lineDepthAlign())
autonomousMode = kCollect;
break;
case kCollect:
if (collectAllBalls())
autonomousMode = kShoot;
break;
case kShoot:
if (shootAllBalls(AUTONOMOUS_DELAY_SWITCH?startTime + AUTONOMOUS_DELAY:-1))
{
//If there are balls left, cycle back to the collect stage
if (collector->getSense(0) || collector->getSense(1)
|| collector->getSense(2))
{
autonomousMode = kCollect;
} else if (AUTONOMOUS_FULL_AUTO_SWITCH)
{
autonomousMode = kRotate180;
}
#if KINECT
else if (kinect->hasKinect())
{
autonomousMode = kKinect;
}
#endif
else
{
autonomousMode = kDone;
}
}
break;
case kRotate180:
if (rotateRobot(180, 5))
autonomousMode = kMoveToBridge;
break;
case kMoveToBridge:
if (!hasResetItem)
{
drive->resetEncoders();
hasResetItem = true;
}
drive->setSpeedL(.25);
drive->setSpeedR(.25);
double distance = (drive->getLPosition() + drive->getRPosition())
/ 2.0;
if (fabs(distance - 7.0) < 0.5)
{
hasResetItem = false;
autonomousMode = kTipBridge;
}
break;
case kTipBridge:
drive->tip(true);
autonomousMode = kDone;
break;
case kKinect:
#if KINECT
kinectCode();
#else
autonomousMode = kDone;
#endif
break;
default:
//We are done
updateShooter(false);
break;
}
drive->setLight(false);
GetWatchdog().Feed();
Wait(0.02);
}
}
/**
* 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");
}
}
