void updateShooter(){
SmartDashboard::PutBoolean("stateDisarmed", stateDisarmed);
SmartDashboard::PutBoolean("stateArming1", stateArming1);
SmartDashboard::PutBoolean("stateArming2", stateArming2);
SmartDashboard::PutBoolean("stateArmed", stateArmed);
SmartDashboard::PutBoolean("stateFiring1", stateFiring1);
float winchLocked = SmartDashboard::GetNumber("winchLocked",-4500.00);
// float winchUnlocked = SmartDashboard::GetNumber("winchUnlocked",-3000.00);
float winchMidway = SmartDashboard::GetNumber("winchMidway",-0.00);
// float winchServoTrigger = SmartDashboard::GetNumber("winchServoTrigger",-0.00);
// float winchOverUnlocked = SmartDashboard::GetNumber("winchOverUnlocked",0.00);
float servoLocked = SmartDashboard::GetNumber("servoLocked",5.00);
float servoUnlocked = SmartDashboard::GetNumber("servoUnlocked",175.00);
float motorStopped = 0.00;
float motorWinding = SmartDashboard::GetNumber("motorWinding",-.70);
float motorUnwinding = SmartDashboard::GetNumber("motorUnwinding",.50);
// float motorFastUnwinding = SmartDashboard::GetNumber("motorFastUnwinding",.80);
if (stateDisarmed == true){
// t_motor.Set(0);
}
if (stick2.GetRawButton(buttonA) == true and stateDisarmed == true){
myServo->SetAngle(servoLocked);
stateDisarmed = false;
stateArming1 = true;
}
if (stateArming1 == true){
t_motor.Set(motorWinding);
myServo->SetAngle(servoLocked);
if (t_motor.GetEncPosition() < winchLocked){
t_motor.Set(motorStopped);
stateArming1 = false;
stateArming2 = true;
}
}
if (stateArming2 == true){
t_motor.Set(motorUnwinding);
if (t_motor.GetEncPosition() >= winchMidway){
t_motor.Set(motorStopped);
stateArming2 = false;
stateArmed = true;
}
}
if (stick.GetRawButton(1) == true and stateArmed == true){
stateArmed = false;
stateFiring1 = true;
}
if (stateFiring1 == true){
t_motor.Set(0);
myServo->SetAngle(servoUnlocked);
stateFiring1 = false;
stateDisarmed = true;
}
}
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();
}