Exemplo n.º 1
0
	void TeleopPeriodic()
	{
		char myString [STAT_STR_LEN];

		if (initialZeroing)  // moving to the inner limit
		{
			sprintf(myString, "initZero ip\n"); //in progress
			SmartDashboard::PutString("DB/String 0", myString);
			if (GetForkLimitSwitchMin())
			{
				SetForkMotor(MOTOR_SPEED_STOP);
				gearToothCounter->Reset();
				initialZeroing = false;
				sprintf(myString, "initZero comp\n"); //complete
				SmartDashboard::PutString("DB/String 0", myString);
			}
		}
		else  //manual control
		{
			//motor control
			if (joystick->GetRawButton(BUT_JS_OUT) && !GetForkLimitSwitchMax())  // move outwards
				SetForkMotor(MOTOR_SPEED_GO);
			else if(joystick->GetRawButton(BUT_JS_IN) && !GetForkLimitSwitchMin())  // move inwards
				SetForkMotor(-MOTOR_SPEED_GO);
			else
				SetForkMotor(MOTOR_SPEED_STOP); //stop

			//counter control
			if (joystick->GetRawButton(BUT_JS_RES_GTC))  // reset the gear tooth counter
				gearToothCounter->Reset();
		}

		//status
		sprintf(myString, "jsOut %d\n", joystick->GetRawButton(BUT_JS_OUT));
		SmartDashboard::PutString("DB/String 1", myString);
		sprintf(myString, "jsIn %d\n", joystick->GetRawButton(BUT_JS_IN));
		SmartDashboard::PutString("DB/String 2", myString);
		sprintf(myString, "jsResGtc %d\n", joystick->GetRawButton(BUT_JS_RES_GTC));
		SmartDashboard::PutString("DB/String 3", myString);
		sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
		SmartDashboard::PutString("DB/String 4", myString);
		sprintf(myString, "gtc #: %d\n", gearToothCounter->Get()); //gtc count
		SmartDashboard::PutString("DB/String 5", myString);
	}
	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);
		}
	}
Exemplo n.º 3
0
	void TeleopPeriodic()
	{
		char myString [STATUS_STR_LEN];

		if (running)
		{
			switch (forkState)
			{
				case closing:
					UpdateGearCount (); //update whenever the forks are moving
					if (GetForkLimitSwitchMin())
					{
						sprintf(myString, "min gear count: %d\n", absGearToothCount);
						SmartDashboard::PutString("DB/String 8", myString);
						SetForkMotor(MOTOR_STOP);
						SetForkMotor(MOTOR_SPEED);
						dsBox->SetOutputs(ALL_LEDS_ON);
						forkState = opening;
					}
					break;

				case opening:
					UpdateGearCount (); //update whenever the forks are moving
					if (GetForkLimitSwitchMax())
					{
						sprintf(myString, "max gear count: %d\n", absGearToothCount);
						SmartDashboard::PutString("DB/String 9", myString);
						SetForkMotor(MOTOR_STOP);
						SetForkMotor(-MOTOR_SPEED);
						dsBox->SetOutputs(ALL_LEDS_OFF);
						forkState = closing;
					}
					break;
			}
		}

		//current monitor check for safety
		if (forkMotor->GetOutputCurrent() > MAX_CUR_TH)
		{
			SetForkMotor(MOTOR_STOP);
			running = false;
		}

		sprintf(myString, "curr: %f\n", forkMotor->GetOutputCurrent());
		SmartDashboard::PutString("DB/String 0", myString);
		sprintf(myString, "State: %d\n", forkState);
		SmartDashboard::PutString("DB/String 1", myString);
		sprintf(myString, "running: %d\n", running);
		SmartDashboard::PutString("DB/String 2", myString);
		sprintf(myString, "forkDir: %d\n", forkDirection);
		SmartDashboard::PutString("DB/String 3", myString);
		sprintf(myString, "abs gear count: %d\n", absGearToothCount);
		SmartDashboard::PutString("DB/String 4", myString);

#if 0
		if (minAVal > aIn->GetValue())
			minAVal = aIn->GetValue();
		if (maxAVal < aIn->GetValue())
			maxAVal = aIn->GetValue();
		sprintf(myString, "min A val: %d\n", minAVal);
		SmartDashboard::PutString("DB/String 5", myString);
		sprintf(myString, "max A val: %d\n", maxAVal);
		SmartDashboard::PutString("DB/String 6", myString);
#endif


	}