Esempio n. 1
0
    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();

    }
Esempio n. 2
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");
	}
Esempio n. 3
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();
	}
  void RobotInit ()
  {
  lw = LiveWindow::GetInstance();
  CameraServer::GetInstance()->SetQuality(50);
   //the camera name (ex "cam0") can be found through the roborio web interface
  CameraServer::GetInstance()->StartAutomaticCapture("cam1");
  AutonState = 0;
  ballarm.Reset();
  ballarm.SetMaxPeriod(.01);
  ballarm.SetMinRate(.02);
  ballarm.SetDistancePerPulse(.9);
  gyroOne.Calibrate();
  UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);

  UpdateSmartDashboad(false,
                      false,
                      false,
                      false,
                      false,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0);
  }
Esempio n. 5
0
	Robot()
	{
#if BUILD_VERSION == COMPETITION
		liftMotor = new CANTalon(CHAN_LIFT_MOTOR);
		liftMotor2 = new CANTalon(CHAN_LIFT_MOTOR2);
#else
		liftMotor = new CANJaguar(CHAN_LIFT_MOTOR);
		liftMotor2 = new CANJaguar(CHAN_LIFT_MOTOR2);
#endif
		liftEncoder = new Encoder(CHAN_LIFT_ENCODER_A, CHAN_LIFT_ENCODER_B, false, Encoder::EncodingType::k4X);
		liftEncoder->SetDistancePerPulse(LIFT_ENCODER_DIST_PER_PULSE);
		liftEncoder->SetPIDSourceParameter(liftEncoder->kDistance);
#if BUILD_VERSION == COMPETITION
		liftEncoder->SetReverseDirection(true);
#endif
		controlLift = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor);
		controlLift->SetContinuous(true); //treat input to controller as continuous; true by default
		controlLift->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
		controlLift->Disable(); //do not enable until in holding position mode
		controlLift2 = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor2);
		controlLift2->SetContinuous(true); //treat input to controller as continuous; true by default
		controlLift2->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
		controlLift2->Disable(); //do not enable until in holding position mode
		liftLimitSwitchMin = new DigitalInput(CHAN_LIFT_LOW_LS);
		liftLimitSwitchMax = new DigitalInput(CHAN_LIFT_HIGH_LS);
		joystick = new Joystick(CHAN_JS);
	}
Esempio n. 6
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;
    }
	C1983_Lift(Encoder *liftSensorChannel, Relay *brake)
	{
		m_liftSensor = liftSensorChannel;
		m_brake = brake;
		m_bIsBraking = true;
		
		m_brake->SetDirection(m_brake->kBothDirections);
		m_liftSensor->SetDistancePerPulse((float)LIFTDISTPERPULSE);
	}
Esempio n. 8
0
        ControlledMotor(
        		Robot* r,
				const char* n,
				CANJaguar* m,
				Encoder* e,
				double dpp,
				DigitalInput* l) :
			robot(r), name(n), motor(m), encoder(e),
			distancePerPulse(dpp),lowerLimitSwitch(l),
			motorController()
		{
        	motorController = new PIDController(.1, .001, 0.0, encoder, motor);
        	encoder->SetDistancePerPulse(dpp);
		}
	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();		
	}
Esempio n. 10
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();

	}
Esempio n. 11
0
	void TeleopInit()
	{
// Initialize the encoder
		sampleEncoder = new Encoder(0, 1, false, Encoder::EncodingType::k4X);
		sampleEncoder->SetMaxPeriod(.1);
		sampleEncoder->SetMinRate(10);
		sampleEncoder->SetDistancePerPulse(5);
		sampleEncoder->SetReverseDirection(true);
		sampleEncoder->SetSamplesToAverage(7);

// Initialize the joystick
		joystick = new Joystick(0);

// Initialize the motor
		motor = new Victor(9);

// Initialize the gear tooth counter
		toothTrigger = new AnalogTrigger(3);
		toothTrigger->SetLimitsRaw(250, 3600);
		gearToothCounter = new Counter(toothTrigger);
//		gearToothCounter->SetUpDownCounterMode();
	}
Esempio n. 12
0
	FECLift()
	{
		theTimer = new Timer();
		//initialized the jag
		theJag = new Jaguar(JAGPORT);
		fakeEncoder = new Encoder(6,6,6,7,fakeEncoder->k4X);
		theEncoder = new Encoder(ENCODERSLOTA, ENCODERCHANNELA, ENCODERSLOTB, ENCODERCHANNELB, false, theEncoder->k4X);
#if RATE
		theEncoder->SetPIDSourceParameter(Encoder::kRate);
#else
		theEncoder->SetPIDSourceParameter(Encoder::kDistance);
#endif
		
		theEncoder->SetDistancePerPulse(DISTANCE_PER_PULSE);
		driverStation = DriverStation::GetInstance();
		theLift = new PIDController(P,I,D,theEncoder,theJag);
		GetWatchdog().SetExpiration(.3);
		
		decel = false;
		done = false;
		writing = true;
		closed = false;
		initFile();
	}
Esempio n. 13
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);
			}
		}
	}
Esempio n. 14
0
	void RobotInit()
	{
		frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
		//camera.reset(new AxisCamera("axis-camera.local"));

		table = NetworkTable::GetTable("GRIP/myContoursReport");

		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		for (std::map<std::string, bool (*)()>::const_iterator it = Autonomous::crossFunctions.begin(); it!= Autonomous::crossFunctions.end(); it++) {
			chooser->AddObject(it->first, (void*)&(it->first));
		}
		SmartDashboard::PutData("Auto Modes", chooser);

		posChooser = new SendableChooser();
		posChooser->AddDefault(posDefault, (void*)&posToDegrees[stoi(posDefault)]);
		for (int i = 1; i < 6; i++) {
			posChooser->AddObject(std::to_string(i), (void*)&posToDegrees[i]);
		}
		SmartDashboard::PutData("Position", posChooser);

		shootChooser = new SendableChooser();
		shootChooser->AddDefault(shootDefault, (void*)&shootDefault);
		shootChooser->AddObject("No", (void*)"No");

		SmartDashboard::PutData("Shoot", shootChooser);

		drive = new RobotDrive(2,3,0,1);
		//drive = new RobotDrive(frontLeft, backLeft, frontRight, backRight);

		drive->SetInvertedMotor(RobotDrive::MotorType::kFrontLeftMotor, true);
		drive->SetInvertedMotor(RobotDrive::MotorType::kRearLeftMotor, true);
		drive->SetInvertedMotor(RobotDrive::MotorType::kFrontRightMotor, true);
		drive->SetInvertedMotor(RobotDrive::MotorType::kRearRightMotor, true);
		drive->SetExpiration(0.2);

		drive->SetMaxOutput(0.5);

		driveStick = new Joystick(0);
		shootStick = new Joystick(1);

		launchPiston = new Solenoid(3);
		//tiltPiston = new DoubleSolenoid(0,1);
		defensePiston = new DoubleSolenoid(0,1);

		launch1 = new VictorSP(4);
		launch2 = new VictorSP(5);
		launch1->SetInverted(true);

		winch = new VictorSP(6);
		otherWinch = new Victor(7);

		gyro = new AnalogGyro(1);
		leftEnc = new Encoder(2, 3, false, Encoder::EncodingType::k1X);
		rightEnc = new Encoder(0,1, false, Encoder::EncodingType::k1X);
		//Configure for inches.t551
		leftEnc->SetDistancePerPulse(-0.06);
		rightEnc->SetDistancePerPulse(0.06);

		launcherSensor = new DigitalInput(9);

		Autonomous::init(drive, gyro, leftEnc, rightEnc);

		timer =  new Timer();
		defenseUp = false;
		debounce = false;
		//if (fork() == 0) {
		//            system("/home/lvuser/grip &");
		//}

		launcherDown = false;
	}
	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
		}
	}
	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
	}
Esempio n. 17
0
/**
 * Set the distance per pulse for this encoder.
 * This sets the multiplier used to determine the distance driven based on the count value
 * from the encoder.
 * Do not include the decoding type in this scale.  The library already compensates for the decoding type.
 * Set this value based on the encoder's rated Pulses per Revolution and
 * factor in gearing reductions following the encoder shaft.
 * This distance can be in any units you like, linear or angular.
 * 
 * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
 * 
 * @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 SetEncoderDistancePerPulse(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel, double distancePerPulse)
{
	Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
	if (encoder != NULL)
		encoder->SetDistancePerPulse(distancePerPulse);
}