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();
}