Team 4213- MetalCow robot code
Written by Thad Hughes in May-July 2012 (Competition code wasn't subclassed)
--------------------------------

A '!' Before an item indicates that it is not yet implemented fully onto the robot

This code assumes the following connections:
Driver Station:
	USB 1: AIRFLO controller for drive
	USB 2: AIRFLO controller for actions
!	USB 3: Logitech Extreme 3D Pro joystick for drive
!	USB 4: Logitech Attack 3 joystick (from 2012 KOP) for actions

Robot:
	General:
		Uses a mecanum drive powered by 4 CIM motors and jaguars
		Has a lift system driven by the AndyMark gearmotor in the 2012 kit of parts and a jaguar
		Has 2 fisher price motors from the 2012 kit of parts in the front of the feeder controlled by a jaguar
		Has 2 throttle control motors from the 2012 kit of parts on the rear of the feeder controlled by spike relays
		
	Digital Sidecar 1:
		PWM 1: Connected to front left drive motor
		PWM 2: Connected to rear left drive motor
		PWM 3: Connected to front right drive motor
		PWM 4: Connected to rear right drive motor
		PWM 5: Connected to lift motor jaguar
		PWM 6: Connected to front feeder jaguar
!		PWM 7: (5v jumper in) Connected to camera mount x servo
!		PWM 8: (5v jumper in) Connected to camera mount y servo
		Relay 1: Connected to rear feeder spike
		Relay 2: Connected to middle feeder spike
		
!		GPIO 1: Connected to bottom rail switch
!		GPIO 2: Connected to bridge rail switch
!		GPIO 3: Connected to top rail switch

!		Analog 1: Connected to robot gyroscope
!		Analog 2: Connected to rear feeder system IR
!		Analog 3: Connected to middle feeder system IR
!		Analog 4: Connected to front feeder system IR

This code will cause the following behavior:
	Autonomous:
		Deployment:
			Starts feeders
			Waits a quarter second before lifting the feeder
			Waits a second before stopping lift
			Waits another second before stopping feeders
!		Kinect driving:
			When head to left:
				Use left arm up-down to control feeder out-in
				Use right arm up-down to control lift down-up
			When head centered:
				Use left arm up-down to control
				Use right arm up-down to control
			When head to right:
	
	Teleop:
		Button 11 on drive pad toggles single/two pad drive
		Button 10 on drive pad toggles tank drive
		Button 9 on drive pad toggles mecanum/striaght wheels (So nothing is torn up)
		Single Pad:
			Top left bumper is .6 throttle
			Lower left bumper is .2 throttle
			Both bumpers are full throttle
			.4 throttle by default
			
			Left X is robot strafing, left Y is robot forward/backward, right X is robot rotation
			
			Dpad up-down controls lift down-up
			
			Button 1 is first feeders intake
			Button 2/3 is first and second feeders intake
			Button 4 is all feeders intake
			Right triggers shoot out balls
		Two Pads:
			Drive Pad:
				A top bumper is .6 throttle
				A lower bumper is .2 throttle
				Both bumpers are full throttle
				.4 throttle by default
				
				If dpad is not touched, robot is driven from joystick axes either threeaxis or tank style, toggled by button 1
				If dpad is touched, robot is driven from dpad axes for movement and right X for rotation
			Action Pad:
				Left stick up-down controls lift down-up
			
				Button 1 is first feeders intake
				Button 2 or 3 is first and second feeders intake
				Button 4 is all feeders intake
				Right triggers shoot out balls