From afad5d4660d6fda620324956da004b1562d33b05 Mon Sep 17 00:00:00 2001
From: Aidan Ferguson <aidanlferguson@gmail.com>
Date: Wed, 10 Feb 2016 13:39:48 -0500
Subject: [PATCH] numerous drive changes because of reasons, plus the starts of
 a counter object that will probably have to be nixed. Not our brightest
 moment.

---
 Robot2016/src/Robot.cpp | 29 +++++++++++++++++++++++++----
 Robot2016/src/Shooter.h | 17 ++++++++++++++++-
 2 files changed, 41 insertions(+), 5 deletions(-)

diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp
index 5e3a67c..2cc4f0a 100644
--- a/Robot2016/src/Robot.cpp
+++ b/Robot2016/src/Robot.cpp
@@ -19,16 +19,18 @@ class Robot: public IterativeRobot
 	RobotDrive drive;
 	Shooter shooter;
 	Joystick driver_stick, operator_stick;
+	Counter ramp_height;
 public:
 	Robot():
-		left_drive(1),			// Left DriveTrain Talons plug into PWM channel 1 with a Y-spliter
-		right_drive(2),			// Right DriveTrain Talons plug  	// left wheel 2
+		left_drive(0),			// Left DriveTrain Talons plug into PWM channel 1 with a Y-spliter
+		right_drive(1),			// Right DriveTrain Talons plug  	// left wheel 2
 		shooter1(10),  			// shooter drive 1
 		shooter2(11),   		// shooter drive 2
 		ramp(12),
 		drive(&left_drive, &right_drive),
+		ramp_height(),
 		shooter(				// initialize Shooter object.
-				&shooter1, &shooter2, &ramp),
+				&shooter1, &shooter2, &ramp, &ramp_height),
 		driver_stick(0), 				// right stick (operator)
 		operator_stick(1)				// left stick (driver)
 	{
@@ -38,6 +40,7 @@ public:
 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;
 
 	LiveWindow *lw = LiveWindow::GetInstance();
 	SendableChooser *chooser;
@@ -51,6 +54,14 @@ 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);
+		inverting = false;
+
 	}
 
 
@@ -96,7 +107,7 @@ private:
 
 	void TeleopPeriodic()
 	{
-		drive.ArcadeDrive(&driver_stick);
+		drive.ArcadeDrive(&driver_stick, true);
 
 		//  This is shit code for testing.  Replace it with real code.
 		if(operator_stick.GetRawButton(RAMP_RAISE))
@@ -123,6 +134,16 @@ private:
 			pickupRunning = false;
 		}
 
+		if(driver_stick.GetRawButton(THUMB) && !inverting)
+		{
+			left_drive.SetInverted(!left_drive.GetInverted());
+			right_drive.SetInverted(!right_drive.GetInverted());
+			inverting = true;
+		}
+		else if(!driver_stick.GetRawButton(THUMB))
+		{
+			inverting = false;
+		}
 
 		float power = (1.0 - operator_stick.GetThrottle()) / 2.0;
 		shooter.SetPower(power);
diff --git a/Robot2016/src/Shooter.h b/Robot2016/src/Shooter.h
index 1fe8b0e..8b7a833 100644
--- a/Robot2016/src/Shooter.h
+++ b/Robot2016/src/Shooter.h
@@ -34,11 +34,12 @@ public:
 	 * s2 is also for the pickup-mechanism and can be controlled independently.
 	 *
 	 */
-	Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r) {
+	Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r, Counter *h) {
 		shooterDrive = new RobotDrive(s1, s2);
 		pickup = s2;
 		ramp = r;
 		rampState = Uncalibrated;
+		rampHeight = h;
 	}
 
 	/**
@@ -57,6 +58,7 @@ public:
 		delete shooterDrive;
 		delete pickup;
 		delete ramp;
+		delete rampHeight;
 	}
 
 	void PickUp(bool state = true) {
@@ -66,6 +68,18 @@ public:
 	void SetRamp(RampState state) {
 		// TODO:
 		// Move the Ramp to the set position.
+		switch (state)
+		{
+		case Shoot:
+			if (ramp->GetForwardLimitOK())
+			{
+				ramp->Set(1);
+			} else
+			{
+				ramp->Set(0);
+				rampHeight->Reset();
+			}
+		}
 	}
 
 	/**
@@ -84,6 +98,7 @@ private:
 	RobotDrive *shooterDrive;
 	CANTalon *pickup;
 	CANTalon *ramp;
+	Counter *rampHeight;
 
 	RampState rampState;
 	RampState targetState;