void Autonomous(void)
{
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Autonomous Mode");
dsLCD->UpdateLCD();
}
/**
* Runs the motors with arcade steering.
*/
void OperatorControl(void)
{
HSLImage *Himage;
Threshold targetThreshold(247, 255, 60, 140, 10, 50);
BinaryImage *matchingPixels;
vector<ParticleAnalysisReport> *pReport;
//myRobot->SetSafetyEnabled(true);
Saftey->SetEnabled(false);
AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11");
mycam.WriteResolution(AxisCamera::kResolution_640x480);
mycam.WriteCompression(20);
mycam.WriteBrightness(25);
Wait(3.0);
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
float X[2];
float Y[2];
float Z[2];
while(IsOperatorControl())
{
X[1] = Stick1->GetX();
X[2] = Stick2->GetX();
Y[1] = Stick1->GetY();
Y[2] = Stick2->GetY();
Z[1] = Stick1->GetZ();
Z[2] = Stick2->GetZ();
Jaguar1->Set(Y[1]);
Jaguar2->Set(Y[2]);
Wait(0.005);
if (mycam.IsFreshImage())
{
Himage = mycam.GetImage();
matchingPixels = Himage->ThresholdHSL(targetThreshold);
pReport = matchingPixels->GetOrderedParticleAnalysisReports();
for (unsigned int i = 0; i < pReport->size(); i++)
{
printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y);
}
delete Himage;
delete matchingPixels;
delete pReport;
}
}
//myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick)
//Wait(0.005); // wait for a motor update time
}
DriverStationLCDTextExample()
{
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
control = new LCDControl();
dsLCD->UpdateLCD();
}
void OperatorControl()
{
// Loop counter to ensure that the program is running (debug helper
// that can be removed when things get more stable)
int sanity, bigSanity = 0;
gamepad.Update();
while (IsOperatorControl() && IsEnabled())
{
controls = Controls::GetInstance();
controls->SetSpeed(LEFT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
controls->SetSpeed(RIGHT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
gamepad.Update();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity);
dsLCD->UpdateLCD();
sanity++;
if (0 == sanity % 20)
{
bigSanity++;
}
Wait(0.05); // wait for a motor update time
}
}
void RobotInit(void)
{
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->UpdateLCD();
//blnShift = true;
}
void Zed::updateDriverStation()
{
DriverStationLCD* lcd = DriverStationLCD::GetInstance();
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, 0, "Shooter Speed: %f",
shooterSpeed);
lcd->UpdateLCD();
}
RobotDemo(void) :
signal(3), signalControlVoltage(7) {
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "SonarTest");
dsLCD->UpdateLCD();
}
Hohenheim(void) {
driverStation = DriverStation::GetInstance();
dsLCD = DriverStationLCD::GetInstance();
pneumaticsControl = PneumaticsControl::getInstance();
shooterControl = ShooterControl::getInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hohenheim 2014 V 3.1");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
}
RobotDemo(void):
myRobot(1, 2), // these must be initialized in the same order
stick(1) // as they are declared above.
{
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "XboxController2");
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
}
SebastianRobot(void) {
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Sebastian V24.2");
dsLCD->UpdateLCD();
GetWatchdog().SetEnabled(false);
led0 = new Relay(2);
led1 = new Relay(3);
flashCount = 0;
led0->Set(Relay::kOff);
led1->Set(Relay::kOff);
isFlashing=false;
}
//should move this to helper function
void robot::feed()
{
DriverStationLCD *lcd = DriverStationLCD::GetInstance();
lcd->Clear();
float th = gyro.getangle();
a = qmod(th * dt + a, -180, 180);
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "BUILD: %i", BUILD);
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "%f", gyrob.GetAngle());
lcd->PrintfLine(DriverStationLCD::kUser_Line3, "%f", arma.GetVoltage());
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%f", armb.GetVoltage());
//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "%f", aa);
lcd->UpdateLCD();
}
void Disabled()
{
while(IsDisabled())
{
LEDLight->Set(Relay::kForward);
rpi->Read();
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "R: %i", rpi->GetMissingPacketcount());
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "x: %i", rpi->GetXPos());
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "y: %i", rpi->GetYPos());
lcd->UpdateLCD();
}
}
void Print ()
{
if (PrintTime.Get() > PRINT_TIME)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK));
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate);
//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed());
lcd->UpdateLCD();
PrintTime.Reset();
PrintTime.Start();
}
}
void OperatorControl(void) {
XboxController *xbox = XboxController::getInstance();
bool isEndGame = false;
GetWatchdog().SetEnabled(true);
_driveControl.initialize();
//_poleVaultControl.initialize();
shooterControl.InitializeOperator();
while (IsEnabled() && IsOperatorControl()) {
GetWatchdog().Feed();
dsLCD->Clear();
if (xbox->isEndGame()) {
isEndGame = !isEndGame;
}
if (!isEndGame) {
shooterControl.Run();
//_poleVaultControl.act();
_driveControl.act();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Normal");
led0->Set((shooterControl.getLED1())?Relay::kOn: Relay::kOff);
led1->Set((shooterControl.getLED2())?Relay::kOn: Relay::kOff);
}
else {
shooterControl.RunEndGame();
//_poleVaultControl.actEndGame();
_driveControl.actEndGame();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "End Game");
flashCount--;
if (flashCount<=0){
isFlashing = !isFlashing;
flashCount=FLASHTIME;
}
led0->Set((isFlashing)?Relay::kOn: Relay::kOff);
led1->Set((isFlashing)?Relay::kOn: Relay::kOff);
}
// dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Flash: %i", flashCount);
dsLCD->UpdateLCD();
Wait(WAIT_TIME); // wait for a motor update time
}
GetWatchdog().SetEnabled(false);
}
void OperatorControl(void) {
GetWatchdog().SetEnabled(true);
dsLCD->Clear();
dsLCD->UpdateLCD();
driveControl.initialize();
pneumaticsControl->initialize();
shooterControl->initialize();
while (IsOperatorControl() && IsEnabled()) {
GetWatchdog().Feed();
driveControl.run();
shooterControl->run();
dsLCD->UpdateLCD();
Wait(0.005); // wait for a motor update time
}
pneumaticsControl->disable();
}
RobotDemo(void):
gamepad(3)
{
dsLCD = DriverStationLCD::GetInstance();
// Output the program name and build date/time in the hope that this will help
// us catch cases where we are downloading a program other than the one
// we think we are downloading. Keep in mind that if this source file
// does not change (and you don't do a complete rebuild) the timestamp
// will not change.
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Menu");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
}
FRC2994_2014():
leftFrontDrive(LEFT_FRONT_DRIVE_PWM),
leftRearDrive(LEFT_REAR_DRIVE_PWM),
rightFrontDrive(RIGHT_FRONT_DRIVE_PWM),
rightRearDrive(RIGHT_REAR_DRIVE_PWM),
robotDrive(&leftFrontDrive, &leftRearDrive, &rightFrontDrive, &rightRearDrive),
rightStick(RIGHT_DRIVE_STICK)
{
// Print an I'M ALIVE message before anything else. NOTHING ABOVE THIS LINE.
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, ROBOT_NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " " __TIME__);
dsLCD->UpdateLCD();
robotDrive.SetExpiration(0.1);
}
/**
* Runs during test mode ```````
*/
void Test() {
TestMode tester(m_ds);
driveDistanceRight.Reset();
driveDistanceRight.Start();
ballGrabber.resetSetPoint();
shooter.motorShutOff();
while (IsTest() && IsEnabled()){
lcd->Clear();
tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick,
&testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic,
&ballGrabber.ballDetector, &analogTestSwitch,
&shooter, &ballGrabber
);
lcd->UpdateLCD();
Wait(0.1);
}
driveDistanceRight.Stop();
}
RobotDemo(void):
leftDriveMotor(LEFT_DRIVE_PWM),
rightDriveMotor(RIGHT_DRIVE_PWM),
myRobot(&leftDriveMotor, &rightDriveMotor), // these must be initialized in the same order
stick(1), // as they are declared above.
stick2(2),
gamepad(3),
collectorMotor(PICKUP_PWM),
indexerMotor(INDEX_PWM),
shooterMotor(SHOOTER_PWM),
armMotor (ARM_PWM),
leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B),
shifter(SHIFTER_A,SHIFTER_B),
greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
potentiometer(ARM_ROTATION_POT),
indexSwitch(INDEXER_SW),
greenClawLockSwitch(CLAW_1_LOCK_SENSOR),
yellowClawLockSwitch(CLAW_2_LOCK_SENSOR),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
jogTimer(),
shooterTimer()
{
m_collectorMotorRunning = false;
m_shooterMotorRunning = false;
m_jogTimerRunning = false;
m_shiftCount = MAX_SHIFTS;
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
dsLCD->UpdateLCD();
myRobot.SetExpiration(0.1);
shifter.Set(DoubleSolenoid::kReverse);
leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE);
leftDriveEncoder.SetMaxPeriod(1.0);
leftDriveEncoder.SetReverseDirection(true); // change to true if necessary
leftDriveEncoder.Start();
}
FRC2994_2014():
leftFrontDrive(LEFT_FRONT_DRIVE_PWM),
leftRearDrive(LEFT_REAR_DRIVE_PWM),
rightFrontDrive(RIGHT_FRONT_DRIVE_PWM),
rightRearDrive(RIGHT_REAR_DRIVE_PWM),
leftCenterDrive(CENTER_LEFT_DRIVE_PWM),
rightCenterDrive(CENTER_RIGHT_DRIVE_PWM),
intake(INTAKE_MOTOR_PWM),
rightWinch(RIGHT_WINCH_MOTOR_PWM),
leftWinch(LEFT_WINCH_MOTOR_PWM),
robotDrive(leftFrontDrive, leftRearDrive, leftCenterDrive, rightCenterDrive, rightFrontDrive, rightRearDrive),
rightStick(RIGHT_DRIVE_STICK),
leftStick(LEFT_DRIVE_STICK),
gamepad(GAMEPAD_PORT),
shifters(SHIFTER_A, SHIFTER_B),
arm(ARM_A, ARM_B),
eject(EJECT_A, EJECT_B),
winchSwitch(WINCH_SWITCH),
leftDriveEncoder(LEFT_ENCODER_A, LEFT_ENCODER_B),
rightDriveEncoder(RIGHT_ENCODER_A, RIGHT_ENCODER_B),
compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
// Robot starts off in a loaded state so a ball can be fit in
loaded(true),
loading(false),
armDown(false)
{
// Print an I'M ALIVE message before anything else. NOTHING ABOVE THIS LINE.
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, ROBOT_NAME);
dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, __DATE__ " " __TIME__);
dsLCD->UpdateLCD();
ds = DriverStation::GetInstance();
// Set the experation to 1.5 times the loop speed.
robotDrive.SetExpiration(LOOP_PERIOD*1.5);
leftDriveEncoder.SetReverseDirection(true);
}
void OperatorControl(void)
{
// Loop counter to ensure that the program us running (debug helper
// that can be removed when things get more stable)
int sanity, bigSanity = 0;
while (IsOperatorControl() && IsEnabled())
{
dsLCD->Clear();
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode");
dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity);
dsLCD->UpdateLCD();
sanity++;
if (0 == sanity % 20)
{
bigSanity++;
}
Wait(1.0); // wait for a motor update time
}
}
virtual void RobotInit() {
CommandBase::init();
DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
dsLCD->UpdateLCD();
//Wait(0.1);
//_autonomousCommand = new AutonomousCommand();
_autonomousCommand = NULL;
_teleopCommand = NULL;
_moveToAndDropBridgeCmd = new MoveToAndDropBridgeCmd();
_noOpCmd = new NoOpCmd();
_driveInSquareCmd = new DriveInSquareCmd();
_kinectDriveCmd = new KinectDriveCmd(CommandBase::oi->kinectLeft, CommandBase::oi->kinectRight, CommandBase::oi->kinectBridgeBtn);
_testRobotCmd = new TestRobotCmd(CommandBase::oi->driveTrigger);
_autoFireCmd = new AutonomousFireCmd();
//_driveWithJoysticksCmd = new DriveWithJoysticksCmd();
_autoChooser = new SendableChooser();
// _autoChooser->AddDefault("Autonomous Fire", _autoFireCmd);
_autoChooser->AddDefault("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
// _autoChooser->AddObject("Move To And Drop Bridge", _moveToAndDropBridgeCmd);
_autoChooser->AddObject("Autonomous Fire", _autoFireCmd);
_autoChooser->AddObject("Drive with Kinect", _kinectDriveCmd);
_autoChooser->AddObject("Drive In Square", _driveInSquareCmd);
SmartDashboard::GetInstance()->PutData("AutoMenu", _autoChooser);
_teleopChooser = new SendableChooser();
_teleopChooser->AddDefault("Drive With Joysticks", _noOpCmd);
_teleopChooser->AddObject("Test Robot", _testRobotCmd);
SmartDashboard::GetInstance()->PutData("TeleopMenu", _teleopChooser);
// SmartDashboard::GetInstance()->PutData("SchedulerData", Scheduler::GetInstance());
// SmartDashboard::GetInstance()->PutData("DriveTrainSubsystem", CommandBase::driveTrainSubsystem);
// SmartDashboard::GetInstance()->PutData("AzimuthSubsystem", CommandBase::azimuthSubsystem);
}
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous(void)
{
Saftey->SetEnabled(false);
//myRobot->SetSafetyEnabled(false);
//myRobot->Drive(0.5, 0.0); // drive forwards half speed
dsLCD = DriverStationLCD::GetInstance();
dsLCD->Clear();
//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Hello World" );
//dsLCD->UpdateLCD();
Wait(0.5);
ds = DriverStation::GetInstance();
switch(ds->GetLocation())
{
case 1:
//Execute Autonomous code #1
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 1");
break;
case 2:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 2");
//Execute Autonomous code #2
break;
case 3:
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 3");
//Execute Autonomous code #3
break;
}
dsLCD->UpdateLCD();
Saftey->SetEnabled(false);
Wait(0.5); // for 2 seconds
delete Jaguar1;
delete Jaguar2;
delete Saftey;
delete dsLCD;
delete ds;
}
void Autonomous(void) {
GetWatchdog().SetEnabled(true);
bool isHybrid = false;
Kinect* kinect = Kinect::GetInstance();
isHybrid = (kinect->GetNumberOfPlayers() > 0);
if (!isHybrid) {
_driveControl.initializeAutonomous();
shooterControl.InitializeAutonomous();
_poleVaultControl.initialize();
} else {
_driveControl.initializeHybrid();
shooterControl.InitializeHybrid();
_poleVaultControl.initialize();
}
while (IsEnabled() && IsAutonomous()) {
GetWatchdog().Feed();
dsLCD->Clear();
if (!isHybrid) { //Run Autonomous
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
"Autonomous Mode");
//if (_driveControl.RunAuto()) {
shooterControl.RunAuto();
//}
// if(_driveControl.RunAuto()){
// _poleVaultControl.deploy();
// }
} else { //Run Hybrid
dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hybrid Mode");
shooterControl.Run();
_driveControl.act();
_poleVaultControl.act();
}
dsLCD->UpdateLCD();
Wait(WAIT_TIME);
}
GetWatchdog().SetEnabled(false);
}
void OperatorControl()
{
if(!m_FromAutonomous){
init();
}
m_FromAutonomous = false;
driveTrain.SetSafetyEnabled(true);
int printDelay = 0;
int shootDelay = 0;
//bool SavePreferencesToFlash = false;
while (IsOperatorControl() && IsEnabled())
{
/*
bool SavePreferences = gamePad.GetRawButton(8);
if (SavePreferences){
double elevatorAngleValue = SmartDashboard::GetNumber("Angle");
dashboardPreferences->PutDouble("Angle", elevatorAngleValue);
SavePreferencesToFlash = true;
}
*/
printDelay ++;
float rJoyStick = limitSpeed(rightJoyStick.GetY());
float lJoyStick = limitSpeed(leftJoyStick.GetY());
bool button6 = gamePad.GetRawButton(6);
//speedLimiter.SetMaxOutput(SmartDashboard::GetNumber("Slider 1"));
driveTrain.TankDrive(lJoyStick, rJoyStick);
//manual mode(no PID) for elevator
//float dPadThumbstick = TestMode::GetThumbstickWithZero(&gamePad);
//ballGrabber.DriveElevatorTestMode(dPadThumbstick);
//Sets motor equal to the elevator sensor.
//TODO Probably don't need but want to test because called inside operate grabber.
ballGrabber.OperatePIDLoop();
if(printDelay == 100){
lcd->Clear();
if(m_display_page_1)
{
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg1");
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "FR %4.0f, BA %4.0f",
frontUltrasonic.GetAverageDistance(),
backUltrasonic.GetAverageDistance());
shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
SmartDashboard::PutNumber("UltrasonicF", 1);
SmartDashboard::PutNumber("UltrasonicB", 1);
SmartDashboard::PutNumber("ElvatorAngle", 2);//Change keyname to ElavatorAngle from (ElvatorAngle)
//^^
if(button6){
m_display_page_1 = false;
}
}
else{
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Teleop pg2 %c", button6 ? '1':'0');
ballGrabber.DisplayDebugInfo(DriverStationLCD::kUser_Line2,lcd);
//lcd->PrintfLine(DriverStationLCD::kUser_Line3, "G%f", ballGrabber.ballDetector.GetDistance());
//ballGrabber.UpDateWithState(DriverStationLCD::kUser_Line3,lcd);
shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
//lcd->PrintfLine(DriverStationLCD::kUser_Line4, "EV%6.2f", ballGrabber.elevatorAngleSensor.GetVoltage());
shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line4, lcd);
//lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%5.3f %5.3f %5.3f", lJoyStick, rJoyStick, SmartDashboard::GetNumber("Slider 1"));
lcd->PrintfLine(DriverStationLCD::kUser_Line5, "DEV=%6.3f", ballGrabber.m_desiredElevatorVoltage);
lcd->PrintfLine(DriverStationLCD::kUser_Line6, "CEV=%5.2f",
ballGrabber.elevatorAngleSensor.GetVoltage());
if(button6){
m_display_page_1 = true;
}
}
lcd->UpdateLCD();
printDelay = 0;
}
//int rotation = elevation.Get();
//the above is commented because we are not using it yet
bool shooterButton = gamePad.GetRawButton(7) || gamePad.GetRawButton(8);//TODO make constants
bool automaticAimButton = gamePad.GetRawButton(1);
//float distanceToWall = frontUltrasonic.GetAverageDistance();
//bool loadShooterButton = gamePad.GetRawButton(8);
if (shooterButton && shootDelay == 0){
shootDelay++;
}
if(shootDelay>0){
shootDelay++;
}
bool ReadyToShoot = (shootDelay>PHOENIX2014_LOOP_COUNT_FOR_SHOOT_DELAY);
shooter.OperateShooter(ReadyToShoot);
if (ReadyToShoot){
shootDelay = 0;
}
bool okToGrab = (shootDelay == 0);//Normaly 0 unless delaying
ballGrabber.OperateGrabber(shooterButton, okToGrab);
//Trying to make some things happen automatically during teleoperated
if(automaticAimButton){
ballGrabber.m_desiredElevatorVoltage = PHOENIX2014_TELEOP_ELEVATOR_ANGLE;
}
//((distanceToWall > (12.0*11.0)) && distanceToWall < (12.0*13.0)){
//lightBulb.Set(Relay::kOn);
//}
//else{
#ifdef WANTWEIRDPULSING
if (printDelay < 30) {
lightBulb.Set(Relay::kForward);
}
else if (printDelay >= 30 && printDelay < 65) {
lightBulb.Set(Relay::kReverse);
}
else {
lightBulb.Set(Relay::kOff);
}
#endif
Wait(0.005);// wait for a motor update time
} // end of while enabled
driveTrain.StopMotor();
ballGrabber.StopPidLoop();
shooter.motorShutOff();
/*
if(SavePreferencesToFlash){
dashboardPreferences->Save();
SavePreferencesToFlash = false;
}
*/
} // end of OperatorControl()
/**
* Drive left & right motors for 2 seconds then stop
*/
void Autonomous()
{
init();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Entered Autonomous");
driveTrain.SetSafetyEnabled(false);
bool checkBox1 = SmartDashboard::GetBoolean("Checkbox 1");
m_FromAutonomous = true;
//float rightDriveSpeed = -1.0;
//float leftDriveSpeed = -1.0;
//int rangeToWallClose = 60;
//int rangeToWallFar = 120;
//Initialize encoder.
//int distanceToShoot = 133;
//int shootDelay = 0;
//ballGrabber.elevatorController.SetSetpoint(PHOENIX2014_INITIAL_AUTONOMOUS_ELEVATOR_ANGLE);
//TODO Remove encoders from code??
driveDistanceRight.Reset();
driveDistanceLeft.Reset();
driveDistanceRight.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_RIGHT);
driveDistanceLeft.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_LEFT);
driveDistanceRight.Start();
driveDistanceLeft.Start();
//int printDelay = 0;
float minDistance = 52.0; // Closer to the wall than this is too close
float maxDistance = 12.0*11.0; // Once at least this close, within shooting range
//float closeDistance = maxDistance + 24.0;
float autoDriveSpeed = 0.55;
//float autoDriveSlowSpeed = 0.38;
int time = 0;
int maxDriveLoop = 4*200; // 4 seconds @200 times/sec
bool shootingBall = false;
bool wantFirstShot = true;
if(checkBox1 == false){
int printDelay = 0;
//Ultrasonic Autonomous
//bool isInRange = false;
while(IsAutonomous() && IsEnabled())
{
float currentDistance = frontUltrasonic.GetAverageDistance();
// Transition isInRange from false to true once
if((minDistance < currentDistance) && (currentDistance < maxDistance)){
//isInRange = true;
}
time++;
bool motorDriveTimeExceeded = time > maxDriveLoop;
bool motorMinMet = time > m_MinDriveLoop;
if(/*isInRange ||*/ motorMinMet){
driveTrain.TankDrive(0.0,0.0);
if((ballGrabber.elevatorAngleSensor.GetVoltage() > PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE - 0.1) &&
(ballGrabber.elevatorAngleSensor.GetVoltage() < PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE + 0.1)){
//Enough to cover break release and start winding again.
shootingBall = shooter.OperateShooter(wantFirstShot);
}
if(shootingBall == true){
wantFirstShot = false;
}
}
else if(motorDriveTimeExceeded){
driveTrain.TankDrive(0.0,0.0);
}
else{
//if((currentDistance < closeDistance) && motorMinMet){
// autoDriveSpeed = autoDriveSlowSpeed;
//}
driveTrain.TankDrive(-0.97 * autoDriveSpeed, -1.0 * autoDriveSpeed);//the DriveTrain is BACKWARD
}
ballGrabber.RunElevatorAutonomous(PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE);
printDelay = printDelay++;
if(printDelay >= 200){
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "In Autonomous");
shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line2, lcd);
shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Ulra=%f", frontUltrasonic.GetAverageDistance());
//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "CEV=%f", ballGrabber.elevatorAngleSensor.GetVoltage());
//lcd->PrintfLine(DriverStationLCD::kUser_Line6, "DEV=%f", ballGrabber.m_desiredElevatorVoltage);
lcd->UpdateLCD();
printDelay = 0;
}
Wait(0.005);
}
lcd->Clear();
lcd->UpdateLCD();
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Exiting Autonomous");
} else {
//Timer Autonomous
driveTrain.TankDrive(-0.5,-0.5);
ballGrabber.DriveElevatorTestMode(-1.0);
lcd->Clear();
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Skip Auto");
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "CheckBox Checked");
lcd->UpdateLCD();
Wait(2.0);
bool shooting = true;
driveTrain.TankDrive(0.0,0.0);
int counter = 0;
while(counter < 600){
shooter.OperateShooter(shooting);
Wait(0.005);
}
}
}
/******************************************************
* Drive left & right motors for 2 seconds then stop *
******************************************************/
void Autonomous(void)
{
//increase 2nd RPM
compressor.Start(); // starts the compressor;
bool BRIDGE = bridge.Get();
bool HIGH = high.Get();
bool MIDDLE = middle.Get();
bool TWOSEC = twosec.Get();
bool FIVESEC = fivesec.Get();
int highRPM = 1800; // 1st 1800 short about 5 ft
int secondhighRPM = 1800; //1st 1400 (did not fire)
int rpmForShooter;
DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory
char debugout [100];
// baddog.SetExpiration(30.0);
// baddog.Feed();
dslcd->Clear();
sprintf(debugout,"Bridge=%u",BRIDGE);
dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
sprintf(debugout,"High=%u",HIGH);
dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
sprintf(debugout,"Middle=%u",MIDDLE);
dslcd->Printf(DriverStationLCD::kUser_Line3,3,debugout);
sprintf(debugout,"TwoSec=%u",TWOSEC);
dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
sprintf(debugout,"FiveSec=%u",FIVESEC);
dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
dslcd->UpdateLCD(); // update the Driver Station with the information in the code */
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0)
{
myRobot.Drive(0.0, 0.0);
Wait(10.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //1700 RPM
{
/*myRobot.Drive(-0.5, 0.0);
Wait(3.0);
*/
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1700);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1700);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //main autonomous code-default
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM((int)highRPM);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM((int)highRPM);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM((int)secondhighRPM);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
// baddog.Feed();
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
//Wait(2.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1900);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1900);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
myRobot.Drive(0.0,0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //position robot-front of key-low RPMs
{
//Robot aims
//Robot shoots
//myRobot.Drive(-0.5, 0.0);
//Wait(3.0);
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(1600);
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1600);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 1)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 0)
{
Wait(2.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 5)
{
Wait(5.0);
//Robot aims
//Robot shoots
myRobot.Drive(-0.5, 0.0);
Wait(3.0);
}
if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 1 && TWOSEC == 1 && FIVESEC == 1)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
targeting.SetLMHTarget(BOGEY_H);
while(ShooterTimer.Get()<2)
{
if (targeting.ProcessOneImage())
{
targeting.ChooseBogey();
targeting.MoveTurret();
rpmForShooter = targeting.GetCalculatedRPM();
shooter.setTargetRPM(rpmForShooter);
Wait(0.01);
}
}
targeting.StopPID();
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get() < 7)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(rpmForShooter);
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 1 && TWOSEC == 0 && FIVESEC == 0)
{
flashring.Set(Relay::kForward);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<7)
{
shooter.setTargetRPM(2000); //high RPM
//wait-0.01
Wait(0.005);
}
lynx.Set(-1.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<1)
{
shooter.setTargetRPM(2000); //high RPM
Wait(0.005);
}
turret.Set(-0.05);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<0.2)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
turret.Set(0.0);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
// lynx.Set(0.0);
shepard.Set(true);
ShooterTimer.Reset();
ShooterTimer.Start();
while(ShooterTimer.Get()<2.0)
{
shooter.setTargetRPM(1800); //low RPM
Wait(0.005);
}
shepard.Set(false);
shooter.setTargetRPM((int)0);
flashring.Set(Relay::kOff);
lynx.Set(0.0);
}
//baddog.Feed();
myRobot.SetSafetyEnabled(false);
}
void OperatorControl(void)
{
float counter;
timer.Start();
float percent;
deadband = .05;
float pi = 3.141592653589793238462643;
float bpotval, fpotval;
float min = 600, max;
float fchgval = .5;
float bchgval = .5;
while (IsOperatorControl())
{
// comp.checkCompressor();
ShootModeSet();
Shoot(true);
fpotval = fpot.GetValue();
bpotval = bpot.GetValue();
counter = timer.Get();
if (controller.GetRawButton(3))
{
frot.SetSpeed(0);
brot.SetSpeed(0);
flmot.SetSpeed(0);
frmot.SetSpeed(0);
blmot.SetSpeed(0);
brmot.SetSpeed(0);
}
else if (controller.GetRawButton(BTN_L1) == false)
{
if (controller.GetRawButton(7))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(0.5);
frmot.Set(0.5);
blmot.Set(0.5);
brmot.Set(0.5);
}
}
else if (controller.GetRawButton(8))
{
if (fpotval < 860)
frot.SetSpeed(-0.5);
if (bpotval < 725)
brot.SetSpeed(-0.5);
if (fpotval > 860 and bpotval > 725)
{
frot.Set(0);
brot.Set(0);
flmot.Set(-0.5);
frmot.Set(-0.5);
blmot.Set(-0.5);
brmot.Set(-0.5);
}
}
else
{
if (controller.GetRawAxis(4) <= 0)
percent = ((acos(controller.GetRawAxis(3)) / pi));
else if (controller.GetRawAxis(4) > 0)
percent = ((acos(-controller.GetRawAxis(3)) / pi));
fpotval = percent * 550 + 330;
percent = (fpotval - 330) / 550;
bpotval = percent * 500 + 245;
if (fpot.GetValue() < fpotval)
fchgval = -.5;
else if (fpot.GetValue() > fpotval)
fchgval = .5;
if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10)
fchgval = 0;
if (bpot.GetValue() > bpotval)
bchgval = -.5;
else if (bpot.GetValue() < bpotval)
bchgval = .5;
if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20)
bchgval = 0;
frot.Set(fchgval);
brot.Set(bchgval);
if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1))
{
if (controller.GetRawAxis(4) > 0)
{
flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
else if (controller.GetRawAxis(4) < 0)
{
flmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
frmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
blmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
brmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
}
}
else
{
flmot.Set(0);
frmot.Set(0);
blmot.Set(0);
brmot.Set(0);
}
contaxis4 = 0;
}
}
else
{
if (contaxis4 == 0)
contaxis4 = controller.GetRawAxis(4);
if (controller.GetRawButton(BTN_R1))
{
if (contaxis4 > 0)
{
flmot.Set(-0.25);
frmot.Set(0.25);
blmot.Set(-0.25);
brmot.Set(0.25);
}
else if (contaxis4 < 0)
{
flmot.Set(0.25);
frmot.Set(-0.25);
blmot.Set(0.25);
brmot.Set(-0.25);
}
}
}
if (float (fpot.GetValue()) < min)
min = float (fpot.GetValue());
else if (float (fpot.GetValue()) > max)
max = float (fpot.GetValue());
if (counter >= .1)
{
lcda->Clear();
lcda->Printf(DriverStationLCD::kUser_Line3, 1, "FPot :: %d", fpotval);
lcda->Printf(DriverStationLCD::kUser_Line4, 1, "BPot :: %d", bpotval);
lcda->UpdateLCD();
timer.Reset();
}
}
}
void OperatorControl()
{
timer.Start();
while (IsOperatorControl())
{
x = Stick.GetRawAxis(1);
y = -Stick.GetRawAxis(2);
if (fabs(x) < 0.1)
{
x = 0;
}
if (fabs(y) < 0.1)
{
y = 0;
}
x2 = x*x;
y2 = y*y;
magnitude = pow((x2+y2),0.5);
angle = atan2(y,x)*180/PI;
magnitude = max(magnitude, -1);
magnitude = min(magnitude, 1);
if (angle < 0)
{
angle += 180;
magnitude *= -1;
}
/*front = 730 - (angle * 2.7888);
if ((front + 5) < float(fpot.GetValue()))
{
fturn.Set(0.5);
}
else if ((front - 5) > float(fpot.GetValue()))
{
fturn.Set(-0.5);
}
else
{
fturn.Set(0);
}*/
back = 235 + (angle * 2.8611);
if ((back + 10) < float(bpot.GetValue()))
{
bturn.Set(0.5);
}
else if ((back - 10) > float(bpot.GetValue()))
{
bturn.Set(-0.5);
}
else
{
bturn.Set(0);
}
if (timer.Get() > 0.1)
{
lcd->Clear();
lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Front = %5.4f", front);
lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Back = %5.4f", back);
lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Front Pot = %5.4f", float(fpot.GetValue()));
lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Back Pot = %5.4f", float(bpot.GetValue()));
lcd->Printf(DriverStationLCD::kUser_Line5, 1, "Angle = %5.4f", angle);
lcd->Printf(DriverStationLCD::kUser_Line6, 1, "Magnitude = %5.4f", magnitude);
lcd->UpdateLCD();
timer.Reset();
timer.Start();
}
}
}
