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;
			}
		}
Exemplo n.º 2
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();
	}