Beispiel #1
0
	void TestPeriodic()
	{
		bool nowButton4 = stick->GetRawButton(4);
		bool nowButton1 = stick->GetRawButton(1);

		if (nowButton4 && !lastButton4) {
			testingLeft = !testingLeft;
		}
		if (nowButton1 && !lastButton1) {
			testingRight = !testingRight;
		}
		lastButton4 = nowButton4;
		lastButton1 = nowButton1;

		if (testingRight) {
			rShooter->Set(kRightHighRPM);
			std::cout << rShooter->GetSpeed() << std::endl;
		} else {
			rShooter->Set(0.0);
		}
		if (testingLeft) {
			lShooter->Set(kLeftHighRPM);
			std::cout << lShooter->GetSpeed() << std::endl;
		} else {
			lShooter->Set(0.0);
		}
	}
Beispiel #2
0
	void SetLiftMotor(float val)
	{
		liftMotor->Set(val);
#if BUILD_VERSION == COMPETITION
		liftMotor2->Set(val);
#endif
	}
Beispiel #3
0
	void DisabledInit(){
		printf("finished %s at %f",mode.c_str(),GetTime());
		leftMotor->SetControlMode(defaultMode);
		rightMotor->SetControlMode(defaultMode);
		leftMotor->Set(0);
		rightMotor->Set(0);
	}
Beispiel #4
0
	void TeleopPeriodic()
	{
		while(1)
		{
			fps->SetLeftRightMotorOutputs(0.5*-drivercontroller->GetRawAxis(1) + drivercontroller->GetRawAxis(2), 0.5*-drivercontroller->GetRawAxis(1) - drivercontroller->GetRawAxis(2));
			if(operatorcontroller->GetRawAxis(1) > 0)
			{
				armminipluator->Set(0.6*operatorcontroller->GetRawAxis(1));
			}
			else if(operatorcontroller->GetRawAxis(1) < 0)
			{
				armminipluator->Set(0.3*operatorcontroller->GetRawAxis(1));
			}

			if(operatorcontroller->GetRawButton(5))
			{
				shro->Set(1);
			}
			else if(operatorcontroller->GetRawAxis(6))
			{
				shro->Set(-1);
			}
			else
			{
				shro->Set(0);
			}
		}
	}
Beispiel #5
0
	//void StartAutomaticCapture(std::shared_ptr<USBCamera>cam0);
	void TeleopPeriodic() {
//		ui.GetData(&wui);
//		m_tank.Drive(wui.LeftSpeed, wui.RightSpeed);
//
//		m_shooter.Rotate(wui.RotateSpeed*3); //70 degrees per second at full value
//		m_shooter.Lift(wui.LiftSpeed*1.193); //4 seconds for 180 degree revolution
//		if(wui.SpinUp) {
//			m_shooter.Spinup(1);
//		}
//		if(wui.Shoot) {
//			m_shooter.Shoot();
//		}
//		if(wui.Pickup) {
//			m_shooter.Pickup();
//		}
//
//		m_suspension.SetFrontLeft(wui.DropFL);
//		m_suspension.SetBackLeft(wui.DropBL);
//		m_suspension.SetFrontRight(wui.DropFR);
//		m_suspension.SetBackRight(wui.DropBR);

//		m_leddar.GetDetections();
//		m_shooter.Update();
		//float RTrigger = m_lStick->GetRawAxis(3);
		//float LTrigger = m_lStick->GetRawAxis(2);

		//if (m_PWMTalonLeftFrontTop == .5)
		//if (abs(RTrigger) < 0.2)
			//RTrigger = 0;
		//if (abs(LTrigger) < 0.2)
			//LTrigger = 0;
		float leftSpeed = m_lStick->GetRawAxis(1);
		float rightSpeed = m_lStick->GetRawAxis(5);
		if (abs(leftSpeed) < 0.2)
			leftSpeed = 0;
		if (abs(rightSpeed) < 0.2)
			rightSpeed = 0;
		//float LTrigger = m_lStick->GetRawAxis(3);
		//float RTrigger = m_lStick->GetRawAxis(2);
		SmartDashboard::PutNumber("Left Stick", leftSpeed);
		SmartDashboard::PutNumber("Right Stick", rightSpeed);
		//SmartDashboard::PutNumber("L Trigger", LTrigger);
		//SmartDashboard::PutNumber("R Trigger", RTrigger);
		SmartDashboard::PutNumber("Left Encoder", leftEncoder->Get());
		SmartDashboard::PutNumber("Right Encoder", rightEncoder->Get());
		drive->TankDrive(leftSpeed, rightSpeed, true);
		//drive->TankDrive(RTrigger, LTrigger, true);
		LEFTDRIVE1->Set(leftSpeed);
		LEFTDRIVE2->Set(leftSpeed);
		RIGHTDRIVE1->Set(rightSpeed);
		RIGHTDRIVE2->Set(rightSpeed);
		//m_PWMTalonLeftFrontTop->Set(RTrigger);
		//m_PWMTalonRightFrontTop->Set(RTrigger);
		//m_PWMTalonRightRearTop->Set(LTrigger);
		//m_PWMTalonLeftRearTop->Set(LTrigger);
	}
		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;
			}
		}
Beispiel #7
0
	void TeleopInit()
	{
		//Set slaves
		l_slave->SetControlMode(CANTalon::ControlMode::kFollower);
		r_slave->SetControlMode(CANTalon::ControlMode::kFollower);
		l_slave->Set(1);
		r_slave->Set(2);
		//l_slave->SetClosedLoopOutputDirection(true);
		//r_slave->SetClosedLoopOutputDirection(true);

		zeroAll();
	}
		void toggleIntakeMode(){
			if (intakeMode == 1){
				intake_Spin_Motor.Set(1);
			}
			if (intakeMode == 0){
				intake_Spin_Motor.Set(0);
			}
			if (intakeMode == -1){
				intake_Spin_Motor.Set(-1);
						}

		}
Beispiel #9
0
	void TeleopPeriodic()
	{
	while(IsOperatorControl() && IsEnabled())
		{
	//	FrontL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2));
		//FrontR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2));
		RearL->Set(.5*Driver->GetRawAxis(0)+.5*Driver->GetRawAxis(2));
		RearR->Set(.5*Driver->GetRawAxis(0)-.5*Driver->GetRawAxis(2));
		/**Manipulator->Set(.5*Operator->GetRawAxis(1));*/
		/** Takes axes value of driver's and operator's joysticks to recieve values that the motors will be set to.
		 *  Pushing left stick forward and backwards on driver controller causes all motors to move forward and backwards, respectively.
		 *  Pushing  right stick left on driver controller causes right motors to speed up making the robot turn left
		 *  Pushing right stick right on driver controller causes left motors to speed up making the robot turn right
		 *  Pushing left stick forward and backwards causes the arm to mover up and down, respectively.
		 */
		}
	}
Beispiel #10
0
	void AutonomousLowBar() {
//		Strategy 2 - start in a normal position lined up with low bar, go through low bars and score boulder in lower goal.
//		-------------------------------------------------------------------------------------------------------------------
// 		backUp straight for a little bit
//      drop arm
//   	backup more under lowbar
//      stop (we might add going to lowgoal later)
		switch(currentState)
		{
		case 1:
			timer->Reset();
			timer->Start();
			currentState = 2;
			break;
		case 2:
			drive->TankDrive(autoSpeed,autoSpeed);
			if(timer->Get() >= .4)
			{
				drive->TankDrive(0.0,0.0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
		case 3:
			intakeLever->Set(autoIntakeSpeed);
			if(timer->Get() >= .5)
			{
				intakeLever->Set(0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			drive->TankDrive(autoSpeed,autoSpeed);
			if(timer->Get() >= autoLength)
			{
				drive->TankDrive(0.0,0.0);
				currentState = 5;
				timer->Reset();
				timer->Stop();
			}
			break;
		}
	}
	Robot() :
		robotDrive(Motor1, Motor2),	// these must be initialized in the same order
		stick(5),		// as they are declared above.
		lw(LiveWindow::GetInstance()),
		autoLoopCounter(0),
		Motor1(21),
		Motor2(12),
		Slave1(20),
		Slave2(14),
		t_motor(13),
		arm_Motor(23),
		finger_Motor(22),
		intake_Spin_Motor(11),
		intake_Winch_Motor(13),
		stick2(4),
		autoLoopCounter2(0)
	{
		robotDrive.SetExpiration(0.1);
		robotDrive.SetSafetyEnabled(false);
		Slave1.SetControlMode(CANSpeedController::kFollower);
		Slave1.Set(21);
		Slave2.SetControlMode(CANSpeedController::kFollower);
		Slave2.Set(12);
		Motor2.SetInverted(true); //12
		Slave2.SetInverted(true);//14
		arm_Motor.SetInverted(false);//23
		t_motor.SetInverted(true);//23
//		t_motor.SetControlMode(CANSpeedController::kVoltage);
//		t_motor.Set(0);
//		CameraServer::GetInstance()->SetQuality(50);

//		CameraServer::GetInstance()->SetSize(2);
//		//the camera name (ex "cam0") can be found through the roborio web interface
//		CameraServer::GetInstance()->StartAutomaticCapture("cam0");

		t_motor.SetControlMode(CANSpeedController::kPercentVbus);
//		t_motor.SetVoltageCompensationRampRate(24.0);
		t_motor.SetFeedbackDevice(CANTalon::QuadEncoder);
		t_motor.SetPosition(0);
//		t_motor.SetPID(1, 0, 0);
		arm_Motor.SetControlMode(CANSpeedController::kPercentVbus);
		finger_Motor.SetControlMode(CANSpeedController::kPercentVbus);
//		ourRangefinder = new AnalogInput(0);

	}
Beispiel #12
0
	void stateLaunching() {
		stateTimer++;
		if (stateTimer == 1) {
			if (shootingHigh) {
				rShooter->Set(kRightHighRPM);
				lShooter->Set(kLeftHighRPM);
			} else {
				rShooter->Set(kRightLowRPM);
				lShooter->Set(kLeftLowRPM);
			}
		} else if (stateTimer > 50 && stateTimer <= 65) {
			shooterInOut->Set(-1.0);
			LOGGER(DEBUG) << "[stateLaunching] Timer: " << stateTimer << " Angle: " << launchPIDSource.PIDGet()
					      << " Right RPM: " << rShooter->GetSpeed() << " Left RPM: " << lShooter->GetSpeed();
		} else if (stateTimer > 65 && stateTimer <= 70) {
			shooterInOut->Set(0.0);
		} else if (stateTimer > 70 && stateTimer <= 75) {
			shooterInOut->Set(0.6);
		} else if (stateTimer > 75) {
			shootingHigh = false;
			stateTimer = 0;
			rShooter->Set(0.0);
			lShooter->Set(0.0);
			shooterInOut->Set(0.0);
			robotState = kOperatorControl;
		}
	}
Beispiel #13
0
	void AutonomousStraightSpy() {
		switch (currentState) {
		case 1:
			timer->Reset();
			timer->Start();
			turnController->Reset();
			turnController->SetSetpoint(ahrs->GetYaw());
			turnController->Enable();
			currentState = 2;
			break;
		case 2:
			intakeLever->Set(0.25);
			if (timer->Get() >= 1) {
				intakeLever->Set(0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
		case 3:
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 5) {
				drive->TankDrive(0.0, 0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			intake->Set(0.5);
			if (timer->Get() >= 2) {
				currentState = 5;
			}
			break;
		case 5:
			intake->Set(0.0);
			drive->TankDrive(0.0, 0.0);
			break;
		}
	}
		void shootingModes(){
			if (manShootMode == 0){
				updateShooter();
			}
			if (manShootMode == 1){
				if (stick2.GetRawAxis(triggerR) > 0){
					t_motor.Set(scaler(-1*(stick2.GetRawAxis(triggerR))));
				}
				else if (stick2.GetRawAxis(triggerL) > 0){
					t_motor.Set(scaler(stick2.GetRawAxis(triggerL)));
				}
				else if (stick2.GetRawAxis(triggerL) == 0 and stick2.GetRawAxis(triggerR) == 0){
					t_motor.Set(0);
				}

				if (stick2.GetRawButton(buttonX) == true){
					myServo->SetAngle(175);
				}
				if (stick2.GetRawButton(buttonX) == false){
					myServo->SetAngle(5);
				}
			}
		}
Beispiel #15
0
	void AutonomousAdjustableStraight() {
		switch (currentState) {
		case 1:
			timer->Reset();
			timer->Start();
			turnController->Reset();
			turnController->SetSetpoint(ahrs->GetYaw());
			turnController->Enable();
			currentState = 2;
			break;
		case 2:
			intakeLever->Set(autoIntakeSpeed);
			if (timer->Get() >= 1) {
				intakeLever->Set(0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
		case 3:
			drive->TankDrive(autoSpeed, autoSpeed);
			intakeLever->Set(-0.1);
			if (timer->Get() >= autoLength) {
				intakeLever->Set(0.0);
				drive->TankDrive(0.0, 0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			intake->Set(0.5);
			shooter->Set(-0.5);
			if (timer->Get() >= 2) {
				currentState = 5;
			}
			break;
		case 5:
			intake->Set(0.0);
			shooter->Set(0.0);
			drive->TankDrive(0.0, 0.0);
			break;
		}
	}
Beispiel #16
0
	void SetForkMotor(float val)
	{
		forkMotor->Set(FORK_MOTOR_DIR*val);
	}
Beispiel #17
0
	void AutonomousSpy() {
//		Strategy 1 - start as spy with a boulder, score in lower goal. Starts with intake facing low goal
//		-------------------------------------------------------------------------------------------------------------------
		switch (currentState) {
		case 1:
			//		-State: stopped
			timer->Reset();
			timer->Start();
			ahrs->ZeroYaw();
			currentState = 2;
			break;

//		--transition: state Driving Forward
		case 2:
			//		-State: Driving Forward
			//		--wait until lined up with low goal
			//		--transition: State stopped
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) { // NEEDS TO BE SET
				//		-State: stopped
				//		--wait until stopped
				drive->TankDrive(0.0, 0.0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
			//		--transition: State Shooting
		case 3:
//		-State: Shooting
//		--wait until shooting complete
			intake->Set(-.5);
			if (timer->Get() >= .7) { //Find Out Actual Time
				intake->Set(0);
				timer->Reset();
				timer->Start();
				currentState = 4;
			}
			break;
			//		--transition: State Backing Up
		case 4:
			//		-State: Backing Up
			//		--wait until off tower ramp
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() > 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				ahrs->Reset();
				currentState = 5;
				turnController->SetSetpoint(-65.5);
				turnController->Enable();
			}
			break;

//		--transition: Turning
		case 5:
			//		-State: Turning Left
			//		--wait until 65 degrees has been reached to line up with low bar
			drive->TankDrive(-0.5, 0.5);
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				timer->Reset();
				timer->Start();
				currentState = 6;
			}
			break;
//		--transition: Backing Up
		case 6:
			//		-State backing Up
			//		--wait until near guard wall
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() >= 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				ahrs->Reset();
				currentState = 7;
				turnController->SetSetpoint(-24.5);
				turnController->Enable();
			}
			break;
//		--transition: Turn Left
		case 7:
//		-State: Turn Right
//		--wait until 25 degree turn has been made to line with low bar
			drive->TankDrive(-0.5, 0.5);
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				timer->Reset();
				timer->Start();
				currentState = 8;
			}
			break;
//		--transition: Back Up
		case 8:
//		-State: Backing Up
//		--wait until backed through low bar
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() >= 1) { // NeedTo Update Value
				timer->Stop();
				currentState = 9;
			}
			break;
//		--transition: Stopped
		case 9:
//		-State: Stopped
			drive->TankDrive(0.0, 0.0);
			break;
		}
	}
Beispiel #18
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();
	}
Beispiel #19
0
	void SetForkMotor(float val)
	{
		curForkSetSpeed = FORK_MOTOR_REV_STATE*val;
		forkMotor->Set(curForkSetSpeed);
	}
Beispiel #20
0
        DriveTrain(Vision* visionTracking) :
                IComponent(new string("DriveTrain")),
                leftDriveMaster(new CANTalon(1)),
                leftDriveSlave1(new CANTalon(3)),
                leftDriveSlave2(new CANTalon(5)),
                rightDriveMaster(new CANTalon(2)),
                rightDriveSlave1(new CANTalon(4)),
                rightDriveSlave2(new CANTalon(6)),
                shift(new Solenoid(4)),
                vision(visionTracking),
                visionPIDSource(new DrivePIDSource()),
                visionPIDOutput(new DrivePIDOutput()),
                visionPID(new PIDController(0.70f, 0, 0, visionPIDSource, visionPIDOutput)),
                angleETC(new ErrorTimeCubed(DRIVE_ANGLE_TOLERANCE, 45.0f, -180.0f, 180.0f)),
                crossTime(new Timer()),
                hasCrossed(false),
                crossState(0),
                isClimbing(true),
                driveTime(new Timer()),
                timedDriveState(0),
                shiftHigh(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_RIGHT)),
                shiftLow(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_LEFT)),
                stateUntoggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BACK)),
                autoCrossToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, NEW_JOYSTICK ? RobotButton::ControlTypes::AXIS : RobotButton::ControlTypes::KEY, JOYSTICK_TRIGGER_RIGHT)),
                reverseToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_X)),
                crossSpeedMultiplier(1.0f),
                crossingForward(true),
                leftSpeedCurrent(0),
                rightSpeedCurrent(0),
                targetDistance(0),
                crossReverse(false),
                reverse(true),
                primaryDriving(false),
                state(DriveState::NONE)
        {
            leftDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
            leftDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
            leftDriveMaster->ConfigEncoderCodesPerRev(1024);
            leftDriveMaster->Enable();

            leftDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
            leftDriveSlave1->Enable();
            leftDriveSlave1->Set(1);

            leftDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
            leftDriveSlave2->Enable();
            leftDriveSlave2->Set(1);

            rightDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
            rightDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
            leftDriveMaster->ConfigEncoderCodesPerRev(1024);
            rightDriveMaster->Enable();

            rightDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
            rightDriveSlave1->Enable();
            rightDriveSlave1->Set(2);

            rightDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
            rightDriveSlave2->Enable();
            rightDriveSlave2->Set(2);

            visionPID->SetInputRange(-1, 1);
            visionPID->SetOutputRange(-1, 1);
            visionPID->SetContinuous(true);
            visionPID->SetAbsoluteTolerance(0.05);
            visionPID->Disable();
        }
Beispiel #21
0
	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);
	}
Beispiel #22
0
	/**
	 * Runs the motor from the output of a Joystick.
	 */
	void OperatorControl() {

		LifterEncoder.StartLiveWindowMode();
		LifterEncoder.Reset();



		while (IsOperatorControl() && IsEnabled()) {
			// Set the motor controller's output.
			// This takes a number from -1 (100% speed in reverse) to +1 (100% speed forwards).
			// lifterA_motor.Set(joy.GetY());
			// lifterB_motor.Set(joy.GetY());


			if ( stick.GetRawButton(1))
			{
			}
			else if (stick.GetRawButton(2))
			{
			}
			else if (stick.GetRawButton(3))
			{
				// Elevator down
				if (BottomLimitSwitch.Get() == 0) {
					lifterA_motor.Set(0.5);
					lifterB_motor.Set(0.5);
				} else {
					lifterA_motor.Set(0);
					lifterB_motor.Set(0);
				}
			}
			else if (stick.GetRawButton(4))
			{
			}
			else if (stick.GetRawButton(5))
			{
				// Elevator up
				if (TopLimitSwitch.Get() == 0) {
					lifterA_motor.Set(-0.5);
					lifterB_motor.Set(-0.5);
				} else {
					lifterA_motor.Set(0);
					lifterB_motor.Set(0);
				}
			}
			else if (stick.GetRawButton(6))
			{
			}
			else if (stick.GetRawButton(7))
			{
			}
			else if (stick.GetRawButton(8))
			{
			}
			else if (stick.GetRawButton(9))
			{
			}
			else if (stick.GetRawButton(10))
			{
			}
			else if (stick.GetRawButton(11))
			{
			}
			else if (stick.GetRawButton(12))
			{
			}
			else
			{
				lifterA_motor.Set(0.0);
				lifterB_motor.Set(0.0);
			}


			if(BottomLimitSwitch.Get()==true)
			{
				LifterEncoder.Reset();
			}

			// Send some stuff to the dashboard
			SmartDashboard::PutBoolean("Top Limit Switch", TopLimitSwitch.Get());
			SmartDashboard::PutBoolean("Bottom Limit Switch", BottomLimitSwitch.Get());
			SmartDashboard::PutNumber("Encoder Position",LifterEncoder.GetRaw());

			Wait(kUpdatePeriod); // Wait 5ms for the next update.
		}
	}
	void OperatorControl()
		{
			while (IsOperatorControl() && IsEnabled())
			{
				robotDrive.ArcadeDrive(scaler(stick.GetZ()),scaler(stick.GetY()));
				SmartDashboard::PutNumber("StickZ",stick.GetZ());
				SmartDashboard::PutNumber("StickZscaled",scaler(stick.GetZ()));


				finger_Motor.Set(scaler(stick2.GetRawAxis(thumbpadL_Y)));
//				scalerValue = stick.GetRawAxis(3);
				arm_Motor.Set(scaler(stick2.GetRawAxis(thumbpadR_Y)));
				manualShooter();
				shootingModes();
				toggleIntake();
				toggleIntakeMode();
				setScalerValue();
//				double volts = ourRangefinder->GetVoltage();
//				SmartDashboard::PutNumber("Voltage",volts);
				//t_motor.Set(stick2.GetZ());
				//t_motor.Set(stick.GetAxis(Joystick::kDefaultThrottleAxis));
//				t_motor.Set(stick.GetAxis(Joystick::kThrottleAxis));
//				finger_Motor.Set(stick2.GetAxis(Joystick::kThrottleAxis));
//				arm_Motor.Set(stick2.GetY());

		//		Current Control mode Debug
				SmartDashboard::PutNumber("Motor30 Current",t_motor.GetOutputCurrent());
				SmartDashboard::PutNumber("Position",t_motor.GetPosition());
//				t_motor.Set(stick.GetAxis(Joystick::Slider));
//				if (stick.GetRawButton(3) == true){
//					t_motor.SetPosition(10000);
//				}

//			if (stick2.GetRawButton(7) == true and buttonpress == false){
//

//									}
//			else if (stick2.GetRawButton(3) == false and buttonpress == true){
//					buttonpress = false;// drive with arcade style (use right stick)
//
//						}
			SmartDashboard::PutBoolean("buttonpress state",buttonpress);

			if (stick2.GetRawButton(buttonBack) == true){
				stateDisarmed = true;
				stateArming1 = false;
				stateArming2 = false;
				stateArmed = false;
				stateFiring1 = false;
				stateFiring2 = false;
				t_motor.SetPosition(0);

			}
			toggleIntake();
//			if (stick2.GetRawButton(5) == true){
//				intake_Spin_Motor.Set(1);
//			}
//			if (stick2.GetRawButton(5) == false){
//				intake_Spin_Motor.Set(0);
//			}
			if (stick.GetRawButton(2) == true and buttonpress2 == false){
				myServo->SetAngle(175);
			}
			if (stick.GetRawButton(2) == false and buttonpress2 == true){
				myServo->SetAngle(5);


			}
//			servoSetPos(servoState);
//			myServo->Set(.5);
			Wait(0.005);// wait for a motor update time
//			motorSetPos(launcherState);
		}
	}