Exemple #1
0
	RobotDemo()
	{
		//Initialize the objects for the drive train
		myRobot = new RobotDrive(1, 2);
		leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
		rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
		leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		leftEnco->Start();
		rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		rightEnco->Start();
		leftStick = new Joystick(1);
		rightStick = new Joystick(2);

		//Initialize the gyro
		gyro = new Gyro(GYRO_PORT);
		gyro->Reset();

		//Initialize the manipulators
		intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
		catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
				LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);

		//Initialize the objects needed for camera tracking
		rpi = new RaspberryPi("17140");
		LEDLight = new Relay(1);
		LEDLight->Set(Relay::kForward);

		//Set the autonomous modes
		autonMode = ONE_BALL_AUTON;
		autonStep = AUTON_ONE_SHOOT;

		//Initialize the lcd
		lcd = DriverStationLCD::GetInstance();
	}
    BuiltinDefaultCode()	{

        m_robotDrive = new RobotDrive (m_lDrive, m_rDrive);
        m_driver = new Joystick (1);
        m_operator = new Joystick (2);
        m_rDrive = new Victor (1);
        m_lDrive = new Victor (2);
        m_climber = new Victor (3);
        m_plate1 = new Victor (8);
        m_plate2 = new Victor (10);
        m_launcher1 = new Victor (4);
        m_launcher2 = new Victor (5);
        m_launcher3 = new Victor (6);
        m_feeder = new Victor (7);
        m_ratchet = new Relay (1);
        m_left = new Encoder (1,2,true);
        m_left->SetDistancePerPulse(1);
        m_left->SetMaxPeriod(1.0);
        m_left->Start();
        m_right = new Encoder (3,4,false);
        m_right->SetDistancePerPulse(1);
        m_right->SetMaxPeriod(1.0);
        m_right->Start();
        m_plateh = new AnalogChannel (1);
        m_climberp = new AnalogChannel (2);
        m_dsLCD = DriverStationLCD::GetInstance();

    }
Exemple #3
0
	RobotSystem(void):
		robotInted(false)
		,stick(1)		// as they are declared above.
		,stick2(2)
		,line1(10)
		,line2(11)
		,line3(12)
		//,camera(AxisCamera::GetInstance())
		,updateCAN("CANUpdate",(FUNCPTR)UpdateCAN)
		,cameraTask("CAMERA", (FUNCPTR)CameraTask)
		,compressor(14,1)
		,EncArm(2,3)
		,EncClaw(5,6)
		,PIDArm(.04,0,0) // .002, .033
		,PIDClaw(.014,.0000014,0)
		,LowArm(.1)
		/*
		,MiniBot1(4)
		,MiniBot2(2)
		,ClawGrip(3)
		*/
		,MiniBot1a(8,1)
		,MiniBot1b(8,2)
		,MiniBot2a(8,3)
		,MiniBot2b(8,4)
		,ClawOpen(8, 8)
		,ClawClose(8,7)
		,LimitClaw(7)
		,LimitArm(13)
	{
	//	myRobot.SetExpiration(0.1);
		GetWatchdog().SetEnabled(false);
		GetWatchdog().SetExpiration(1);
		compressor.Start();
		debug("Waiting to init CAN");
		Wait(2);
		
		Dlf = new CANJaguar(6,CANJaguar::kSpeed);
		Dlb = new CANJaguar(3,CANJaguar::kSpeed);
		Drf = new CANJaguar(7,CANJaguar::kSpeed);
		Drb = new CANJaguar(2,CANJaguar::kSpeed);
		arm1 = new CANJaguar(5);
		arm1_sec = new CANJaguar(8);
		arm2 = new CANJaguar(4);
		
		
		EncArm.SetDistancePerPulse(.00025);
		EncClaw.SetDistancePerPulse(.00025);
		EncClaw.SetReverseDirection(false);
		EncArm.SetReverseDirection(true);
		EncArm.Reset();
		EncClaw.Reset();
		
		
		updateCAN.Start((int)this);
		//cameraTask.Start((int)this);
		EncArm.Start();
		EncClaw.Start();
		debug("done initing");
	}
	void TeleopInit(void) {
		printf("Robot teleop initializing...\n");

		GetWatchdog().Feed();
		
		compressor->Start();
		kicker->SafeReset();
		
		leftDrivetrainEncoder->Start();
		rightDrivetrainEncoder->Start();
		
		printf("Robot teleop initialization complete.\n");
	}
	// 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();
		}
	}
	void OperatorControl(void)
	{
		NetTest();
		return;
		myRobot.SetSafetyEnabled(true);
		digEncoder.Start();
		const double ppsTOrpm = 60.0/250.0;   //Convert from Pos per Second to Rotations per Minute by multiplication
                                              // (See the second number on the back of the encoder to replace 250 for different encoders)
        const float VoltsToIn = 41.0;         // Convert from volts to cm by multiplication (volts from ultrasonic).
                                              // This value worked for distances between 1' and 10'.
		
		while (IsOperatorControl())
		{
			if (stick.GetRawButton(4)) {
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), -1);
			} 
			else if (stick.GetRawButton(5))
			{
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 1);
			}
			else 
			{
				myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
			}
			
			myRobot.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(), 0);
			
			SmartDashboard::PutNumber("Digital Encoder RPM", abs(digEncoder.GetRate()*ppsTOrpm));
			SmartDashboard::PutNumber("Ultrasonic Distance inch", (double) ultra.GetAverageVoltage()*VoltsPerInch);
			SmartDashboard::PutNumber("Ultrasonic Voltage", (double) ultra.GetAverageVoltage());

			Wait(0.1);
		}
		digEncoder.Stop();
	}
	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();
	}	
/**
 * Start the encoder counting.
 *
 * @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
 */
void StartEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
	Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
	if (encoder != NULL)
	{
		encoder->Start();
	}
}
Exemple #9
0
    // WHen robot is enabled
    void init() {
        log->info("initializing");
        log->print();
        climber = NULL;
        leftDriveEncoder->Reset();
        leftDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
        leftDriveEncoder->Start();
		rightDriveEncoder->Reset();
        rightDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
        rightDriveEncoder->Start();
        //leftClimber->motorController->Disable();
		leftClimber->encoder->Reset();
        leftClimber->encoder->Start();
        //rightClimber->motorController->Disable();
        rightClimber->encoder->Reset();
        rightClimber->encoder->Start();
        bcdValue = bcd->value();
        loadSwitchOldState = loaderSwitch->Get();
        #if 0
		bool leftDone = false;
		bool rightDone = false;
        // Only do this for some BCD values?
        while (!leftDone || !rightDone){
			if (!leftDone)
				leftDone = leftClimber->UpdateState(-1.0, -30);
			if (!rightDone)
				rightDone = rightClimber->UpdateState(-1.0, -30);
	        log->info("Wait: Ll Rl: %d %d",
	        		leftClimber->lowerLimitSwitch->Get(),
	        		rightClimber->lowerLimitSwitch->Get());
	        log->print();
		}
#endif
        climbState = NotInitialized;
        cameraElevateAngle =
        		(cameraElevateMotor->GetMaxAngle()-cameraElevateMotor->GetMinAngle()) * 2/3;
        cameraPivotAngle = 0;
        cameraPivotMotor->SetAngle(cameraPivotAngle);
        cameraElevateMotor->SetAngle(cameraElevateAngle);
        loading = false;
        loaderDisengageDetected = false;
        //This is a rough guess of motor power it should be based on voltage
        shooterMotorVolts = 8.0; // volts as a fraction of 12V
    	loadCount = 0;
    }
Exemple #10
0
	PnumaticArmTest (void):
	stick(1)
	,up(7,7)
	,down(7,8)
	,enc(1,2)
	,pid(.045,.00000,.16)
	,low(.05)
	{
		enc.SetReverseDirection(true);
		enc.Start();
	}
Exemple #11
0
	void OperatorControl(void)
	{
		digEncoder.Start();
		const double ppsTOrpm = 60.0/250.0;	//This converts from Pos per Second to Rotations per Minute (See second number on back of encoder to replace 250 if you need it)
		
		while (IsOperatorControl())
		{
			SmartDashboard::PutNumber("Digital Encoder RPM", digEncoder.GetRate()*ppsTOrpm);
			Wait(0.1);
		}
		digEncoder.Stop();
	}
Exemple #12
0
	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl(void)
	{
       encoder.Start();
		while (IsEnabled())
		{
			float controlv = control.GetVoltage();
			motor.Set(controlv/5); //get's voltage from control and divides it by 5
			double rate = encoder.GetRate();
			double distance = encoder.GetDistance();
			printf ("%f %f \n", rate, distance);

		}
	}
	BuiltinDefaultCode()	{
		//Initialze drive controllers
		m_rDrive1 = new Talon (1);
		m_rDrive2 = new Talon (2);
		m_lDrive1 = new Talon (3);
		m_lDrive2 = new Talon (4);

		//Initialize ramrod motor
		m_ramMotor = new Talon (5);

		//Initialize Arm
		m_arm = new ArmWrapper (7, 6, 5, 6, 10);
		m_arm->StartPID(0.0, 0.0, 0.0);

		//initialize bGrabber motor
		m_roller = new Talon (8);

		//Initialize ramrod servo
		m_ramServo = new Servo (9);

		//Initialize drive wrappers
		m_rDrive = new DriveWrapper (m_rDrive1, m_rDrive2);
		m_lDrive = new DriveWrapper (m_lDrive1, m_lDrive2);

		//Initialize robot drive
		m_robotDrive = new RobotDrive (m_lDrive, m_rDrive);

		//Initialize ramrod encoder
		m_ramEncoder = new Encoder (7,8,false);
		m_ramEncoder->SetDistancePerPulse(1);
		m_ramEncoder->SetMaxPeriod(1.0);
		m_ramEncoder->Start();

		//Initialize Compressor
		m_compressor = new Compressor(9, 1);

		//shifters
		m_shifters = new Solenoid(1);

		//Initialize bGrabber Solenoids
		m_bArm = new Solenoid (2);
		m_catch = new Solenoid (3);

		//Initialize joysticks
		m_driver = new JoystickWrapper (1);
		m_operator = new JoystickWrapper (2);

		//Grab driver station object
		m_dsLCD = DriverStationLCD::GetInstance();		
	}
Exemple #14
0
	RobotDemo(void):
		leftDriveMotor(LEFT_DRIVE_PWM),
		rightDriveMotor(RIGHT_DRIVE_PWM),
		myRobot(&leftDriveMotor, &rightDriveMotor),	// these must be initialized in the same order
		stick(1),									// as they are declared above.
		stick2(2),
		gamepad(3),
		collectorMotor(PICKUP_PWM),
		indexerMotor(INDEX_PWM),
		shooterMotor(SHOOTER_PWM),
		armMotor (ARM_PWM),
		leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B),
		shifter(SHIFTER_A,SHIFTER_B),
		greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
		yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
		potentiometer(ARM_ROTATION_POT),
		indexSwitch(INDEXER_SW),
		greenClawLockSwitch(CLAW_1_LOCK_SENSOR),
		yellowClawLockSwitch(CLAW_2_LOCK_SENSOR),
		compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
		jogTimer(),
		shooterTimer()
	{
		m_collectorMotorRunning = false;
		m_shooterMotorRunning   = false;
		m_jogTimerRunning       = false;
		m_shiftCount            = MAX_SHIFTS;
		
		dsLCD = DriverStationLCD::GetInstance();
		dsLCD->Clear();
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);

		dsLCD->UpdateLCD();
		myRobot.SetExpiration(0.1);
		shifter.Set(DoubleSolenoid::kReverse);
		
		leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE);
		leftDriveEncoder.SetMaxPeriod(1.0);
		leftDriveEncoder.SetReverseDirection(true);  // change to true if necessary
		leftDriveEncoder.Start();

	}
Exemple #15
0
	/**
	 * Runs during test mode	```````
	 */
	void Test() {
		TestMode tester(m_ds);
		driveDistanceRight.Reset();
		driveDistanceRight.Start();
		ballGrabber.resetSetPoint();
		shooter.motorShutOff();
		while (IsTest() && IsEnabled()){
			lcd->Clear();
			tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick,
								  &testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic,
								  &ballGrabber.ballDetector, &analogTestSwitch,
								  &shooter, &ballGrabber
								  );
			lcd->UpdateLCD();
			Wait(0.1);
			
		}
		driveDistanceRight.Stop();
		}
	void OperatorControl(void)
	{	
		//Open the file
		

		//setup watchdog	
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.3);
		theEncoder->Start();
		//theLift->Enable();
#if RATE

#endif
		float maxHeight = 0;
		float maxRate = 0;

		while (IsOperatorControl())
		{	
			printf("Distance %f rate %f maxHeight %f maxRate %f\n", 
					theEncoder->GetDistance(),
					theEncoder->GetRate(),
					maxHeight,
					maxRate);
			if(writing)
			{
				write();
			}else if (!closed){
				printf("CLOSED!");
				myfile.close();
				closed = true;
			}
			
			Wait(.01);
			if(theEncoder->GetDistance() > maxHeight) maxHeight = theEncoder->GetDistance();
			if(theEncoder->GetRate() > maxRate) maxRate = theEncoder->GetRate();
			//check whether we should switch modes
			/*if (decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance()) <= MAXDECEL && !decel)
			{
				decelRate = decelToStop(theEncoder->GetRate(),SETPOINT_METERS - theEncoder->GetDistance());
				cout<<"DecelRate: "<<decelRate<<endl;
				decel = true;
				printf("BEGINNING DECELERATION\n");
				initTime = GetTime();
			}*/
			
			/*if(theEncoder->GetDistance() > 1.0)
			{
				theJag->Set(0.0);
				writing = false;
				//theLift->SetPID(.3,0,0);
				//theLift->SetSetpoint(0.0);
				done = true;
			}else if(!done){
				theJag->Set(1.0);
				//theLift->SetSetpoint(1.5);
			}
			*/
			if(theEncoder->GetDistance() >= SETPOINT_METERS)
			{
				done = true;
				theJag->Set(0.0);
				writing = false;
			}else if (!done){
				theJag->Set(1.0);
			}
			rateSetpoint = vFinal + (decelRate * (GetTime() / pow(10,9)));
#if 0
			if(theEncoder->GetDistance() >= SETPOINT_METERS)
			{
				theJag->Set(0.0);
				writing = false;
			}else if(decel){
#if RATE
				//theLift->SetSetpoint(rateSetpoint);
				theJag->Set(0.0);
#else
				theLift->SetSetpoint(SETPOINT_METERS);
#endif
			}else{
				theJag->Set(1.0);
			}
#if RATE
			
#else
			theLift->SetSetpoint(SETPOINT_METERS);
#endif
#endif
			GetWatchdog().Feed();
		}
	}
	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
	}
	void OperatorControl(void)
	{
		autonomous = false;
		//myRobot.SetSafetyEnabled(false);
		//myRobot.SetInvertedMotor(kFrontLeftMotor, true);
		//	myRobot.SetInvertedMotor(3, true);
		//variables for great pid
		double rightSpeed,leftSpeed;
		float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP;
		float stickY[2];
		float stickYAbs[2];
		bool lightOn = false;
		AxisCamera &camera = AxisCamera::GetInstance();
		camera.WriteResolution(AxisCameraParams::kResolution_160x120);
		camera.WriteMaxFPS(5);
		camera.WriteBrightness(50);
		camera.WriteRotation(AxisCameraParams::kRotation_0);
		rightEncoder->Start();
		leftEncoder->Start();
		liftEncoder->Start();
		rightEncoder->Reset();
		leftEncoder->Reset();
		liftEncoder->Reset();
		bool fastest = false; //transmission mode
		float reduction = 1; //gear reduction from 
		bool bDrivePID = false;
		//float maxSpeed = 500;
		float liftPower = 0;
		float liftPos = -10;
		bool disengageBrake = false;
		int count = 0;
		//int popCount = 0;
		double popStart = 0;
		double popTime = 0;
		int popStage = 0;
		int liftCount = 0;
		int liftCount2 = 0;
		const float LOG17 = log(17.61093344);
		float liftPowerAbs = 0;
		float gripError = 0;
		float gripErrorAbs = 0;
		float gripPropMod = 0;
		float gripIntMod = 0;
		bool shiftHigh = false;
		leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution,
		rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio
		DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO();
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.3);

		PIDDriveLeft->SetOutputRange(-1, 1);
		PIDDriveRight->SetOutputRange(-1, 1);
		//PIDDriveLeft->SetInputRange(-244,244);
		//PIDDriveRight->SetInputRange(-244,244);
		PIDDriveLeft->SetTolerance(5);
		PIDDriveRight->SetTolerance(5);
		PIDDriveLeft->SetContinuous(false);
		PIDDriveRight->SetContinuous(false);
		//PIDDriveLeft->Enable();
		//PIDDriveRight->Enable();
		PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		
		liftSP = 0;
		PIDLift->SetTolerance(10);
		PIDLift->SetContinuous(false);
		PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
		PIDLift->Enable();
		
		gripSP = 0;
		float goalGripSP = 0;
		bool useGoalSP = true;
		bool gripPIDOn = true;
		float gripP[10];
		float gripI[10];
		float gripD[10];
		PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up
		PIDGrip->SetTolerance(5);
		PIDGrip->SetSetpoint(0);
		PIDGrip->Enable();
		miniDep->Set(miniDep->kForward);
		int i = 0;
		while(i < 10)
		{
			gripP[i] = GRIPPROPGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripI[i] = GRIPINTGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripD[i] = GRIPDERIVGAIN;
			i++;
		}

		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			count++;
#if !(SKELETON)
			sendVisionData();
#endif
			/*
			if(LIFTLOW1BUTTON && !(counts%10)) printf("LIFTLOW1BUTTON\n");
			if(LIFTLOW2BUTTON && !(counts%10)) printf("LIFTLOW2BUTTON\n");
			if(LIFTMID1BUTTON && !(counts%10)) printf("LIFTMID1BUTTON\n");
			if(LIFTMID2BUTTON && !(counts%10)) printf("LIFTMID2BUTTON\n");
			if(LIFTHIGH1BUTTON && !(counts%10)) printf("LIFTHIGH1BUTTON\n");
			if(LIFTHIGH2BUTTON && !(counts%10)) printf("LIFTHIGH2BUTTON\n");
			*/
			/*
			if(lsLeft->Get()) printf("LSLEFT\n");
			if(lsMiddle->Get()) printf("LSMIDDLE\n");
			if(lsRight->Get()) printf("LSRIGHT\n");
			*/
			stickY[0] = stickL.GetY();
			stickY[1] = stickR.GetY();
			stickYAbs[0] = fabs(stickY[0]);
			stickYAbs[1] = fabs(stickY[1]);
			if(bDrivePID)
			{
	#if 0
				frontLeftMotor->Set(stickY[0]);
				rearLeftMotor->Set(stickY[0]);
				frontRightMotor->Set(stickY[1]);
				rearRightMotor->Set(stickY[1]);
				
				if(!(count%5)) printf("Speeds: %4.2f %4.2f Outputs: %f %f \n", leftEncoder->GetRate(),
						rightEncoder->GetRate(), frontLeftMotor->Get(), frontRightMotor->Get());
	#endif		
				if(stickYAbs[0] <= 0.05 )
				{
					leftSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveLeft->Reset();
						PIDDriveLeft->Enable();
					}
				}
				else leftSP = stickY[0] * stickY[0] * (stickY[0]/stickYAbs[0]); //set points for pid control
				if(stickYAbs[1] <= 0.05)
				{
					rightSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveRight->Reset();
						PIDDriveRight->Enable();
					}
				}
				else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]);
				
				if (BACKWARDBUTTON)
				{
					tempRightSP = rightSP;
					tempLeftSP = leftSP;
					rightSP = -tempLeftSP;
					leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically.
				}
				
				PIDDriveLeft->SetSetpoint(leftSP);
				PIDDriveRight->SetSetpoint(rightSP);
					
				
				leftSpeed = leftEncoder->GetRate();
				rightSpeed = rightEncoder->GetRate();
				if(!(count++ % 5))
				{
				printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", 
						leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP,
						PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(),
						frontRightMotor->Get());
						
				
				//printf("Throttle value: %f", stickR.GetThrottle());
				if(PIDDriveRight->OnTarget()) printf("Right on \n");
				if(PIDDriveLeft->OnTarget()) printf("Left on \n");
				}
					
				if(PIDRESETBUTTON)
				{
					//PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); 
					//PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN);
					PIDDriveLeft->Reset();
					PIDDriveRight->Reset();
					PIDDriveLeft->Enable();
					PIDDriveRight->Enable();
				}
			}
			else
			{
				if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset();
				if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset();
				if(DEMOSWITCH)
				{
					stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height
					stickY[1] = stickY[0]*(1 - lift->getPosition());
				}
				if(stickYAbs[0] > 0.05)
				{
					frontLeftMotor->Set(stickY[0]);
					rearLeftMotor->Set(stickY[0]);
				}
				else
				{
					frontLeftMotor->Set(0);
					rearLeftMotor->Set(0);
				}
				if(stickYAbs[1] > 0.05)
				{
					frontRightMotor->Set(-stickY[1]);
					rearRightMotor->Set(-stickY[1]);
				}
				else
				{
					frontRightMotor->Set(0);
					rearRightMotor->Set(0);
				}
			}
			
			if(stickL.GetRawButton(2) && stickL.GetRawButton(3) && stickR.GetRawButton(2) &&
					stickR.GetRawButton(3) && BACKWARDBUTTON && !(count%5)) bDrivePID = !bDrivePID;
			
			if((SHIFTBUTTON && shiftHigh) || DEMOSWITCH)
			{
				shifter->Set(shifter->kReverse);
				shiftHigh = false;
				maxSpeed = 12;
			}
			else if(!SHIFTBUTTON && !shiftHigh && !DEMOSWITCH)
			{
				shifter->Set(shifter->kForward);
				shiftHigh = true;
				maxSpeed = 25; //last value 35
			}

			sendIOPortData();
#if !(SKELETON)
			/*
			if(LIGHTBUTTON) lightOn = true;
			else lightOn = false;
			if(!lightOn) light->Set(light->kOff);
			if(lightOn) light->Set(light->kOn);
			*/
			if(!MODESWITCH)
			{
				lastLiftSP = liftSP;
				if(!PIDLift->IsEnabled()) PIDLift->Enable();
				if(LIFTLOW1BUTTON) liftSP = LIFTLOW1;
				if(LIFTLOW2BUTTON) liftSP = LIFTLOW2;
				if(LIFTMID1BUTTON) liftSP = LIFTMID1;
				if(LIFTMID2BUTTON) liftSP = LIFTMID2;
				if(LIFTHIGH1BUTTON) liftSP = LIFTHIGH1;
				if(LIFTHIGH2BUTTON && !(DEMOSWITCH && (stickYAbs[0] > 0.05 || stickYAbs[1] > 0.05))) liftSP = LIFTHIGH2;
				
				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(lastLiftSP + 0.06);
					PIDLift->SetPID(12.5 + 1.5*lift->getPosition(), LIFTINTGAIN + 0.6 + 3*lift->getPosition()/10, 0);
					lift->brakeOff();
					disengageBrake = true;
					if(!liftCount) liftCount = count;
				}
				//set brake (because near)
				if(fabs(PIDLift->GetError()) < 0.01 && !lift->isBraking() && !disengageBrake)
				{
					if(liftCount == 0) liftCount = count;
					if(count - liftCount > 3)
					{
						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(!(count%50)) printf("Braking/Not PID \n");
				}
				if(lift->isBraking() && !(count%10)) PIDLift->SetSetpoint(lift->getPosition());
				if(fabs(PIDLift->GetError()) < 0.01 && disengageBrake && count - liftCount > 3)
				{
					disengageBrake = false;
					if(liftEncoder->PIDGet() < liftSP) PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN - 0.015);
					else PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN + 0.015);
					liftCount = 0;
				}
				
				//pid
				/*
				else if(!(fabs(PIDLift->GetError()) < 0.04) && !lift->isBraking() && liftPos == -20)
				{
					PIDLift->Enable();
					liftPos = -10;
					printf("PID GO PID GO PID GO PID GO PID GO \n");
				}
				*/
				//when liftPos is positive, measures position
				//when liftPos = -10, is pidding
				//when liftPos = -20, has just released brake
			}
			else //(MODESWITCH)
			{
				if(PIDLift->IsEnabled()) PIDLift->Reset();
				liftPower = .8*pow(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116), 2)*(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)/fabs(2*((log(LIFTSLIDER + 0.003/.0208116511)/LOG17) + 0.116)));
				liftPowerAbs = fabs(liftPower);
				if(liftPowerAbs > 1) liftPower /= liftPowerAbs;
				//if(!(count%5)) printf("Slider: %f", liftPower);
				
				if(lift->isBraking() && liftPowerAbs > 0.05) lift->brakeOff();
				else if(!lift->isBraking() && liftPowerAbs <= 0.05 && !(count%5)) lift->brakeOn();
				if (liftPowerAbs > 0.05)
				{
					liftMotor1->Set(liftPower);
					liftMotor2->Set(liftPower);
				}
				else
				{
					liftMotor1->Set(0);
					liftMotor2->Set(0);
				}
			}
			if(MODESWITCH && LIFTLOW1BUTTON && LIFTMID1BUTTON && LIFTHIGH1BUTTON) liftEncoder->Reset();
			/*
			if(!(count%5))
			{
				printf("Lift pos: %f Lift error: %f Lift SP: %f \n", liftPIDSource->PIDGet(),
						PIDLift->GetError(), PIDLift->GetSetpoint());
			}
			*/
			if(!(count%5) && MODESWITCH && GRIPPERPOSUP && GRIPPERPOSDOWN && GRIPPERPOP)
			{	
				gripPIDOn = !gripPIDOn;
				printf("Switch'd\n");
			}
			if(gripPIDOn)	
			{
				if(!PIDGrip->IsEnabled()) PIDGrip->Enable();
				if(GRIPPERPOSUP && !GRIPPERPOSDOWN)
				{
					goalGripSP = 0;
				}
				else if(GRIPPERPOSDOWN && !GRIPPERPOSUP && lift->getPosition() < 0.5)
				{
					goalGripSP = 1;
				}
				/*
				else if(!GRIPPERPOSDOWN && !GRIPPERPOSUP)
				{
					goalGripSP = 0.5;
				}
				*/
				
				gripError = PIDGrip->GetError();
				gripErrorAbs = fabs(gripError);
				PIDGrip->SetSetpoint(goalGripSP);
				
				if(gripErrorAbs < 0.4) PIDGrip->SetOutputRange(-0.4, 0.6); //negative is up
				else PIDGrip->SetOutputRange(-0.9, 0.8); //negative is up
				if(gripErrorAbs > 0.0 && gripErrorAbs < 0.2)
				{
					PIDGrip->SetPID(GRIPPROPGAIN - 1.25*(1 - gripErrorAbs) + gripPropMod, GRIPINTGAIN + gripIntMod, 0.3 + 0.1*(1 - gripPIDSource->PIDGet()));
				}
				else
				{
					PIDGrip->SetPID(GRIPPROPGAIN - 1.*(1 - gripErrorAbs) + gripPropMod, 0, 0.02);
				}
				if(fabs(gripPIDSource->PIDGet()) < 0.03 && PIDGrip->GetSetpoint() == 0)
				{
					gripLatch1->Set(true);
					gripLatch2->Set(false);
				}
				else if(!(gripLatch1->Get() && PIDGrip->GetSetpoint() == 0) || 
						gripPIDSource->PIDGet() < 0) 
				{
					gripLatch1->Set(false);
					gripLatch2->Set(true);
				}
					
				if(gripLatch1->Get() && !(count%20)) 
				{
					PIDGrip->Reset();
					PIDGrip->Enable();
				}
				/*
				if(stickL.GetRawButton(1) && !(count%5)){
					gripIntMod -= 0.001;
				}
				
				if(stickR.GetRawButton(1) &&!(count%5))
				{
					gripIntMod += 0.001;
				}
				if(stickL.GetRawButton(2) && !(count%5))
				{
					gripPropMod -= 0.1;
				}
				if(stickL.GetRawButton(3) && !(count%5))
				{
					gripPropMod += 0.1;
				}
				*/
				/*
				if(LIFTBOTTOMBUTTON)
				{
					PIDGrip->Reset();
					PIDGrip->Enable();
				}
				*/
				
				if(!(count%5))
				{
					printf("Gripper pos: %f Gripper error: %f Grip power: %f \n",
							gripPIDSource->PIDGet(), PIDGrip->GetError(), gripMotor1->Get());
				}
				
			
			}

			//Calibration routine
			else
			{
				if(gripLatch1->Get() == true) 
				{
					gripLatch1->Set(false);
					gripLatch2->Set(true);
				}
				if(PIDGrip->IsEnabled()) PIDGrip->Reset();
				if(GRIPPERPOSUP)
				{
					gripMotor1->Set(-0.5);
					//gripMotor2->Set(0.5);
				}
				else if(GRIPPERPOSDOWN)
				{
					gripMotor1->Set(0.5);
					//gripMotor2->Set(-0.5);
					
				}
				else
				{
					gripMotor1->Set(0);
					//gripMotor2->Set(0);
				}
			}
			//if(!(count%5)) printf("Grip volts: %f \n", gripPot->GetVoltage());
			//if(!(count%5)) printf("Grip 1 voltage: %f \n", gripMotor1->Get());
			if(GRIPPEROPEN && !popStage)
			{
#if !(INNERGRIP)
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#else
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#endif
			}
			else if(!popStage)
			{
#if !(INNERGRIP)
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#else
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#endif
			}
			if(GRIPPERPOP && !popStage && goalGripSP == 0 && !(GRIPPEROPEN && GRIPPERCLOSE)) popStage = 1;
			if(popStage) popTime = GetClock();
			if(popStage == 1)
			{
				//popCount = count;
				popStart = GetClock();
				popStage++;
				//printf("POP START POP START POP START \n");
			}
			if(popStage == 2)
			{
#if !(INNERGRIP)
				gripOpen1->Set(true);
				gripOpen2->Set(false);
#else
				gripOpen1->Set(false);
				gripOpen2->Set(true);
#endif
				popStage++;
				//printf("GRIP OPEN GRIP OPEN GRIP OPEN \n");
			}
			if(popStage == 3 && popTime - popStart > 0.0) //used to be 0.15
			{
				gripPop1->Set(true);
				gripPop2->Set(false);
				popStage++;
				//printf("POP OUT POP OUT POP OUT \n");
			}
			if(popStage == 4 && popTime - popStart > .75) //time was 0.9
			{
				gripPop1->Set(false);
				gripPop2->Set(true);
				popStage++;
				//printf("POP IN POP IN POP IN \n");
			}
			if(popStage == 5 && popTime - popStart > 0.9)	popStage = 0; //time was 1.05
			
			if(MINIBOTSWITCH && !(MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1))) miniDep->Set(miniDep->kReverse);
			else if(MINIBOTSWITCH && MODESWITCH && stickR.GetRawButton(1) && stickL.GetRawButton(1)) miniDep->Set(miniDep->kOn);
#endif			
			if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kOff);
			/*
			if(stickR.GetRawButton(1)) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kForward);
			*/
			Wait(0.02);				// wait for a motor update time
		}
	}
	RobotDemo()
	{
#if YEAR_2013
		drive = new RobotDrive(left_drive_motor_A_PWM, left_drive_motor_B_PWM,
				right_drive_motor_A_PWM, right_drive_motor_B_PWM);
		drive->SetExpiration(15);
		drive->SetSafetyEnabled(false); 
#endif
	
		drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
		drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
		drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
		drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
		//Joystick
		//ds = new DriverStation();
		drive_stick_sec = new Joystick(right_stick);
		drive_stick_prim = new Joystick(left_stick);
		operator_stick = new Joystick(operator_joystick);
		//Motors
#if JAGUAR_SWITCH
		shooter_motor_front = new Jaguar(shooter_front_motor);
		shooter_motor_back = new Jaguar(shooter_back_motor);
#else
		shooter_motor_front = new Talon(shooter_front_motor);
		shooter_motor_back = new Talon(shooter_back_motor);
#endif
#if DUMB_DRIVE_CODE
		left_drive_motor_A = new Victor(left_drive_motor_A_PWM);
		left_drive_motor_B = new Victor(left_drive_motor_B_PWM);
		right_drive_motor_A = new Victor(right_drive_motor_A_PWM);
		right_drive_motor_B = new Victor(right_drive_motor_B_PWM);
#endif
#if YEAR_2013
		climbing_motor = new Victor(climbing_motor_PWM);
#endif
		//limit switches
		top_claw_limit_switch = new DigitalInput(top_claw_limit_switch_port);
		bottom_claw_limit_switch = new DigitalInput(
				bottom_claw_limit_switch_port);

		//Encoders
		front_shooter_encoder = new Encoder(shooter_motor_front_encoder_A_port,
				shooter_motor_front_encoder_B_port, false);
		back_shooter_encoder = new Encoder(shooter_motor_back_encoder_A_port,
				shooter_motor_back_encoder_B_port, false);

		//solenoids
		shooter_angle_1 = new Solenoid(SHOOTER_ANGLE_SOLENOID_1);
		shooter_angle_2 = new Solenoid(SHOOTER_ANGLE_SOLENOID_2);
		shooter_fire_piston_A = new Solenoid(shooter_fire_piston_solenoid_A);
		shooter_fire_piston_B = new Solenoid(shooter_fire_piston_solenoid_B);
		//Timers
		shooter_piston_timer = new Timer();
		VC_timer = new Timer();
		loop_time_measure_timer = new Timer();
		shooter_reset = new Timer();
		pid_code_timer = new Timer();
		autonomous_timer = new Timer();
		error_timer = new Timer();
		retraction_timer = new Timer();
		override_timer = new Timer();
		stabilizing_timer = new Timer();
		shooter_stop_timer = new Timer();
		//Compressor
		compressor1 = new Compressor(PRESSURE_SWITCH, compressor_enable);
#if CAMERA
		//Camera
		camera = &(AxisCamera::GetInstance(AxisCameraIpAddr));
		camera->WriteResolution(AxisCamera::kResolution_640x480);
		AxisCamera::GetInstance();
#endif

		//Function starter
		compressor1 ->Start();
		//float RPS;
		//PIDController pid1 (0.1 , 0.001 ,0.0 , &RPS , test_motor );
		loop_time_measure_timer ->Start();
		VC_timer ->Start();
		//Variable Initialization
		arcadedrive = true;
		test_encoder_value = 0;
		total_test_encoder_value = 0;
		old_RPS = 0;
		new_RPS = 0;
		second_count = 0;
		set_speed = 0.5;
		cycle_counter = 0;
		//test_motor ->Set(set_speed);
		// float RPS;
		//int old_encoder_value;
		//double old_timer;
		speed2 = 0.0;
		test_speed = 0.3;
		button = false;
		pickup_on = false;
		switch_on = false;
		kicker_piston_on = false;
		kicker_button_on = false;
		average_counter = 0;
		counter = 1;
		number = 1;
		additive_error = 0;
		prev_error_front = 0;
		prev_desired_RPS = 0;
		ROC = 0;
		claw_ = false;
		claw_go_down = false;
		constant_ = false;
		constant_desired_RPS = false;
		divided = 0;
		//RPS_encoder_1 = new Encoder(9, 10);// for 2012 robot
		shooter_motor_back_RPS = shooter_motor_back->Get();
		//RPS_encoder_1 -> SetDistancePerPulse(1 / 250);//TODO figure out how this works
		first_press = true;
		test_RPS = false;
		desired_RPS_control = 0.0;
		slow_control = 0;
		//RPS_encoder_1 ->Start();
		pid_code_timer ->Start();
		front_shooter_encoder->Start();
		back_shooter_encoder->Start();
		autonomous_timer ->Start();
		error_timer ->Start();
		retraction_timer->Start();
		stabilizing_timer->Start();
		override_timer->Start();
	}
Exemple #20
0
	/**
	 * Drive left & right motors for 2 seconds then stop
	 */ 
	void Autonomous()
	{
		init();
		lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Entered Autonomous");
		driveTrain.SetSafetyEnabled(false);
		bool checkBox1 = SmartDashboard::GetBoolean("Checkbox 1");
		m_FromAutonomous = true;
		//float rightDriveSpeed = -1.0;
		//float leftDriveSpeed = -1.0;
		//int rangeToWallClose = 60;
		//int rangeToWallFar = 120;
		//Initialize encoder.
		//int distanceToShoot = 133;
		//int shootDelay = 0;
		//ballGrabber.elevatorController.SetSetpoint(PHOENIX2014_INITIAL_AUTONOMOUS_ELEVATOR_ANGLE);
		//TODO Remove encoders from code??
		driveDistanceRight.Reset();
		driveDistanceLeft.Reset();
		driveDistanceRight.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_RIGHT);
		driveDistanceLeft.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_LEFT);
		driveDistanceRight.Start();
		driveDistanceLeft.Start();
		//int printDelay = 0;
		float minDistance = 52.0;  // Closer to the wall than this is too close
		float maxDistance = 12.0*11.0; // Once at least this close, within shooting range
		//float closeDistance = maxDistance + 24.0;
		float autoDriveSpeed = 0.55;
		//float autoDriveSlowSpeed = 0.38;
		int time = 0;
		int maxDriveLoop = 4*200; // 4 seconds @200 times/sec

		bool shootingBall = false;
		bool wantFirstShot = true;

		if(checkBox1 == false){
			int printDelay = 0;
			//Ultrasonic Autonomous
			//bool isInRange = false;
			while(IsAutonomous() && IsEnabled())
			{
				float currentDistance = frontUltrasonic.GetAverageDistance();
				// Transition isInRange from false to true once
				if((minDistance < currentDistance) && (currentDistance < maxDistance)){
					//isInRange = true;
				}
				time++;
				bool motorDriveTimeExceeded = time > maxDriveLoop;
				bool motorMinMet = time > m_MinDriveLoop;
				if(/*isInRange ||*/ motorMinMet){
					driveTrain.TankDrive(0.0,0.0);
					if((ballGrabber.elevatorAngleSensor.GetVoltage() > PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE - 0.1) &&
							(ballGrabber.elevatorAngleSensor.GetVoltage() < PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE + 0.1)){
						//Enough to cover break release and start winding again.
						
						shootingBall = shooter.OperateShooter(wantFirstShot);
					}
					if(shootingBall == true){
						wantFirstShot = false;
					}
				}
				else if(motorDriveTimeExceeded){
					driveTrain.TankDrive(0.0,0.0);
				}
				else{
					//if((currentDistance < closeDistance) && motorMinMet){
					//	autoDriveSpeed = autoDriveSlowSpeed;
					//}
					driveTrain.TankDrive(-0.97 * autoDriveSpeed, -1.0 * autoDriveSpeed);//the DriveTrain is BACKWARD
				}
				ballGrabber.RunElevatorAutonomous(PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE);
				printDelay = printDelay++;
				if(printDelay >= 200){
					lcd->Clear();
					lcd->PrintfLine(DriverStationLCD::kUser_Line1, "In Autonomous");
					shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line2, lcd);
					shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
					lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Ulra=%f", frontUltrasonic.GetAverageDistance());
					//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "CEV=%f", ballGrabber.elevatorAngleSensor.GetVoltage());
					//lcd->PrintfLine(DriverStationLCD::kUser_Line6, "DEV=%f", ballGrabber.m_desiredElevatorVoltage);
					lcd->UpdateLCD();
					printDelay = 0;
				}
				Wait(0.005);
			}
			lcd->Clear();
			lcd->UpdateLCD();
			lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Exiting Autonomous");
		} else {
			//Timer Autonomous
			driveTrain.TankDrive(-0.5,-0.5);
			ballGrabber.DriveElevatorTestMode(-1.0);
			lcd->Clear();
			lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Skip Auto");
			lcd->PrintfLine(DriverStationLCD::kUser_Line2, "CheckBox Checked");
			lcd->UpdateLCD();
			Wait(2.0);
			bool shooting = true;
			driveTrain.TankDrive(0.0,0.0);
			int counter = 0;
			while(counter < 600){
				shooter.OperateShooter(shooting);
				Wait(0.005);
			}
		}
	}