Merge branch 'shooter'

Robot2016/src/Robot.cpp

@@ -8,6 +8,11 @@
 #define THUMB 		2	// Thumb button number
 #define RAMP_RAISE  3   // Button 3 for Raising Ramp
 #define RAMP_LOWER  4	// Button 4 to lower ramp.
+#define UNJAM		11
+
+#define DEADZONE_RADIUS 0.01 	// Deadzone Radius prevents tiny twitches in the joystick's value from
+								// affecting the robot.  Use this for cleaning up drive train and shooter.
+								// Also used for detecting changes in an axis' value.
 
 #endif  // BUTTON_LAYOUT
 
@@ -15,22 +20,20 @@ class Robot: public IterativeRobot
 {
 	TalonSRX left_drive, right_drive;
 	CANTalon shooter1, shooter2,
-			ramp;
+			launch_spinny;
 	RobotDrive drive;
 	Shooter shooter;
 	Joystick driver_stick, operator_stick;
-	Counter ramp_height;
 public:
 	Robot():
-		left_drive(0),			// Left DriveTrain Talons plug into PWM channel 1 with a Y-spliter
+		left_drive(0),			// Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter
 		right_drive(1),			// Right DriveTrain Talons plug  	// left wheel 2
-		shooter1(10),  			// shooter drive 1
+		shooter1(11),  			// shooter drive 1
-		shooter2(11),   		// shooter drive 2
+		shooter2(10),   		// shooter drive 2
-		ramp(12),
+		launch_spinny(12),
 		drive(&left_drive, &right_drive),
-		ramp_height(),
 		shooter(				// initialize Shooter object.
-				&shooter1, &shooter2, &ramp, &ramp_height),
+				&shooter1, &shooter2, &launch_spinny),
 		driver_stick(0), 				// right stick (operator)
 		operator_stick(1)			// left stick (driver)
 	{
@@ -41,6 +44,10 @@ private:
 	// instance variables
 	bool pickupRunning;  // don't want to spam the Talon with set messages.  Toggle the pickup when a button is pressed or released.
 	bool inverting;
+	bool ramping;
+	bool shooting;
+	bool unjamming;
+	float shooter_power;
 
 	LiveWindow *lw = LiveWindow::GetInstance();
 	SendableChooser *chooser;
@@ -54,13 +61,17 @@ private:
 		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
 		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
 		SmartDashboard::PutData("Auto Modes", chooser);
-		ramp_height.SetUpSource(1);
-		ramp.Enable();
 		shooter1.Enable();
 		shooter2.Enable();
 		left_drive.SetInverted(true);
 		right_drive.SetInverted(true);
+		//launch_spinny.SetInverted(true);
 		inverting = false;
+		pickupRunning = false;
+		ramping = false;
+		shooting = false;
+		unjamming = false;
+		shooter_power = 0;
 
 	}
 
@@ -76,9 +87,6 @@ private:
 	 */
 	void AutonomousInit()
 	{
-		shooter.CalibrateRamp();
-		shooter.SetRamp(Shoot);
-
 		autoSelected = *((std::string*)chooser->GetSelected());
 		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
 		std::cout << "Auto selected: " << autoSelected << std::endl;
@@ -101,41 +109,84 @@ private:
 
 	void TeleopInit()
 	{
-		shooter.CalibrateRamp();
+
-		shooter.SetRamp(Shoot); // start in the full up position!
 	}
 
 	void TeleopPeriodic()
 	{
+		//std::cout << "Ramp position: "<< launch_spinny.GetEncPosition() << std::endl;
 		drive.ArcadeDrive(&driver_stick, true);
 
+		//bool rampDoing = false;
 		//  This is shit code for testing.  Replace it with real code.
-		if(operator_stick.GetRawButton(RAMP_RAISE))
+		if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
 		{
-			ramp.Set(1);
+			std::cout << "Raising Ramp.";
+			//launch_spinny.Set(1);
+			shooter.RaiseRamp();
+			ramping =true;
 		}
-		else if(operator_stick.GetRawButton(RAMP_LOWER))
+		else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
 		{
-			ramp.Set(-1);
+			std::cout << "Lowering Ramp.";
+			//launch_spinny.Set(-1);
+			shooter.LowerRamp();
+			ramping = true;
 		}
-		else
+		/*else if(!ramping && operator_stick.GetRawButton(TRIGGER))
 		{
-			ramp.Set(0);
+			shooter.BoostRamp();
+			ramping = true;
+		}*/
+		else if(ramping
+				&& !operator_stick.GetRawButton(RAMP_LOWER)
+				&& !operator_stick.GetRawButton(RAMP_RAISE))
+		{
+			std::cout << "Stopping Ramp.";
+			shooter.StopRamp();
+			ramping = false;
 		}
 
+
+		if(!unjamming && operator_stick.GetRawButton(UNJAM))
+		{
+			unjamming = true;
+			shooter.Unjam();
+		}
+		else if(unjamming && !operator_stick.GetRawButton(UNJAM))
+		{
+			shooter.PickUp(false);
+			unjamming = false;
+		}
+
+
+		/*
+		 * 	Run the Shooter only while the THUMB button is held down on the operator stick.
+		 *	the 'pickupRunning' boolean is there to prevent the shooter from calling PickUp
+		 *	every itteration of the TeleopPeriodic method (once every 10ms!)
+		 *	The pickup is disabled when the thumb button is released, but the code still
+		 *  has 'pickupRunning' as true.
+		 */
 		if(operator_stick.GetRawButton(THUMB) && !pickupRunning)
 		{
 			shooter.PickUp();
 			pickupRunning = true;
 		}
-		else if(pickupRunning)
+		else if(!operator_stick.GetRawButton(THUMB) && pickupRunning)
 		{
 			shooter.PickUp(false);
 			pickupRunning = false;
 		}
 
+		/*
+		 *	The 'inverting' variable is used to make sure that the drive train isn't getting
+		 *	inverted every itteration of the TeleopPeriodic method while the button is held down.
+		 *	This is important because the TeleopPeriodic method executes something like once every 10ms.
+		 *	Thus, this if-else if pair make the button a toggle.
+		 */
 		if(driver_stick.GetRawButton(THUMB) && !inverting)
 		{
+			std::cout << "Inverting Drive Train.";
 			left_drive.SetInverted(!left_drive.GetInverted());
 			right_drive.SetInverted(!right_drive.GetInverted());
 			inverting = true;
@@ -145,8 +196,41 @@ private:
 			inverting = false;
 		}
 		
-		float power = (1.0 - operator_stick.GetThrottle()) / 2.0;
+		
-		shooter.SetPower(power);
+		/*
+		 * 	Unlike the previous actions, this method does need to be called every itteration of
+		 *	TeleopPeriodic.  This is because the logic running this operation needs to be checked
+		 *	Every time the method is called.  This cannot be a loop in the Shoot method because
+		 *  that would lock the robot every time the trigger is hit.
+		 */
+		if(operator_stick.GetRawButton(TRIGGER))
+		{
+			shooting = true;
+			shooter.Shoot();
+		}
+		else if(shooting  && !operator_stick.GetRawButton(TRIGGER))
+		{
+			shooting = false;
+			shooter.StopShooter();
+		}
+		
+        // This code will become obsolete after the Shooter logic is complete.
+		float opThrottle = SaneThrottle(operator_stick.GetThrottle());
+		
+		if(!pickupRunning && ( opThrottle > shooter_power + DEADZONE_RADIUS
+				||	opThrottle < shooter_power - DEADZONE_RADIUS))
+		{
+			shooter.SetPower(opThrottle);
+		}
+	}
+
+	/**
+	 *	Takes the gross raw throttle input from joystick and returns a 
+	 *	value between 0.0-1.0 (no negative values)
+	 */
+	float SaneThrottle(float rawThrottle)
+	{
+		return ((1.0 - rawThrottle) / 2.0);
 	}
 
 	void TestPeriodic()

Robot2016/src/Shooter.h

@@ -9,77 +9,151 @@
 #define SRC_SHOOTER_H_
 
 #define PICKUP_POWER 0.5
-#define RAMP_LOWER_DURATION 2 //Rotations.
+#define LAUNCH_POWER 1
+#define SPINUP_TIME 1.5 // seconds.
+#define LAUNCH_TIME 0.5
 
-
+class Shooter 
-/**
+{
- * You can use the values assigned to each of these values as the number
- * of rotations to run the motor down from the "Shoot" position.
- *
- * This might not be the best way to do it, and also requires that we
- * figure out how to read the Hall Effect signal from the motor.
- */
-enum RampState {
-	Shoot = 0, // 0 rotations
-	Half = (RAMP_LOWER_DURATION / 2),  // 1 rotation?
-	Down = RAMP_LOWER_DURATION,   // 2 rotations?
-	Uncalibrated = -1,
-	Transitioning = -2
-};
-
-class Shooter {
 public:
+
 	/**
-	 * Shooter talons and ramp talon.
+	 * Shooter talons and launch-spinny talon.
 	 * s2 is also for the pickup-mechanism and can be controlled independently.
 	 *
 	 */
-	Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r, Counter *h) {
+	enum ShooterState
-		shooterDrive = new RobotDrive(s1, s2);
+	{
+		READY,
+		ON_FIRE,
+		SPINNINGUP,
+		LAUNCH,
+		LAUNCHING,
+		RESETTING
+	};
+
+	Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r)
+	{
+	//	shooterDrive = new RobotDrive(s1, s2);
+		launcher = s1;
 		pickup = s2;
-		ramp = r;
+		launch_spinny = r;
-		rampState = Uncalibrated;
+		ready = true;
-		rampHeight = h;
+		state = READY;
 	}
 
 	/**
 	 * Call this method on TeleopInit so that the ramp is properly
 	 * set at the beginning of the match.
 	 */
-	RampState CalibrateRamp() {
-		// TODO:
-		// Raise ramp until limit switch is triggered,
-		// then lower the ramp to its lower limit.
 
-		return Down;
+	virtual ~Shooter() 
-	}
+	{
-
+		delete launcher;
-	virtual ~Shooter() {
-		delete shooterDrive;
 		delete pickup;
-		delete ramp;
+		delete launch_spinny;
-		delete rampHeight;
 	}
 	
-	void PickUp(bool state = true) {
+	void StopShooter()
-		pickup->Set((float) (state * PICKUP_POWER));
+	{
+		ready = true;
+		launch_spinny->Set(0);
+		launcher->Set(0);
+		pickup->Set(0);
 	}
 
-	void SetRamp(RampState state) {
+	void LowerRamp()
-		// TODO:
+	{
-		// Move the Ramp to the set position.
+		launch_spinny->Set(-1);
+	}
+
+	void RaiseRamp()
+	{
+		launch_spinny->Set(1);
+	}
+
+	void StopRamp()
+	{
+		launch_spinny->Set(0);
+	}
+
+	void BoostRamp()
+	{
+		launch_spinny->Set(1);
+	}
+	
+	void Shoot()
+	{
+	// TODO: Shooter Logic should go as follows:
+	
+	/*
+		Assuming a ball is held in the shooter.  When the trigger is pulled,
+		the launch-spinny should be STOPPED.  Start a Timer object counting
+		When... too hard to write.. I emailed you a flow chart.
+	 */
 		switch (state)
 		{
-		case Shoot:
+			case READY:
-			if (ramp->GetForwardLimitOK())
 			{
-				ramp->Set(1);
+				state = SPINNINGUP;
+				launch_spinny->Set(-1);
+				launcher->Set(LAUNCH_POWER);
+				pickup->Set(LAUNCH_POWER);
+				shotClock.Reset();
+				shotClock.Start();
+				break;
+			}
+			case SPINNINGUP:
+			{
+				if (shotClock.Get() > SPINUP_TIME)
+				{
+					state = LAUNCH;
+					shotClock.Reset();
+					shotClock.Start();
 				} else
 				{
-				ramp->Set(0);
+					std::cout << "*Goku noises*\n";
-				rampHeight->Reset();
+				}
+				break;
+			}
+			case LAUNCH:
+			{
+				launch_spinny->Set(LAUNCH_POWER);
+				state = LAUNCHING;
+				break;
+			}
+			case LAUNCHING:
+			{
+				if (shotClock.Get() > LAUNCH_TIME)
+				{
+
+					state = RESETTING;
+				}
+				break;
+			}
+			case RESETTING:
+			{
+				launch_spinny->Set(0);
+				launcher->Set(0);
+				pickup->Set(0);
+				state = READY;
+				break;
+			}
+			case ON_FIRE:
+			{
+				std::cout << "Something is wrong with the launch sequence.\n";
+				break;
 			}
 		}
+		
+	}
+	
+	void PickUp(bool state = true) 
+	{
+		pickup->Set((float) (state * PICKUP_POWER));
+		launcher->Set((float) (state * PICKUP_POWER * -1));
+		//launch_spinny->Set(-1.0*PICKUP_POWER);
+		//std::cout << "picking up!\n";
 	}
 
 	/**
@@ -90,19 +164,22 @@ public:
 		pickup->Set(-1 * PICKUP_POWER);
 	}
 
-	void SetPower(float power) {
+	void SetPower(float power) 
-		shooterDrive->TankDrive(power, -power, false);
+	{
+		pickup->Set(power);
+		launcher->Set(power);
+		//std::cout << "setting shooter power" << std::endl;
 	}
 private:
 
-	RobotDrive *shooterDrive;
+	//RobotDrive *shooterDrive;
+	CANTalon *launcher;
 	CANTalon *pickup;
-	CANTalon *ramp;
+	CANTalon *launch_spinny;
-	Counter *rampHeight;
+	ShooterState state;
 	
-	RampState rampState;
+	Timer shotClock;  
-	RampState targetState;
+	bool ready;
-	RampState previousState;
 };
 
 #endif /* SRC_SHOOTER_H_ */