float getTrigger()
{
return m_joy->GetRawAxis(3);
}
int GetJoystickButton(const Joystick& joystick)
{
return joystick.GetPressedButton();
}
bool GetJoystickPovIndexAndDirection(const Joystick& joystick, int& povIndex, int& direction)
{
return joystick.GetPressedPovIndexAndDirection(povIndex, direction);
}
// To determine which drive control type the robot to use
// In case user operator wants different controls
void Robot::DriverControl(int driveControl) {
switch (driveControl) {
// Arcade drive (think racing games) w/ 1 joysticks
case ARCADE_1:
// If the turn is outside the joystick's standard drift, robot will assume the robot is turning
// If not, robot will assume it is pushed, or has naturally drifted
if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
gyro.Reset();
} else {
// The turn will be the opposite of what the gyro says the angle of unintentional drift is,
// which will have the robot go straight
myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Arcade drive w/ 2 joysticks
case ARCADE_2:
if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
gyro.Reset();
} else {
myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Arcade drive w/ left stick on gamepad (the knockoff xbox controller)
case ARCADE_GAMEPAD_1:
if (fabs(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X));
gyro.Reset();
} else {
myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Arcade drive w/ BOTH gamepad
case ARCADE_GAMEPAD_2:
if (fabs(gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X));
gyro.Reset();
} else {
myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Tank drive (requires 2 sticks; each stick controls its respective 'tread' or side;
// Ex: moving the right stick moves only the wheels on the right) w/ gamepad
case TANK_GAMEPAD:
joystickDifference = gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) - gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y);
joystickAverage = (gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) + gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y))/2;
if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
myRobot.TankDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y));
gyro.Reset();
} else {
myRobot.ArcadeDrive(joystickAverage, editedGyroRate*GYRO_SCALE_FACTOR);
}
break;
// Tank drive w/ joysticks
case TANK_2:
joystickDifference = lStick.GetRawAxis(LEFT_STICK_Y) - rStick.GetRawAxis(RIGHT_STICK_Y);
joystickAverage = (lStick.GetRawAxis(LEFT_STICK_Y) + rStick.GetRawAxis(RIGHT_STICK_Y))/2;
if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
myRobot.TankDrive(lStick.GetRawAxis(LEFT_STICK_Y), rStick.GetRawAxis(RIGHT_STICK_Y));
gyro.Reset();
} else {
myRobot.ArcadeDrive(joystickAverage, -rStick.GetRawAxis(RIGHT_STICK_Y));
}
break;
}
}
int GetJoystickButton(const Joystick& joystick)
{
return joystick.GetReleasedButton();
}
float getjoyRightX()
{
return m_joy->GetRawAxis(3);
}
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()
bool get2()
{
return m_joy->GetRawButton(2);
}
bool get3()
{
return m_joy->GetRawButton(3);
}
float getjoyLeftY()
{
return m_joy->GetRawAxis(2);
}
bool get1()
{
return m_joy->GetRawButton(1);
}
float getjoyLeftX()
{
return m_joy->GetRawAxis(1);
}
void DropDrive::HandleDrive(Joystick& left, Joystick& right) {
DriveTrain.TankDrive(left.GetRawAxis(1)*-1.0, right.GetRawAxis(1)*-1.0);
// SmartDashboard::PutNumber("Gyro heading", gyro.GetAngle());
}
float getDPADX()
{
return m_joy->GetRawAxis(6);
}
bool getStart()
{
return m_joy->GetRawButton(10);
}
bool get4()
{
return m_joy->GetRawButton(4);
}
bool getSelect()
{
return m_joy->GetRawButton(9);
}
bool getRightTrigger1()
{
return m_joy->GetRawButton(6);
}
float getjoyRightY()
{
return m_joy->GetRawAxis(4);
}
bool getRightTrigger2()
{
return m_joy->GetRawButton(8);
}
void OperatorControl(void)
{
JankyTurret turret(7,11,10);
JankyTargeting targ(&turret);
JankyShooter shoot(SHOOTER_JAGUAR_CHANNEL,SHOOTER_ENCODER_A,SHOOTER_ENCODER_B);
int rpmForShooter=0;
while (IsOperatorControl())
{
//GetImage()
//hsl.Save
// myRobot.ArcadeDrive(stick);
shoot.GetCurrentRPM();
if (button.Get()==true)
{
LEDRelay.Set(Relay::kForward);
if (button_H.Get()==true)
{
targ.SetLMHTarget(BOGEY_H);
SmartDashboard::PutString("Targeting","High Button Pressed");
}
if (button_M.Get()==true)
{
targ.SetLMHTarget(BOGEY_M);
SmartDashboard::PutString("Targeting","Medium Button Pressed");
}
if (button_L.Get()==true)
{
targ.SetLMHTarget(BOGEY_L);
SmartDashboard::PutString("Targeting","Low Button Pressed");
}
if (button_H.Get()==true || button_M.Get()==true || button_L.Get()==true)
{
if (targ.ProcessOneImage())
{
targ.ChooseBogey();
targ.MoveTurret();
rpmForShooter = targ.GetCalculatedRPM();
shoot.setTargetRPM(rpmForShooter);
targ.InteractivePIDSetup();
}
}
targ.StopPID();
shoot.setTargetRPM(0);
}
else
{
float lazysusan =stick.GetZ();
turret.Set(lazysusan);
LEDRelay.Set(Relay::kOff);
// Set shooter speed to zero.
}
// float desired=abs(stick.GetY() *1000) + 100;
// smarty->PutInt("Desired RPM1", (int)desired);
// shoot.setTargetRPM((int)desired);
Wait(0.05);
}
// After teleop runs, save preferences again.
Preferences::GetInstance()->Save();
}
bool getLeftTrigger1()
{
return m_joy->GetRawButton(5);
}
void GetJoystickPovIndicesAndDirections(InputSourceVector& inputSources, const Joystick& joystick)
{
joystick.GetPressingPovIndicesAndDirections(inputSources);
}
bool getLeftTrigger2()
{
return m_joy->GetRawButton(7);
}
void GetJoystickPovIndicesAndDirections(InputSourceVector& inputSources, const Joystick& joystick)
{
joystick.GetReleasedPovIndicesAndDirections(inputSources);
}
bool getL3()
{
return m_joy->GetRawButton(11);
}
bool GetJoystickAxisIndexAndDirection(const Joystick& joystick, int& axisIndex, int& direction)
{
return joystick.GetPressedAxisIndexAndDirection(axisIndex, direction);
}
bool getR3()
{
return m_joy->GetRawButton(12);
}
void GetJoystickButtons(InputSourceVector& inputSources, const Joystick& joystick)
{
joystick.GetPressedButtons(inputSources);
}
bool getLB()
{
return m_joy->GetRawButton(5);
}
