Merge branch 'master' of code.dublinschool.org:repos/FRC/FRC2016

Robot2016/src/Robot.cpp

@@ -11,10 +11,15 @@
 #define RAMP_LOWER  3	// Button 4 to lower ramp.
 #define UNJAM		11
 
-#define DEADZONE_RADIUS 0.01 	// Deadzone Radius prevents tiny twitches in the joystick's value from
+#define DEADZONE_RADIUS 0.05 	// 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.
 
+#define TURN_FACTOR 1.5			// Left(x,y)  = y*(1 + TF*x)  :  x < 0
+								// 			  = y 			  :  x >= 0
+								// Right(x,y) = y			  :  x < 0
+								// 			  = y*(1 - TF*x)  :  x >= 0
+
 #endif  // BUTTON_LAYOUT
 
 class Robot: public IterativeRobot
@@ -51,6 +56,7 @@ private:
 	bool shooting;
 	bool unjamming;
 	bool arming;
+	bool arcade;
 	float shooter_power;
 
 	LiveWindow *lw = LiveWindow::GetInstance();
@@ -77,6 +83,7 @@ private:
 		unjamming = false;
 		arming = false;
 		shooter_power = 0;
+		arcade = false;
 
 	}
 
@@ -118,7 +125,31 @@ private:
 		std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
 		std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
 		drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75);
-
+		if(driver_stick.GetRawButton(7))
+		{
+			arcade = true;
+		}
+		if(driver_stick.GetRawButton(8))
+		{
+			arcade = false;
+		}
+		if (arcade)
+		{
+			drive.ArcadeDrive(driver_stick);
+		}
+		else
+		{
+			if (driver_stick.GetRawButton(THUMB))
+			{
+				float left = driver_stick.GetTwist();
+				float right = -driver_stick.GetTwist();
+				drive.TankDrive(left, right);
+			}
+			else
+			{
+				UpdateDrive();
+			}
+		}
 		//bool rampDoing = false;
 		//  This is shit code for testing.  Replace it with real code.
 		if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
@@ -189,21 +220,21 @@ private:
 		 *	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)
+		if(driver_stick.GetRawButton(TRIGGER) && !inverting)
 		{
 			std::cout << "Inverting Drive Train.";
 			left_drive.SetInverted(!left_drive.GetInverted());
 			right_drive.SetInverted(!right_drive.GetInverted());
 			inverting = true;
 		}
-		else if(!driver_stick.GetRawButton(THUMB))
+		else if(!driver_stick.GetRawButton(TRIGGER))
 		{
 			inverting = false;
 		}
 		
 		
 		/*
-		 * 	Unlike the previous actions, this method does need to be called every itteration of
+		 * 	Unlike the previous actions, this method does need to be called every iteration 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.
@@ -257,6 +288,45 @@ private:
 	{
 		lw->Run();
 	}
+
+	void SimpleDrive()
+	{
+		float x = -driver_stick.GetX();
+		float y = -driver_stick.GetY();
+		float left = 0;
+		float right = 0;
+
+		if (x > 0)
+		{
+			right = y;
+			left = (1- x*TURN_FACTOR)*y ;
+		}
+		else
+		{
+			left = y;
+			right = (1+x*TURN_FACTOR)*y;
+		}
+
+		drive.TankDrive(left, right);
+	}
+
+	void UpdateDrive()
+	{
+		float x = -driver_stick.GetX();
+		float y = -driver_stick.GetY();
+		if (x > 0)
+		{
+			float right = y * SaneThrottle(driver_stick.GetThrottle());
+			float left = (1-x)*y * SaneThrottle(driver_stick.GetThrottle());
+			drive.TankDrive(left, right);
+		}
+		else
+		{
+			float left = y * SaneThrottle(driver_stick.GetThrottle());
+			float right = (1+x)*y * SaneThrottle(driver_stick.GetThrottle());
+			drive.TankDrive(left, right);
+		}
+	}
 };
 
 START_ROBOT_CLASS(Robot)