void TeleopPeriodic() { //Drive if(SmartDashboard::GetBoolean("DB/Button 0", false)) { sc_left -> Set(-evan -> GetRawAxis(1)); sc_right -> Set(evan -> GetRawAxis(5)); } else { sc_left -> Set(-evan->GetRawAxis(1) + .5*evan->GetRawAxis(0)); sc_right -> Set(evan->GetRawAxis(1) + .5*evan->GetRawAxis(0)); } //Intake if (i_limit->Get()) { if(hunter->GetRawButton(1)) { l_shoot->Set(-1); r_shoot->Set(1); if(count == 0) { timer1->Start(); count = 1; } } else if(evan->GetRawAxis(2) > 0.2) { intake->Set(-evan->GetRawAxis(2)); } else{ intake->Set(0); } } else { if(evan->GetRawAxis(3) > 0.2) { intake->Set(.40*evan->GetRawAxis(3)); } else if(evan->GetRawAxis(2) > 0.2) { intake->Set(-evan->GetRawAxis(2)); } else { intake->Set(0); } } if(timer1->Get()>2){ intake->Set(1); timer2->Start(); if(timer2->Get()>.5){ l_shoot->Set(0); r_shoot->Set(0); intake->Set(0); timer1->Stop(); timer1->Reset(); timer2->Stop(); timer2->Reset(); count = 0; } } //Fleshlight fleshlight->Set(evan->GetRawButton(1) ? Relay::Value::kForward : Relay::Value::kOff);; /* //DPAD Stuff switch (evan -> GetPOV()) { case (0): l_shoot -> Set(-.25*evan -> GetRawAxis(3)); r_shoot -> Set(.25*evan -> GetRawAxis(3)); break; case (90): l_shoot -> Set(-.5*evan -> GetRawAxis(3)); r_shoot -> Set(.5*evan -> GetRawAxis(3)); break; case (180): l_shoot -> Set(-.75*evan -> GetRawAxis(3)); r_shoot -> Set(.75*evan -> GetRawAxis(3)); break; case (270): l_shoot -> Set(evan-> GetRawAxis(3)); r_shoot ->Set(-evan->GetRawAxis(3)); break; default: l_shoot -> Set(-evan -> GetRawAxis(3)); r_shoot -> Set(evan -> GetRawAxis(3)); break; }*/ //Hunter controls Arm if ((hunter->GetRawAxis(5) < 0.1 and hunter->GetRawAxis(5) > -0.1) or (a_limit->Get() and hunter->GetRawAxis(5) > 0)) { l_arm->Set(0); r_arm->Set(0); } else { l_arm -> Set(-hunter-> GetRawAxis(5)); r_arm -> Set(hunter-> GetRawAxis(5)); } //let evan control arm /*if ((evan->GetRawAxis(5) < 0.1 and evan->GetRawAxis(5) > -0.1) or (a_limit->Get() and evan->GetRawAxis(5) > 0)) { l_arm->Set(0); r_arm->Set(0); } else { l_arm -> Set(evan-> GetRawAxis(5)); r_arm -> Set(-evan-> GetRawAxis(5)); }*/ //FORCEFEEDBACK evan -> SetRumble(Joystick::RumbleType::kLeftRumble, hunter -> GetRawButton(2)); evan -> SetRumble(Joystick::RumbleType::kRightRumble, hunter -> GetRawButton(2)); //Message SmartDashboard::PutNumber("DB/String 0", evan->GetPOV() ); //std::cout << evan->GetPOV() << std::endl; //Wait Wait(0.001); }
/****************************************************************************** * Function: TeleopPeriodic * * Description: This is the function that is called during the periodic period * for teleop. ******************************************************************************/ void TeleopPeriodic() { float DrvRY, DrvLY; float BallRollerSpd; float FishTapeSpd; float WenchSpd; float LgtY, LgtZ; double LgtPOV; int LgtP1; bool LgtT0; bool LgtB1,LgtB3,LgtB4,LgtB5,LgtB6,LgtB7,LgtB8,LgtB9,LgtB11,LgtB10,LgtB12; bool XbxB4, XbxB5, XbxB2, XbxB1; bool BallSw; int XbxPOV; double ArmP; double ArmError; double desiredPos; double LoArmCurve; while (IsOperatorControl() && IsEnabled()) { /* Read sensor values here: */ ArmP = ballarm.GetDistance(); BallSw = BlSw.Get(); /* Read Xbox controller commands here: */ DrvLY = -xboxDrive.GetRawAxis(1); DrvRY = -xboxDrive.GetRawAxis(5); XbxB1 = xboxDrive.GetRawButton(1); XbxB2 = xboxDrive.GetRawButton(2); XbxB4 = xboxDrive.GetRawButton(4); XbxB5 = xboxDrive.GetRawButton(5); XbxPOV = xboxDrive.GetPOV(0); /* Read Logictec joystick commands here: */ LgtY = Logitech.GetRawAxis(1); LgtZ = Logitech.GetRawAxis(2); LgtP1 = Logitech.GetPOV(0); LgtT0 = Logitech.GetRawButton(1); LgtB1 = Logitech.GetRawButton(2); LgtB3 = Logitech.GetRawButton(3); LgtB4 = Logitech.GetRawButton(4); LgtB5 = Logitech.GetRawButton(5); LgtB6 = Logitech.GetRawButton(6); LgtB7 = Logitech.GetRawButton(7); LgtB8 = Logitech.GetRawButton(8); LgtB9 = Logitech.GetRawButton(9); LgtB10 = Logitech.GetRawButton(10); LgtB11 = Logitech.GetRawButton(11); LgtB12 = Logitech.GetRawButton(12); LgtPOV = (double)LgtP1; // DriverStation:: //double GetMatchTime(); // SmartDashboard::PutNumber("MatchTime", GetMatchTime()); if(LgtB12 && LgtB10) { ballarm.Reset(); } /* Set the desired position of the ball arm. */ if (LgtB7) { /* This is the middle position of the arm: */ desiredPos = 120; } else if (LgtB8) { /* This is the upper position of the arm: */ desiredPos = 210; } else if((LgtB3) || (LgtB5) ||(LgtB6) || (LgtB9) || (LgtB11)) { /* Default */ desiredPos = 0; } /* Set the ball roller state: */ if (LgtT0 == true && BallSw == true) { BallRollerSpd = 1; } else if (LgtB1 == true) { BallRollerSpd = -1; } else { BallRollerSpd = 0.0; } /* Set the output to the fish tape: */ if (LgtP1 == 180) { FishTapeSpd = -1.0; } else if(LgtP1 == 0) { FishTapeSpd = 1.0; } else { FishTapeSpd = 0.0; } /* Set the output to the lower arm: */ LoArmCurve = LgtY*LowArmGx; // SmartDashboard:: /* Determine the wench state */ if (LgtB4 == true) { WenchSpd = 1; } else if (XbxB4) { WenchSpd = -.5; } else { WenchSpd = 0.0; } /* if(GetMatchTime() < 30) { Light1.Set(0); Light2.Set(0); }*/ /* Output data to the smart dashboard: */ ReadAutonSwitch(); UpdateSmartDashboad(Sw1.Get(), Sw2.Get(), Sw3.Get(), Sw4.Get(), BlSw.Get(), AutonState, 0, ballarm.GetDistance(), gyroOne.GetAngle(), accel.GetX(), accel.GetY(), accel.GetZ(), (double)DrvLY, (double)DrvRY); UpdateActuatorCmnds(BallRollerSpd, desiredPos, LgtB3, LgtB5, LgtB6, LgtB7, LgtB8, LgtB9, LgtB11, DrvLY, DrvRY, FishTapeSpd, LoArmCurve, WenchSpd); /* Force the program to wait a period of time in order to conserve power: */ Wait(kUpdatePeriod); // Wait 5ms for the next update. Scheduler::GetInstance()->Run(); } }
void TeleopPeriodic() { if(tick==10) if (ds->IsSysBrownedOut()) { ds->ReportError("[ERROR] BROWNOUT DETECTED!!"); } if(tick == 15) if (!ds->IsNewControlData()) { ds->ReportError( "[ERROR] NO DATA FROM DRIVER STATION IN THIS TICK!"); } if(tick==20) if (!ds->IsDSAttached()) { ds->ReportError("[ERROR] DRIVER STATION NOT DETECTED!"); } if (stick.GetRawButton(10)) zeroSanics(); if (stick.GetRawButton(8)) { leftIRZero = 0; rightIRZero = 0; } tick++; if (liftStick.GetRawButton(2)) { double canScale = liftStick.GetRawAxis(2); canScale += 1; canScale = 2 - canScale; canScale /= 2; canGrabber.SetSpeed(canScale); } else if (liftStick.GetRawButton(3)) { double canScale = liftStick.GetRawAxis(2); canScale += 1; canScale = 2 - canScale; canScale /= 2; canGrabber.SetSpeed(-canScale); } else canGrabber.SetSpeed(0); double speed; //Calculate scalar to use for POV/Adjusted drive double scale = stick.GetRawAxis(3); scale += 1; scale = 2 - scale; scale /= 2; //Use pov/hat switch for movement if enabled if (stick.GetRawButton(1) && stick.GetRawButton(2)) { AutomaticLineup(); } else if (stick.GetRawButton(1)) { double leftVolts = leftIR.GetAverageVoltage() - leftIRZero; double rightVolts = rightIR.GetAverageVoltage() - leftIRZero; if (rightVolts + VOLTAGE_TOLERANCE > leftVolts && rightVolts - VOLTAGE_TOLERANCE < leftVolts) { robotDrive.MecanumDrive_Cartesian(0, 0, 0); } else if (rightVolts > leftVolts) robotDrive.MecanumDrive_Cartesian(0, 0, 0.2); else if (leftVolts > rightVolts) robotDrive.MecanumDrive_Cartesian(0, 0, -0.2); } else if (stick.GetRawButton(6)) { //Rotate robotDrive.MecanumDrive_Polar(0, 0, scale); } else if (stick.GetRawButton(5)) { //Rotate robotDrive.MecanumDrive_Polar(0, 0, -scale); } else if (stick.GetPOV(0) != -1) { //If POV moved, move polar (getPOV returns an angle in degrees) robotDrive.MecanumDrive_Polar(scale, -stick.GetPOV(0), 0); } else if (stick.GetRawButton(2)) { //Drive with scalar robotDrive.MecanumDrive_Cartesian(-stick.GetRawAxis(0) * scale, stick.GetRawAxis(1) * scale, stick.GetRawAxis(2) * scale); } else { //Drive normally robotDrive.MecanumDrive_Cartesian(-stick.GetX(), stick.GetY(), stick.GetZ()); } speed = -liftStick.GetY(); //bool canGoUp = maxUp.Get(); bool canGoUp = true; //bool canGoDown = maxDown.Get(); bool canGoDown = true; //If at a limit switch and moving in that direction, stop if (speed > 0 && !canGoUp) speed = 0; if (speed < 0 && !canGoDown) speed = 0; chainLift.SetSpeed(speed); if (tick >50) { if (SmartDashboard::GetBoolean("Smart Dashboard Enabled")) { //Smart Dash outputs //SmartDashboard::PutNumber("X Acceleration: ", accel.GetX()); //SmartDashboard::PutNumber("Y Acceleration: ", accel.GetY()); //SmartDashboard::PutNumber("Z Acceleration: ", accel.GetZ()); SmartDashboard::PutBoolean("Switch 1: (up)", maxUp.Get()); SmartDashboard::PutBoolean("Switch 2: (down)", maxDown.Get()); SmartDashboard::PutBoolean("Switch 3: (mid)", midPoint.Get()); SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get()); SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get()); //SmartDashboard::PutBoolean("RobotDrive Alive?", // robotDrive.IsAlive()); //SmartDashboard::PutBoolean("ChainLift Alive?", // robotDrive.IsAlive()); SmartDashboard::PutNumber("Left Sensor", leftIR.GetAverageVoltage()); SmartDashboard::PutNumber("Right Sensor", rightIR.GetAverageVoltage()); SmartDashboard::PutNumber("Left w zero", leftIR.GetAverageVoltage() - leftIRZero); SmartDashboard::PutNumber("Rigt w zero", rightIR.GetAverageVoltage() - rightIRZero); SmartDashboard::PutNumber("PDP 14 Current", pdp.GetCurrent(14)); SmartDashboard::PutNumber("PDP 15 Current", pdp.GetCurrent(15)); } tick = 0; } }
void TeleopPeriodic() override { float leftPower, rightPower; // Get the values for the main drive train joystick controllers leftPower = -leftjoystick->GetY(); rightPower = -rightjoystick->GetY(); float multiplier; // TURBO mode if (rightjoystick->GetRawButton(1)) { multiplier = 1; } else { multiplier = 0.5; } // wtf is a setpoint - it's an angle to turn to if (leftjoystick->GetRawButton(6)) { turnController->Reset(); turnController->SetSetpoint(0); turnController->Enable(); ahrs->ZeroYaw(); //ahrs->Reset(); } // Press button to auto calculate angle to rotate bot to nearest ball // if(leftjoystick->GetRawButton(99)) // { // ahrs->ZeroYaw(); // turnController->Reset(); // turnController->SetSetpoint(mqServer.GetDouble("angle")); // turnController->Enable(); // aimState = 1; // } switch(aimState) { default: case 0: // No camera assisted turning //Drive straight with one controller, else: drive with two controllers if (leftjoystick->GetRawButton(1)) { drive->TankDrive(leftPower * multiplier, leftPower * multiplier, false); } else if (leftjoystick->GetRawButton(2)) { drive->TankDrive(leftPower * multiplier + rotateRate, leftPower * multiplier + -rotateRate, false); } else { drive->TankDrive(leftPower * multiplier, rightPower * multiplier, false); } break; case 1: // Camera assisted turning, deny input from controllers drive->TankDrive(rotateRate, -rotateRate, false); if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) { aimState = 0; // Finished turning, auto assist off turnController->Disable(); turnController->Reset(); } break; } // That little flap at the bottom of the joystick float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2); // Depending on the button, our intake will eat or shoot the ball if (controlstick->GetRawButton(2)) { intake->Set(-scaleIntake); shooter->Set(scaleIntake); } else if (controlstick->GetRawButton(1)) { intake->Set(scaleIntake); shooter->Set(-scaleIntake); } else { intake->Set(0); shooter->Set(0); } // Control the motor that lifts and descends the intake bar float intake_lever_power = 0; if (controlstick->GetRawButton(6)) { manual = true; intake_lever_power = .3; // intakeLever->Set(.30); // close } else if (controlstick->GetRawButton(4)) { manual = true; intake_lever_power = -.4; // intakeLever->Set(-.40); // open } else if (controlstick->GetRawButton(3)){ manual = true; intake_lever_power = -scaleIntake; // intakeLever->Set(-scaleIntake); } else if (controlstick->GetRawButton(5)) { manual = true; intake_lever_power = scaleIntake; // intakeLever->Set(scaleIntake); } else { if (manual) { manual = false; lastLiftPos = intakeLever->GetEncPosition(); intakeLever->SetControlMode(CANTalon::ControlMode::kPosition); intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder); intakeLever->SetPID(1, 0.001, 0.0); intakeLever->EnableControl(); } intake_hold = true; intakeLever->Set(lastLiftPos); } if (manual) { intake_hold = false; intakeLever->SetControlMode(CANTalon::ControlMode::kPercentVbus); intakeLever->Set(intake_lever_power); } if (controlstick->GetRawButton(11)) { lift->Set(true); liftdown->Set(false); } else if (controlstick->GetRawButton(12)){ lift->Set(false); liftdown->Set(true); } else if (controlstick->GetRawButton(7)) { liftdown->Set(false); } if (controlstick->GetRawButton(9)) { winch->Set(scaleIntake); } else if (controlstick->GetRawButton(10)) { winch->Set(-scaleIntake); } else { winch->Set(0); } if (controlstick->GetPOV() == 0 && !bounce ) { constantLift -= 0.05; bounce = true; } else if (controlstick->GetPOV() == 180 && !bounce) { constantLift += 0.05; bounce = true; } else if (controlstick->GetPOV() == 270 && !bounce) { constantLift = 0; bounce = true; } else { bounce = false; } UpdateDashboard(); }