void Drive (float speed, int dist)
	{
		leftDriveEncoder.Reset();
		leftDriveEncoder.Start();
		
		int reading = 0;
		dist = abs(dist);
		
		// The encoder.Reset() method seems not to set Get() values back to zero,
		// so we use a variable to capture the initial value.
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
		dsLCD->UpdateLCD();

		// Start moving the robot
		robotDrive.Drive(speed, 0.0);
		
		while ((IsAutonomous()) && (reading <= dist))
		{
			reading = abs(leftDriveEncoder.Get());				
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
			dsLCD->UpdateLCD();
		}

		robotDrive.Drive(0.0, 0.0);
		
		leftDriveEncoder.Stop();
	}	
	// Runs during test mode
	// Test
	// * 
	void Test()
	{
		shifters.Set(DoubleSolenoid::kForward);

		leftDriveEncoder.Start();
		leftDriveEncoder.Reset();

		int start = leftDriveEncoder.Get();

		while (IsTest()) {
			if (rightStick.GetRawButton(7)) {
				robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
			}
			else {
				robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
			}

			if (gamepad.GetEvent(4) == kEventClosed) {
				start = leftDriveEncoder.Get();
			}

			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
			dsLCD->UpdateLCD();

			gamepad.Update();
		}
	}
Exemple #3
0
	float RawControl::averageEncoders()
	{
		float traveled;
		traveled=(((abs(wheelEncoderFR->Get()))+(abs(wheelEncoderFL->Get()))+(abs(wheelEncoderBR->Get()))+(abs(wheelEncoderBL->Get())))/4);
		traveled/=250;
		traveled=traveled * 8 * 3.14;	
		traveled=fabs(traveled);
		return traveled;
	}
Exemple #4
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);
	}
// Called repeatedly when this Command is scheduled to run
void RPMHoldingCommand::Execute() {
	double countsPerRevolution = 360;

	double currentTime = timer->Get();
	double timeDifference = currentTime - lastTime;
	lastTime = currentTime;
	
	Encoder* motorEncoder = prototypingsubsystem->GetMotorEncoder();
	int currentCount = motorEncoder->Get();
	int countDifference = currentCount - lastCount;
	double rawRPM = countDifference * (60.0 / countsPerRevolution) / timeDifference;
	double rpm = GetRPM(rawRPM);
	printf("rawRPM %f rpm %f\n", rawRPM, rpm);
	rpmSource->inputRPM(rpm);
	//rpmSource->inputRPM(rpm);
	
	printf("setpoint %f RPM %f result %f error %f \n", controller->GetSetpoint(), rpm, controller->Get(), controller->GetError());
	
	SmartDashboard *sd = oi->getSmartDashboard();

	oi->DisplayEncoder(motorEncoder);
	oi->DisplayPIDController(controller);
	
	sd->PutDouble("Count Difference", countDifference);
	sd->PutDouble("Current count", currentCount);
	sd->PutDouble("Time difference", timeDifference);
	sd->PutDouble("Calculated RPM", rpm);
	sd->PutDouble("Motor speed", prototypingsubsystem->GetMotor()->Get());
	
	sd->PutDouble("Counts per revolution", countsPerRevolution);
	
	lastCount = currentCount;

}
/**
 * Gets the current count.
 * Returns the current count on the Encoder.
 * This method compensates for the decoding type.
 * 
 * @deprecated Use GetEncoderDistance() in favor of this method.  This returns unscaled pulses and GetDistance() scales using value from SetEncoderDistancePerPulse().
 *
 * @return Current count from the Encoder.
 * 
 * @param aSlot The digital module slot for the A Channel on the encoder
 * @param aChannel The channel on the digital module for the A Channel of the encoder
 * @param bSlot The digital module slot for the B Channel on the encoder
 * @param bChannel The channel on the digital module for the B Channel of the encoder
 */
INT32 GetEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
	Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
	if (encoder != NULL)
		return encoder->Get();
	else
		return 0;
}
Exemple #7
0
	void TeleopPeriodic()
	{
		drive_mode_t new_mode = drive_mode_chooser.GetSelected();
		SmartDashboard::PutString("current mode", new_mode == TANK_DRIVE ? "Tank" : "Arcade");
		if (new_mode != drive_mode)
			SetDriveMode(new_mode);
		if (drive_mode == TANK_DRIVE) {
			left_speed = accel(left_speed, pilot->LeftY(), TICKS_TO_ACCEL);
			right_speed = accel(right_speed, pilot->RightY(), TICKS_TO_ACCEL);
			drive->TankDrive(left_speed, right_speed);
		}
		else {
			rot_speed = accel(rot_speed, pilot->RightX(), TICKS_TO_ACCEL);
			SmartDashboard::PutNumber("rotation speed", rot_speed);
			rot_speed = pilot->RightX();
			move_speed = accel(move_speed, pilot->LeftY(), TICKS_TO_ACCEL);
			drive->ArcadeDrive(move_speed * MOVE_SPEED_LIMIT, -rot_speed * MOVE_SPEED_LIMIT, false);
		}
		SmartDashboard::PutBoolean("clamp open", clamp->isOpen());
		SmartDashboard::PutBoolean("sword in", clamp->isSwordIn());

		SmartDashboard::PutData("gyro", gyro);

//		for (uint8 i = 0; i <= 15; ++i)
//			SmartDashboard::PutNumber(std::string("current #") + std::to_string(i), pdp->GetCurrent(i));
		SmartDashboard::PutNumber("Current", pdp->GetTotalCurrent());

		if (pilot->ButtonState(GamepadF310::BUTTON_A)) {
			clamp->open();
		}
		else if (pilot->ButtonState(GamepadF310::BUTTON_B)){
			clamp->close();
		}

		clamp->update();

		SmartDashboard::PutNumber("accelerometer Z", acceler->GetZ());

		SmartDashboard::PutNumber("Encoder", encoder->Get());

		flywheel->Set(pilot->RightTrigger());

		if (pilot->LeftTrigger() != 0)
			flywheel->Set(-pilot->LeftTrigger());


		SmartDashboard::PutNumber("Left Trigger:", pilot->LeftTrigger());

		if (pilot->ButtonState(GamepadF310::BUTTON_X)) {
			cameraFeeds-> changeCam(cameraFeeds->kBtCamFront);
		}
		if (pilot->ButtonState(GamepadF310::BUTTON_Y)){
			cameraFeeds-> changeCam(cameraFeeds->kBtCamBack);
		}

		cameraFeeds->run();

	}
Exemple #8
0
	void Claw(double joy) {
		if(joy < 10) joy = 10;
		if(joy > 230) joy = 230;
		int location = EncClaw.Get();
		double speed = PIDClaw(joy, location);
		//if(location < 15) speed *= .2;
		if(speed > .32) speed = .32;
		if(speed < -.32) speed = -.32;
		if(speed < .1 && speed > -.1) speed = 0;
		arm2->Set(speed,2);
	}
Exemple #9
0
inline void lifterPositionTaskFunc(uint32_t joystickPtr, uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t pdpPtr, uint32_t heightPtr, uint32_t isLiftingPtr ...) {//uint is a pointer and not an integer
	double *height = (double *) heightPtr;//initializes double
	Joystick *joystick = (Joystick *) joystickPtr;
	Talon *liftTalon = (Talon *) liftTalonPtr;
	Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
	Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
	Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
	PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
	bool *isLifting = (bool *) isLiftingPtr;

	*isLifting = true;//tells robot.cpp that thread is running

	if (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height) {//checks to see if encoder is higher than it's supposed to be
		if (liftLowerLimit->Get() == false) {//starts to spin motor to pass startup current
			liftTalon->Set(1);//move down
			Wait(Constants::liftDelay);
		}
		while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftLowerLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too high and hasn't hit a limit switch or been cancelled
			SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder in SmartDashboard
			liftTalon->Set(.7);//move down
		}
	}
	else {
		if (liftUpperLimit->Get() == false) {//starts to spin motor to pass startup current
			liftTalon->Set(-1);//move up
			Wait(Constants::liftDelay);
		}
		while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftUpperLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too low and hasn't hit a limit switch or been cancelled
			SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder on SmartDashboard
			liftTalon->Set(-1);//move up
		}
	}

	liftTalon->Set(0);//stop
	*isLifting = false;//tells robot.cpp that thread is finished
}
Exemple #10
0
inline void lifterBrakeTaskFunc(uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t isBrakingPtr ...) {//uint is a pointer and not an integer
	Talon *liftTalon = (Talon *) liftTalonPtr;
	Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
	Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
	Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
	bool *isBraking = (bool *) isBrakingPtr;
	PIDController pid(Constants::liftBrakeP, Constants::liftBrakeI, Constants::liftBrakeD, liftEncoder, liftTalon);
	Timer timer;

	//TODO enable and test out brake
	if (!Constants::liftBrakeIsEnabled) {
		return;
	}

	timer.Start();

	while (timer.Get() < Constants::liftBrakeUpTime) {
		liftTalon->Set(Constants::liftBrakeUpPower);
	}

	liftTalon->Set(0);

	pid.Enable();
	pid.SetSetpoint(liftEncoder->Get());

	while (*isBraking) {
		if ((pid.Get() > 0 && liftLowerLimit->Get()) || (pid.Get() < 0 && liftUpperLimit->Get())){
			liftTalon->Set(0);
		}
		else {
			liftTalon->Set(pid.Get());
		}
		SmartDashboard::PutBoolean("Braking", true);


	}

	pid.Disable();
	SmartDashboard::PutBoolean("Braking", false);
	liftTalon->Set(0);
}
Exemple #11
0
	void Arm(double joy) {
		int location = EncArm.Get();
		/*
		if(location < 10 && joy < 0) joy = 0;
		if(location > 110 && joy > 0) joy = 0;
		arm1->Set(joy);
		arm1_sec->Set(joy);
		return;
		*/
		
		if(joy < -10) joy = -10;
		if(joy > 110) joy = 110;
		
		double speed = PIDArm(joy, location);
		if(speed > .5) speed = .5;
		if(speed < -.3) speed = -.3;
		if(speed < 0 && location < 10) speed = 0;
		if(speed > 0 && location > 110) speed = 0;
		speed = LowArm(speed);
		if(speed < .01 && speed > -.01) speed = 0;
		arm1->Set(speed,3);
		arm1_sec->Set(speed,3);
		
	}
void RobotDemo::RPS_control_code(float desired_RPS)
// THE 360 COUNT PER REVOLUTION ENCODERS ARE GOOD FOR 10K RPM, BUT THE CIM motor MAXIMUM SPEED IS APPROXIMATELY 5K.
{
#if 1
	/* If the difference between the old and new RPS (typ ~35) by greater than 1 either way,
	 * reset the ingegral terms to prevent stale values from affecting our PID calculations
	 */
	
	if (desired_RPS != prev_desired_RPS)
	{
		if (fabs(prev_desired_RPS - desired_RPS) >= 1.0)
		{
			integral_back = 0;
			integral_front = 0;
		}
		cout << desired_RPS << " " << prev_desired_RPS << endl;
		prev_desired_RPS = desired_RPS;
	}
#endif
	//motor = new Victor(motor_pwm);
	//test_encoder = new Encoder(encoder_pwm1, encoder_pwm2, true);
	if ((pid_code_timer->Get()) >= pidtime)
	{
		float actual_time = pid_code_timer ->Get();
		prev_error_back = error_back;
		prev_error_front = error_front;
		RPS_back = (back_shooter_encoder->Get() / 360.0) / actual_time;
		RPS_front = (front_shooter_encoder->Get() / 360.0) / actual_time;
		error_back = desired_RPS - RPS_back;
		error_front = desired_RPS - RPS_front;
		integral_back = integral_back + (error_back * actual_time);
		integral_front = integral_front + (error_front * actual_time);
		derivitive_back = (error_back - prev_error_back) / actual_time;
		derivitive_front = (error_front - prev_error_front) / actual_time;

		//take error and set it to motors
		{
			RPS_speed_back = (error_back * first_pterm) + (integral_back
					* iterm) + (derivitive_back * dterm);
			RPS_speed_front = (error_front * first_pterm) + (integral_front
					* iterm) + (derivitive_front * dterm);
		}

		if (RPS_speed_back > 1)
		{
			RPS_speed_back = 1;
		}
		if (RPS_speed_front > 1)
		{
			RPS_speed_front = 1;
		}
		if (RPS_speed_back < 0)
		{
			RPS_speed_back = 0;
		}
		if (RPS_speed_front < 0)
		{
			RPS_speed_front = 0;
		}
		//cout << RPS_speed << endl;
		//cout << autonomous_timer->Get() << "   RPS_back: " << RPS_back

		//<< "   RPS_front: " << RPS_front << "   Desired RPS:"
		//<< autonomous_desired_RPS << endl;
		shooter_motor_back ->Set(RPS_speed_back);
		shooter_motor_front ->Set(RPS_speed_front);
		counter++;
#if 0
		if (counter == 5)
		{
			counter = 1;
			printf(
					"                                         %i)%f   %f   %f   %f\n",
					number, RPS, RPS_speed, error,
					shooter_motor_back->Get());
			number++;
		}
		else

		{
			printf("%f   %f   %f   %f\n", RPS, RPS_speed, error,
					shooter_motor_back->Get());
		}
#endif
		pid_code_timer->Reset();
		back_shooter_encoder ->Reset();
		front_shooter_encoder ->Reset();
	}
}
Exemple #13
0
	void TeleopPeriodic()
	{
		//camera->GetImage(frame);
		//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
		//CameraServer::GetInstance()->SetImage(frame);


		printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());

		drive->ArcadeDrive(driveStick);
		drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
		//printf("%f\n", (1-stick->GetThrottle())/2);
		//leftMotor->Set(0.1);
		//rightMotor->Set(0.1);

		if (shootStick->GetRawAxis(3) > 0.5) {
			launch1->Set(1.0);
			launch2->Set(1.0);
		} else if (shootStick->GetRawAxis(2) > 0.5) {
			printf("Power Counter: %i\n", powerCounter);
			if (powerCounter < POWER_MAX) {
				powerCounter++;
				launch1->Set(-0.8);
				launch2->Set(-0.8);
			} else {
				launch1->Set(-0.6);
				launch2->Set(-0.6);
			}
		} else {
			launch1->Set(0.0);
			launch2->Set(0.0);
			powerCounter = 0.0;
		}

		//use this button to spin only one winch, to lift up.
		 if (shootStick->GetRawButton(7)) {
		 	otherWinch->Set(0.5);
		 } else if (shootStick->GetRawButton(8)) {
			 otherWinch->Set(-0.5);
		 } else {
		 	otherWinch->Set(0.0);
		 }

		if (shootStick->GetRawButton(5)) {
			winch->Set(-0.7);

			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(-0.5);
			}
		} else if (shootStick->GetRawButton(6)) {
			winch->Set(0.7);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(0.5);
			}
		} else {
			winch->Set(0.0);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//,				otherWinch->Set(0.0);
			}
		}
		

		if (shootStick->GetRawButton(1)) {
			launchPiston->Set(1);
		} else {
			launchPiston->Set(0);
		}

		if (shootStick->GetRawButton(3)) {
			Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
		}

		if (shootStick->GetRawButton(3) && debounce == false) {
			debounce = true;
			if (defenseUp) {
				defensePiston->Set(DoubleSolenoid::Value::kReverse);
				defenseUp = false;
			} else {
				defenseUp =true;
				defensePiston->Set(DoubleSolenoid::Value::kForward);
			}
		} else if (!shootStick->GetRawButton(3)){
			debounce = false;
		}
	}
Exemple #14
0
	void OperatorControl(void)
	{
		AxisCamera &camera = AxisCamera::GetInstance();
		miniBotTime.Start();
		initRobot();
		
		debug("in telop");
		compressor.Start();
		GetWatchdog().SetEnabled(true);
		/*int l1, l2, l3;
		while (IsOperatorControl()) {
			GetWatchdog().Feed();
			//char val = (line1.Get() & 0x01) | (line2.Get() & 0x02) | (line3.Get() & 0x04);
			//if(l1 != line1.Get() || l2 != line2.Get() || l3 != line3.Get()) {
			//	cerr << "change " << (l1 = line1.Get()) << "\t" << (l2 = line2.Get()) << "\t" << (l3 = line3.Get()) << endl;
			//}
			cerr << "Change "<< line1.Get() <<"\t" << line2.Get() << "\t" << line3.Get() << endl;
			Wait(0.2);
		}*/
		char count=0, pneumaticCount=0;
		// was .125 when loop at .025
		lowPass lowSpeed(.04), lowStrafe(.04), lowTurn(.04), lowClaw(.04), lowArm(.04), lowArmLoc(.05);
		
		double ClawLocation=0, ArmLocation=0, OldArmLocation=0;
		
		while (IsOperatorControl() && !IsDisabled())
		{
			GetWatchdog().Feed();
			float speed = -1*stick.GetRawAxis(2);
			float strafe = stick.GetRawAxis(1);
			float turn = stick.GetRawAxis(3);

			if(!stick.GetRawButton(7)) {
				speed /= 2;
				strafe /= 2;
				turn /= 2;
			}
			if(stick.GetRawButton(8)) {
				speed /= 2;
				strafe /= 2;
				turn /= 2;
			}
			if(stick.GetRawButton(2)) {
				speed = 0;
				turn = 0;
			}
			
			Drive(lowSpeed(speed), lowTurn(turn), lowStrafe(strafe));		
			
			
			
#ifndef NDEBUG
			if(stick2.GetRawButton(10)) {
				robotInted = false;
				initRobot();
			}
#endif
			
			
			if(stick2.GetRawButton(7) && (miniBotTime.Get() >= 110 || (stick2.GetRawButton(9) && stick2.GetRawButton(10)))) { // launcher
				// the quick launcher
				MiniBot1a.Set(true);
				MiniBot1b.Set(false);
				
			} 
			if(!stick2.GetRawButton(10) && stick2.GetRawButton(9)) { // deploy in
				MiniBot2a.Set(false);
				MiniBot2b.Set(true);
				//MiniBot2a.Set(false);
				//MiniBot2b.Set(true);
			}
			if(stick2.GetRawButton(5)) { // top deploy out
				MiniBot2a.Set(true);
				MiniBot2b.Set(false);
			}
			
			
			if(stick2.GetRawButton(6)) { // open
				ClawOpen.Set(true);
				ClawClose.Set(false);
			}
			if(stick2.GetRawButton(8)) { // closed
				ClawOpen.Set(false);
				ClawClose.Set(true);
				ClawLocation += 2;
			}
			
			/*156 straight
			 * 56 90 angle
			 * 10 back
			 */
			
			if(stick2.GetRawButton(1)) { // top peg
				ClawLocation = 156;
				ArmLocation = 105;
			}
			if(stick2.GetRawButton(2)) {
				ClawLocation = 111; // the 90angle / middle peg
				ArmLocation = 50;
			}
			if(stick2.GetRawButton(3)) { // off ground
				ClawLocation = 176;
				ArmLocation = 5;
			}
			if(stick2.GetRawButton(4)) {
				ClawLocation = 0; // back
				ArmLocation = 0;
			}
			
			double tmpClaw = .7*lowClaw(stick2.GetRawAxis(4));
			if(tmpClaw < .2 && tmpClaw > -.2) tmpClaw = 0;
			
			double tmpArm = .4*lowArm(-1*stick2.GetRawAxis(2));
			if(tmpArm < .2 && tmpArm > -.2) tmpArm = 0;
			if(tmpArm > .5) tmpArm = .5;
			if(tmpArm < -.5) tmpArm = -.5;
						
			
			ClawLocation += tmpClaw + tmpArm; // the right joy stick y
			
			if(ClawLocation < 10) ClawLocation = 10;
			if(ClawLocation > 230) ClawLocation = 230;
			
			Claw(ClawLocation);
			
			
			ArmLocation += tmpArm;
			if(ArmLocation > 110) ArmLocation = 110;
			if(ArmLocation < -10)  ArmLocation = -10;
			
			Arm(lowArmLoc(ArmLocation));
			OldArmLocation = ArmLocation;
						
#ifndef NDEBUG
			if(count++%20==0){
			cerr << EncClaw.Get() << '\t' << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << ArmLocation << '\t' << EncArm.Get() << endl; 
				//	cerr << arm1->GetOutputCurrent() << '\t' << arm1_sec->GetOutputCurrent() << '\t' << arm2->GetOutputCurrent() << '\t'
			//	 	<< EncArm.Get () << '\t' << LimitArm.Get() << '\t' << EncClaw.Get() << '\t' << LimitClaw.Get() << '\t' << ClawLocation << endl;
				//cerr << '\t' << EncArm.Get() <<'\t' << arm1->Get() << endl;
			//	cerr << Dlf->Get() << '\t' << Dlf->GetSpeed() << '\t' << Dlb->GetSpeed() <<'\t' << Drf->GetSpeed() <<'\t' << Drb->GetSpeed() <<endl;//'\t' << line1.Get() << "\t" << line2.Get() << "\t" << line3.Get() << endl;
			//	cerr << '\t' << Dlb->GetSpeed() << '\t';
			}
#endif			

			if(pneumaticCount++==0) {
				ClawOpen.Set(false);
				ClawClose.Set(false);
				MiniBot1a.Set(false);
				MiniBot1b.Set(false);
				MiniBot2a.Set(false);
				MiniBot2b.Set(false);
			}
			
			Wait(0.01);				// wait for a motor update time
		}
	}
Exemple #15
0
	/**
	 * Runs the motors with Mecanum drive.
	 */
	void OperatorControl()//teleop code
	{
		robotDrive.SetSafetyEnabled(false);
		gyro.Reset();
		grabEncoder.Reset();
		timer.Start();
		timer.Reset();
		double liftHeight = 0; //variable for lifting thread
		int liftHeightBoxes = 0; //another variable for lifting thread
		int liftStep = 0; //height of step in inches
		int liftRamp = 0; //height of ramp in inches
		double grabPower;
		bool backOut;
		uint8_t toSend[10];//array of bytes to send over I2C
		uint8_t toReceive[10];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 1;//set the byte to send to 1
		i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
		bool isGrabbing = false;//whether or not grabbing thread is running
		bool isLifting = false;//whether or not lifting thread is running
		bool isBraking = false;//whether or not braking thread is running
		float driveX = 0;
		float driveY = 0;
		float driveZ = 0;
		float driveGyro = 0;
		bool liftLastState = false;
		bool liftState = false; //button pressed
		double liftLastTime = 0;
		double liftTime = 0;
		bool liftRan = true;
		Timer switchTimer;
		Timer grabTimer;
		switchTimer.Start();
		grabTimer.Start();


		while (IsOperatorControl() && IsEnabled())
		{
			// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
			// This sample does not use field-oriented drive, so the gyro input is set to zero.

			toSend[0] = 1;
			numToSend = 1;


			driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
			driveY = driveStick.GetRawAxis(Constants::driveYAxis);
			driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
			driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;


			if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
				toSend[0] = 6;
				numToSend = 1;

				if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
					driveY = 0;
					driveZ = 0;
				}
				else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
					driveX = 0;
					driveZ = 0;
				}
				else {//if Z is greater than X and Y, then it will only go in the direction of Z
					driveX = 0;
					driveY = 0;
				}
			}

			if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
				toSend[0] = 7;
				driveZ = 0;//Stops Z while Z lock is pressed
			}

			if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
				driveGyro = 0;//gyro stops while field lock is enabled
			}

			driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
			driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
			driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
			robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive




			if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
				pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
				if(grabTimer.Get() < 1) {
					toSend[0] = 6;
				}
			}
			else {
				pickup.setGrabber(0);
				grabTimer.Reset();
				toSend[0] = 6;
			}

			if (Constants::grabLiftInverted) {
				pickup.setLifter(-Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
			}
			else {
				pickup.setLifter(Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree)); //defines the lifter
			}


			SmartDashboard::PutNumber("Lift Power", Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree));
			SmartDashboard::PutBoolean("Is Lifting", isLifting);

			if (Constants::scaleJoysticks(grabStick.GetY(), Constants::liftDeadZone, Constants::liftMax, Constants::liftDegree) != 0 || isLifting) { //if the robot is lifting
				isBraking = false; //stop braking thread
				SmartDashboard::PutBoolean("Braking", false);
			}
			else if(!isBraking) {
				isBraking = true; //run braking thread
				pickup.lifterBrake(isBraking);//brake the pickup
			}



			if (grabStick.GetRawButton(Constants::liftFloorButton)) {
				liftHeight = 0;
				pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
				liftRan = true;
			}

			liftTime = timer.Get();
			liftState = grabStick.GetRawButton(Constants::liftButton);

			if (liftState) { //if button is pressed
				if (!liftLastState) {
					if (liftTime - liftLastTime < Constants::liftMaxTime) {
						if (liftHeightBoxes < Constants::liftMaxHeightBoxes) {
							liftHeightBoxes++; //adds 1 to liftHeightBoxes
						}
					}
					else {
						liftHeightBoxes = 1;
						liftRamp = 0;
						liftStep = 0;
					}
				}
				liftLastTime = liftTime;
				liftLastState = true;
				liftRan = false;
			}
			else if (grabStick.GetRawButton(Constants::liftRampButton)) {
				if (liftTime - liftLastTime > Constants::liftMaxTime) {
					liftHeight = 0;
					liftStep = 0;
				}
				liftRamp = 1; //prepares to go up ramp
				liftLastTime = liftTime;
				liftRan = false;
			}
			else if (grabStick.GetRawButton(Constants::liftStepButton)) {
				if (liftTime - liftLastTime > Constants::liftMaxTime) {
					liftHeight = 0;
					liftRamp = 0;
				}
				liftStep = 1; //prepares robot for step
				liftLastTime = liftTime;
				liftRan = false;
			}
			else {
				if (liftTime - liftLastTime > Constants::liftMaxTime && !liftRan) {

					liftHeight = liftHeightBoxes * Constants::liftBoxHeight + liftRamp * Constants::liftRampHeight + liftStep * Constants::liftStepHeight; //sets liftHeight
					if (liftHeightBoxes > 0) {
						liftHeight -= Constants::liftBoxLip;
					}
					pickup.lifterPosition(liftHeight, isLifting, grabStick);//start lifting thread
					liftRan = true;
				}
				liftLastState = false;
			}

			if (grabStick.GetRawButton(Constants::grabToteButton)) {//if grab button is pressed
				grabPower = Constants::grabToteCurrent;
				backOut = true;
				if (!isGrabbing) {
					pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
				}
			}
			else if (grabStick.GetRawButton(Constants::grabBinButton)) {//if grab button is pressed
				grabPower = Constants::grabBinCurrent;

				backOut = false;
				if (!isGrabbing) {
					pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);//start grabber thread
				}
			}
			else if (grabStick.GetRawButton(Constants::grabChuteButton)) {//if grab button is presset
				SmartDashboard::PutBoolean("Breakpoint -2", false);
				SmartDashboard::PutBoolean("Breakpoint -1", false);
				SmartDashboard::PutBoolean("Breakpoint 0", false);
				SmartDashboard::PutBoolean("Breakpoint 1", false);
				SmartDashboard::PutBoolean("Breakpoint 2", false);
				SmartDashboard::PutBoolean("Breakpoint 3", false);
				SmartDashboard::PutBoolean("Breakpoint 4", false);
				//Wait(.5);
				if (!isGrabbing) {
					//pickup.grabberChute(isGrabbing, grabStick);//start grabber thread
				}
			}

			//determines what the LED's look like based on what the Robot is doing
			if (isGrabbing) {
				toSend[0] = 5;
				numToSend = 1;
			}
			if (isLifting) {//if the grabbing thread is running
				if (Constants::encoderToDistance(liftEncoder.Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < liftHeight) {
					toSend[0] = 3;
				}
				else {
					toSend[0] = 4;
				}
				numToSend = 1;//sends 1 byte to I2C
			}

			if(!grabOuterLimit.Get()) { //tells if outer limit is hit with lights
				if(switchTimer.Get() < 1) {
					toSend[0] = 6;
				}
			}
			else {
				switchTimer.Reset();
			}

			if (driveStick.GetRawButton(Constants::sneakyMoveButton)) {
				toSend[0] = 0;
				numToSend = 1;
			}

			float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12;	// distance from ultrasonic sensor
			float rotations = (float) liftEncoder.Get();	// rotations on encoder
			SmartDashboard::PutNumber("Distance", distance);	// write stuff to smart dash
			SmartDashboard::PutNumber("Current", pdp.GetCurrent(Constants::grabPdpChannel));
			SmartDashboard::PutNumber("LED Current", pdp.GetCurrent(Constants::ledPdpChannel));
			SmartDashboard::PutNumber("Lift Encoder", rotations);
			SmartDashboard::PutNumber("Lift Height", liftHeight);
			SmartDashboard::PutNumber("Grab Encoder", grabEncoder.Get());
			SmartDashboard::PutBoolean("Grab Inner", grabInnerLimit.Get());
			SmartDashboard::PutBoolean("Grab Outer", grabOuterLimit.Get());
			SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
			SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
			SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
			SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
			SmartDashboard::PutNumber("Throttle", grabStick.GetZ());


			i2c.Transaction(toSend, 1, toReceive, 0);//send and receive information from arduino over I2C
			Wait(0.005); // wait 5ms to avoid hogging CPU cycles
		} //end of teleop
		isBraking = false;
		toSend[0] = 0;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
	}
	void Autonomous(void)
	{
#if 1
		/*int autoMode = 0;
		autoMode |= bcd1->Get();
		autoMode <<= 1;
		autoMode |= bcd2->Get();
		autoMode <<= 1;
		autoMode |= bcd3->Get()
		;*/
		//double ignoreLineStart = 0;
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.2);		
		float liftSP = 0;
		PIDLift->SetTolerance(10);
		PIDLift->SetContinuous(false);
		PIDLift->SetOutputRange(-0.75, 1); //BUGBUG
		PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
		PIDLift->Enable();
		PIDGrip->SetSetpoint(0);
		PIDGrip->Enable();
		stopCount = 0;

		float reduction;
		int counts = 0;
		leftEncoder->Start();
		rightEncoder->Start();
		leftEncoder->Reset();
		rightEncoder->Reset();
		liftEncoder->Start();
		liftEncoder->Reset();
		leftEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
		rightEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
		double avgEncoderCount;
		float leftSpeed = .2, rightSpeed = .2;
		short int lsL,lsM,lsR;
		lineFollowDone = false;
		counts = 0;
		//int fancyWaiter = 0;
		int popstage = 0;
		goPop = false;
		double backStart = 0;
		double backTime = 0;
		double popStart = 0;
		double popTime = 0;
		bool backDone = false;
		miniDep->Set(miniDep->kForward);
		
		int liftCount = 0;
		bool disengageBrake = false;
		float lastLiftSP = 0;
		
		gripOpen1->Set(true);
		gripOpen2->Set(false);
		
		gripLatch1->Set(true);
		gripLatch2->Set(false);
		
		
		while(IsAutonomous())
		{
			if(!(counts % 100))printf("%2.2f \n",getDistance());
			if(backStart) backTime = GetClock();
			if(popStart) popTime = GetClock();
			
			//if(!ignoreLineStart)ignoreLineStart = GetClock();
			
			if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kOff);
			
			if(counts%3 == 0)
			{
				leftValue = lsLeft->Get();
				middleValue = lsMiddle->Get();
				rightValue = lsRight->Get();
			}
			counts++;
			GetWatchdog().Feed();
			//avgEncoderCount = (leftEncoder->Get() + rightEncoder->Get())/2;
			//myRobot.SetLeftRightMotorOutputs(.2,.2);
			
			//All three/five autonomous programs will do the same thing up until 87 inches from the wall
			
			if (counts % 100 == 0){//TESTING
				if(lsLeft->Get()){
					lsL = 1;
				}else{
					lsL = 0;
				}
				if(lsRight->Get()){
					lsR = 1;
				}else{
					lsR = 0;
				}
				if(lsMiddle->Get()){
					lsM = 1;
				}else{
					lsM = 0;
				}
				//printf("Encoder: %2.2f \n", (float)avgEncoderCount);
				//printf("Distance: %2.2f SensorL:%1d SensorM:%1d SensorR:%1d\n",getDistance(),lsL,lsM,lsR);//TESTING
			}
			
#if FOLLOWLINE
			/*if(GetClock() - ignoreLineStart <= 0.5)
			{
				frontLeftMotor->Set(-.4);
				rearLeftMotor->Set(-.4);
				frontRightMotor->Set(.4);
				rearRightMotor->Set(.4);
			}
			else */if (false){//(avgEncoderCount <= SECONDBREAKDISTANCE){
				followLine();
			}
#else
			if (getDistance() > 24){
				frontLeftMotor->Set(-leftSpeed);
				rearLeftMotor->Set(-leftSpeed);
				frontRightMotor->Set(rightSpeed);
				rearRightMotor->Set(rightSpeed);
				if(leftEncoder->Get() > rightEncoder->Get() && leftSpeed == .2){
					rightSpeed += .03;
				}else if(leftEncoder->Get() >rightEncoder->Get() && leftSpeed > .2){
					leftSpeed -= .03;
				}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed == .2){
					leftSpeed += .03;
				}else if(leftEncoder->Get() < rightEncoder->Get() && rightSpeed > .2){
					rightSpeed -= .03;
				}
			}
#endif
			else{
				if(counts % 100 == 0) {printf("DISTANCE: %2.2f\n",getDistance());}
				switch(FOLLOWLINE)
				{
				case STRAIGHTLINEMODE: //Straight line. Scores on middle column of left or right grid.
					//if(lineFollowDone && !(counts %50)) printf("Lift error: %f \n", PIDLift->GetError());
					lastLiftSP = liftSP;
					
					if(!lineFollowDone)
					{
						followLine();
					}
					else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
					{
						//if(counts % 50 == 0)printf("Go backward\n");
						frontLeftMotor->Set(.3);
						rearLeftMotor->Set(.3);
						frontRightMotor->Set(-.3);
						rearRightMotor->Set(-.3);
						//PIDLift->SetSetpoint(LIFTMID2);
						liftSP = LIFTHIGH2 + 0.025;
						//fancyWaiter = counts;
						backStart = GetClock();
						printf("Setpoint set setpoint set setpoint set \n");
						/*
						if(leftValue && middleValue && rightValue)
						{
							printf("Stopped 2nd time\n");
							goPop = true;
							frontLeftMotor->Set(0);
							rearLeftMotor->Set(0);
							frontRightMotor->Set(0);
							rearRightMotor->Set(0);
							PIDLift->SetSetpoint(LIFTHIGH2);
						}
						*/
					}
#if 1				//if we've backed up for half a second and we're not popping
					else if((backTime - backStart) > 0.65 && !backDone)
					{
						printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
						frontLeftMotor->Set(0);
						rearLeftMotor->Set(0);
						frontRightMotor->Set(0);
						rearRightMotor->Set(0);
						//PIDLift->SetSetpoint(LIFTMID2);
						liftSP = LIFTHIGH2;
						backDone = true;
						//Wait(.01);
						//lift->brakeOff();
						//fancyWaiter = counts;
						//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
					}
					/*
					else if(lift->getPosition() < LIFTHIGH2)
					{
						//Move teh lifts
						//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
						PIDLift->SetSetpoint(LIFTHIGH2);
					}
					*/
					//if the lift is at the top and we're done backing up
					else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
					{
						if(!popStart) popStart = GetClock();
						if(popstage == 0)
						{
							//lift->brakeOn();
							//PIDLift->SetSetpoint(lift->getPosition());
							popstage++;
							printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
						}
						else if(popstage == 1 && popTime - popStart > 0.1)
						{
#if !(INNERGRIP)
							gripOpen1->Set(true);
							gripOpen2->Set(false);
#else
							gripOpen1->Set(false);
							gripOpen2->Set(true);
#endif
							popstage++;
							printf("OPEN OPEN OPEN OPEN OPEN \n");
						}
						else if(popstage == 2 && popTime - popStart > 0.35)
						{
							gripPop1->Set(true);
							gripPop2->Set(false);
							popstage++;
							printf("POP POP POP POP POP POP POP \n");
						}
						else if(popstage == 3 && popTime - popStart > 1.35)
						{
							gripPop1->Set(false);
							gripPop2->Set(true);
							
							frontLeftMotor->Set(.2);
							rearLeftMotor->Set(.2);
							frontRightMotor->Set(-.2);
							rearRightMotor->Set(-.2);
							
							popstage++;
							printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
						}
						else if(popstage == 4 && popTime - popStart > 1.85)
						{
							printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
							frontLeftMotor->Set(0);
							rearLeftMotor->Set(0);
							frontRightMotor->Set(0);
							rearRightMotor->Set(0);
							//PIDLift->SetSetpoint(0);
							liftSP = 0;
						}
						/*
						 else if(popstage == 4 && popTime - popStart > 1.85)
						{
							printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
							frontLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
							rearLeftMotor->Set(min(0.3, 0.25*(popTime - popStart - 1.85)));
							frontRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
							rearRightMotor->Set(max(-0.3, -0.25*(popTime - popStart - 1.85)));
							//PIDLift->SetSetpoint(0);
							liftSP = 0;
							popstage++;
						}
						else if(popstage == 5 && popTime - popStart > 4.85)
						{
							frontLeftMotor->Set(0);
							rearLeftMotor->Set(0);
							frontRightMotor->Set(0);
							rearRightMotor->Set(0);
						}
						*/
					}
					
					//Start tele-op lift code
					if(!lift->isBraking() && !disengageBrake)
					{
						PIDLift->SetSetpoint(liftSP);
						if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1)
						{
							//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
							PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1);
						}
						else PIDLift->SetOutputRange(-0.75, 1);
					}
					if(lift->isBraking() && lastLiftSP != liftSP)
					{
						PIDLift->SetSetpoint(lift->getPosition() + 0.04);
						PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
						lift->brakeOff();
						disengageBrake = true;
						if(!liftCount) liftCount = counts;
					}
					//set brake (because near)
					if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
					{
						if(liftCount == 0) liftCount = counts;
						if(counts - liftCount > 1000)
						{
							PIDLift->Reset();
							liftMotor1->Set(LIFTNEUTRALPOWER);
							liftMotor2->Set(LIFTNEUTRALPOWER);
							Wait(0.02);
							lift->brakeOn();
							Wait(0.02);
							liftMotor1->Set(0.0);
							liftMotor2->Set(0.0);
							PIDLift->Enable();
							//PIDLift->SetSetpoint(lift->getPosition());
							liftCount = 0;
						}
						if(counts%3000) printf("Braking/Not PID \n");
					}
					if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
					if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
					{
						disengageBrake = false;
						PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
						liftCount = 0;
					}
					//End tele-op lift code
#endif
					//myRobot.SetLeftRightMotorOutputs(0,0);
					break;
				case NOLINEMODE: //Fork turn left. Scores on far right column of left grid.
					lineFollowDone = true;
					if(!lineFollowDone){}
					else if(!PIDLift->GetSetpoint() && !popstage && !backStart)
					{
					//if(counts % 50 == 0)printf("Go backward\n");
						frontLeftMotor->Set(.3);
						rearLeftMotor->Set(.3);
						frontRightMotor->Set(-.3);
						rearRightMotor->Set(-.3);
						//PIDLift->SetSetpoint(LIFTMID2);
						liftSP = LIFTHIGH2;
						//fancyWaiter = counts;
						backStart = GetClock();
						printf("Setpoint set setpoint set setpoint set \n");
						/*
						if(leftValue && middleValue && rightValue)
						{
							printf("Stopped 2nd time\n");
							goPop = true;
							frontLeftMotor->Set(0);
							rearLeftMotor->Set(0);
							frontRightMotor->Set(0);
							rearRightMotor->Set(0);
							PIDLift->SetSetpoint(LIFTHIGH2);
						}
						*/
					}
#if 1				//if we've backed up for half a second and we're not popping
					else if((backTime - backStart) > 0.65 && !backDone)
					{
						printf("Stopping!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!ONE\n");
						frontLeftMotor->Set(0);
						rearLeftMotor->Set(0);
						frontRightMotor->Set(0);
						rearRightMotor->Set(0);
						//PIDLift->SetSetpoint(LIFTMID2);
						liftSP = LIFTHIGH2;
						backDone = true;
						//Wait(.01);
						//lift->brakeOff();
						//fancyWaiter = counts;
						//printf("Fancy waiter set Fancy waiter set Fancy waiter set");
					}
					/*
					else if(lift->getPosition() < LIFTHIGH2)
					{
						//Move teh lifts
						//if(counts % 100 == 0) printf("Stopping because lineFollowDone == true\n");
						PIDLift->SetSetpoint(LIFTHIGH2);
					}
					*/
					//if the lift is at the top and we're done backing up
					else if(PIDLift->GetSetpoint() && fabs(liftSP - lift->getPosition()) < 0.015 && backDone)
					{
						if(!popStart) popStart = GetClock();
						if(popstage == 0)
						{
							//lift->brakeOn();
							//PIDLift->SetSetpoint(lift->getPosition());
							popstage++;
							printf("BRAKE BRAKE BRAKE BRAKE BRAKE \n");
						}
						else if(popstage == 1 && popTime - popStart > 0.1)
						{
#if !(INNERGRIP)
							gripOpen1->Set(true);
							gripOpen2->Set(false);
#else
							gripOpen1->Set(false);
							gripOpen2->Set(true);
#endif
							popstage++;
							printf("OPEN OPEN OPEN OPEN OPEN \n");
						}
						else if(popstage == 2 && popTime - popStart > 0.35)
						{
							gripPop1->Set(true);
							gripPop2->Set(false);
							popstage++;
							printf("POP POP POP POP POP POP POP \n");
						}
						else if(popstage == 3 && popTime - popStart > 1.35)
						{
							gripPop1->Set(false);
							gripPop2->Set(true);
							
							frontLeftMotor->Set(.2);
							rearLeftMotor->Set(.2);
							frontRightMotor->Set(-.2);
							rearRightMotor->Set(-.2);
							
							popstage++;
							printf("UNPOP UNPOP UNPOP UNPOP UNPOP \n");
						}
						else if(popstage == 4 && popTime - popStart > 1.85)
						{
							printf("DOWN DOWN DOWN DOWN DOWN DOWN \n");
							frontLeftMotor->Set(0);
							rearLeftMotor->Set(0);
							frontRightMotor->Set(0);
							rearRightMotor->Set(0);
							//PIDLift->SetSetpoint(0);
							liftSP = 0;
						}
					}
					
					//Start tele-op lift code
					if(!lift->isBraking() && !disengageBrake)
					{
						PIDLift->SetSetpoint(liftSP);
						if(liftSP == 0 && liftPIDSource->PIDGet() < 0.1) //BUGBUG
						{
							//PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, 3*LIFTDERIVGAIN);
							PIDLift->SetOutputRange(-liftPIDSource->PIDGet() - 0.1, 1.); //BUGBUG
						}
						else PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
					}
					if(lift->isBraking() && lastLiftSP != liftSP)
					{
						PIDLift->SetSetpoint(lift->getPosition() + 0.04);
						PIDLift->SetPID(11.5 + 2*lift->getPosition(), LIFTINTGAIN + 0.4 + 3*lift->getPosition()/10, 0);
						lift->brakeOff();
						disengageBrake = true;
						if(!liftCount) liftCount = counts;
					}
					//set brake (because near)
					if(fabs(PIDLift->GetError()) < 0.015 && !lift->isBraking() && !disengageBrake)
					{
						if(liftCount == 0) liftCount = counts;
						if(counts - liftCount > 1000)
						{
							PIDLift->Reset();
							liftMotor1->Set(LIFTNEUTRALPOWER);
							liftMotor2->Set(LIFTNEUTRALPOWER);
							Wait(0.02);
							lift->brakeOn();
							Wait(0.02);
							liftMotor1->Set(0.0);
							liftMotor2->Set(0.0);
							PIDLift->Enable();
							//PIDLift->SetSetpoint(lift->getPosition());
							liftCount = 0;
						}
						if(counts%3000) printf("Braking/Not PID \n");
					}
					if(lift->isBraking() && !(counts%100000)) PIDLift->SetSetpoint(lift->getPosition());
					if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && counts - liftCount > 1000)
					{
						disengageBrake = false;
						PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
						liftCount = 0;
					}
					//End tele-op lift code
#endif
					//myRobot.SetLeftRightMotorOutputs(0,0);
					break;
				case FORKRIGHTMODE://Fork turn right. Scores on far left column of right grid.
					if(leftEncoder->GetDistance() <= rightEncoder->GetDistance() + 6)
					{
						frontLeftMotor->Set(.2);
						rearLeftMotor->Set(.2);
						frontRightMotor->Set(0);
						rearRightMotor->Set(0);
					}
					else if(getDistance() >= SCOREDISTANCE) 
					{
						followLine();
					}
					//score here				
					//myRobot.SetLeftRightMotorOutputs(0,0);
					break;
				}
			}
		}
		/*frontRightMotor->Set(0);
		rearRightMotor->Set(0);
		frontLeftMotor->Set(0);
		rearLeftMotor->Set(0);*/
		Wait(.02);
#endif
	}
Exemple #17
0
	void OperatorControl(void)
	{
		// feed watchdog ---    TheVessel.SetSafetyEnabled(true);
		GetWatchdog().Kill();
		
		if (!game){
		AlmostZeroPi = manEnc->GetDistance() -1;//Zeros manipulator encoder-- only here for tele op practice w/out auto
		cmp->Start();
		}
		
		cd->MecanumDrive(0, 0, 0);
		minibot->Set(DoubleSolenoid::kForward);
		while(IsOperatorControl() && IsEnabled())
		{
			GetWatchdog().Kill();
			
			if(IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kReverse);
			else if(!IO.GetDigital(10)) minibot->Set(DoubleSolenoid::kForward);
		
			cd->MecanumDrive(controller);//passes joystick controller's input to cd mechdrv
			
			if(stick->GetRawButton(1))
			{
			}
			else if(stick->GetRawButton(2))
			{
			}
			else if(stick->GetRawButton(3))
			{
				reset = true;//              ENABLES NO RESTRICTION MANIPULATOR MOVEMENT
			}
			else if(stick->GetRawButton(4))
			{
				cout << "Manipulator Encoder Value:   " << manEnc->Get() << "/n";//prints manip encoder to console if open;
				Wait(.1f);//pause so not above a billion times
			}
			else if(stick->GetRawButton(6))
			{
				open->Set(true);//self explanatory, eote pnuematics
				closed->Set(false);
			}
			else if(stick->GetRawButton(8))
			{
				closed->Set(true);
				open->Set(false);
			}
			
			

			ShoulderJag->Set(stick->GetY());//shoulder moves with stick->GetY()
			if (stick->GetTwist() < 0 || manEnc->GetDistance() < AlmostZeroPi || reset)//man extending, enc not at 0, or reset true
			{
				manJag->Set(stick->GetTwist()*.3);//shoulder above but for wrist, also, only 3/10ths power
			}

		
		}
		cmp->Stop();//stops compressor
		while (manEnc->GetDistance() < AlmostZeroPi)//brings manipulator back to 0 so not need reset
		{
			manJag->Set(.1);
		}
	}