logging code might have been merged with master

Robot2016/src/Robot.cpp

@@ -8,14 +8,20 @@
 #include <sys/time.h>
 #include <vector>
 #include <cmath>
+#include <math.h>
 
 #ifndef BUTTON_LAYOUT
 #define BUTTON_LAYOUT
 
 #define TRIGGER 	1  	// Trigger button number
 #define THUMB 		2	// Thumb button number
-#define RAMP_RAISE  3   // Button 3 for Raising Ramp
+#define RAMP_RAISE  5   // Button 3 for Raising Ramp
-#define RAMP_LOWER  4	// Button 4 to lower ramp.
+#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
+								// affecting the robot.  Use this for cleaning up drive train and shooter.
+								// Also used for detecting changes in an axis' value.
 
 #endif  // BUTTON_LAYOUT
 
@@ -25,14 +31,15 @@
 
 class Robot: public IterativeRobot
 {
-	Talon left_drive, right_drive;
+	TalonSRX left_drive, right_drive;
 	CANTalon shooter1, shooter2,
-			ramp;
+			ramp,
+			arms;
 	RobotDrive drive;
 	Shooter shooter;
 	Joystick driver_stick, operator_stick;
 	//DriverStation DriverStation::GetInstance();
-	float power;
+	//float power;
 public:
 	Robot():
 		left_drive(0),			// Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter
@@ -40,13 +47,13 @@ public:
 		shooter1(11),  			// shooter drive 1
 		shooter2(10),   		// shooter drive 2
 		ramp(12),
+		arms(13),
 		drive(&left_drive, &right_drive),
 		shooter(				// initialize Shooter object.
 				&shooter1, &shooter2, &ramp),
 		driver_stick(0), 				// right stick (operator)
-		operator_stick(1)				// left stick (driver)
+		operator_stick(1)			// left stick (driver)
 	{
-		//DriverStation::GetInstance().GetInstance();
 
 	}
 
@@ -54,6 +61,11 @@ 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;
+	bool arming;
+	float shooter_power;
 
 	LiveWindow *lw = LiveWindow::GetInstance();
 	SendableChooser *chooser;
@@ -81,12 +93,18 @@ private:
 		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
 		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
 		SmartDashboard::PutData("Auto Modes", chooser);
-		ramp.Enable();
 		shooter1.Enable();
 		shooter2.Enable();
 		left_drive.SetInverted(true);
 		right_drive.SetInverted(true);
+		//ramp.SetInverted(true);
 		inverting = false;
+		pickupRunning = false;
+		ramping = false;
+		shooting = false;
+		unjamming = false;
+		arming = false;
+		shooter_power = 0;
 
 	}
 
@@ -102,9 +120,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;
@@ -128,44 +143,89 @@ private:
 
 	void TeleopInit()
 	{
-		shooter.CalibrateRamp();
+
-		shooter.SetRamp(Shoot); // start in the full up position!
-		power = 0;
 	}
 
 	void TeleopPeriodic()
 	{
 		LogCSVData();
-		std::cout << "Ramp position: "<< ramp.GetEncPosition() << std::endl;
+		std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
-		drive.ArcadeDrive(&driver_stick, true);
+		std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
+		drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75);
 
+		//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.";
+			//ramp.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.";
+			//ramp.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(TRIGGER))
+		{
+			shooter.ShootLow();
+			unjamming = true;
+		}
+		else if(unjamming && !operator_stick.GetRawButton(UNJAM) && !operator_stick.GetRawButton(TRIGGER))
+		{
+			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 iteration 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 iteration 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;
@@ -174,14 +234,59 @@ private:
 		{
 			inverting = false;
 		}
-
+		
-		if(((1.0 - operator_stick.GetThrottle()) / 2.0) > power + 0.005||((1.0 - operator_stick.GetThrottle()) / 2.0) < power -0.005)
+		
+		/*
+		 * 	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))
 		{
-			power = (1.0 - operator_stick.GetThrottle()) / 2.0;
+			shooting = true;
-			shooter.SetPower(power);
+			shooter.Shoot();
+		}
+		else if(shooting  && !operator_stick.GetRawButton(TRIGGER))
+		{
+			shooting = false;
+			shooter.StopShooter();
+		}*/
+		
+		if(!arming && driver_stick.GetRawButton(RAMP_RAISE))
+		{
+			arming = true;
+			arms.Set(1);
+		}
+		else if(!arming && driver_stick.GetRawButton(RAMP_LOWER))
+		{
+			arming = true;
+			arms.Set(-1);
+		}
+		else if(arming && !driver_stick.GetRawButton(RAMP_RAISE) && !driver_stick.GetRawButton(RAMP_LOWER))
+		{
+			arming = false;
+			arms.Set(0);
+		}
+
+        // 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

@@ -8,89 +8,156 @@
 #ifndef SRC_SHOOTER_H_
 #define SRC_SHOOTER_H_
 
-#define PICKUP_POWER 0.8
+#define PICKUP_POWER 0.75
-#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) {
+	enum ShooterState
+	{
+		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;
-		rampState = Uncalibrated;
+		ready = true;
+		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() 
-	}
+	{
-
-	virtual ~Shooter() {
 		delete launcher;
 		delete pickup;
 		delete ramp;
 	}
-
+	
-	void PickUp(bool state = true) {
+	void StopShooter()
-		pickup->Set((float) (state * PICKUP_POWER));
+	{
-		std::cout << "picking up!\n";
+		ready = true;
+		ramp->Set(0);
+		launcher->Set(0);
+		pickup->Set(0);
 	}
 
-	void SetRamp(RampState state) {
+	void LowerRamp()
-		// TODO:
+	{
-		// Move the Ramp to the set position.
+		ramp->Set(-0.5);
+		if(ramp->Limits::kReverseLimit){
+			SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be
+		} else {
+			SmartDashboard::PutNumber("ramp", 1);
+		}
+	}
+
+	void RaiseRamp()
+	{
+		ramp->Set(0.5);
+		if(ramp->Limits::kForwardLimit){
+			SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be
+		} else {
+			SmartDashboard::PutNumber("ramp", 1);
+		}
+	}
+
+	void StopRamp()
+	{
+		ramp->Set(0);
+	}
+	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())
+				state = SPINNINGUP;
+				ramp->Set(-1);
+				launcher->Set(PICKUP_POWER);
+				pickup->Set(PICKUP_POWER);
+				shotClock.Reset();
+				shotClock.Start();
+				break;
+			}
+			case SPINNINGUP:
+			{
+				if (shotClock.Get() > SPINUP_TIME)
 				{
-					ramp->Set(1);
+					state = LAUNCH;
+					shotClock.Reset();
+					shotClock.Start();
 				} else
 				{
-					ramp->Set(0);
+					std::cout << "*Goku noises*\n";
 				}
+				break;
 			}
-			case Half:
+			case LAUNCH:
 			{
-				//yeah, put something here when you get the encoder working.
+				ramp->Set(1);
-				std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n";
+				state = LAUNCHING;
+				break;
 			}
-			case Down:
+			case LAUNCHING:
 			{
-				//see half.
+				if (shotClock.Get() > LAUNCH_TIME)
-				std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n";
+				{
+
+					state = RESETTING;
+				}
+				break;
+			}
+			case RESETTING:
+			{
+				ramp->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 * -0.75));
+		//ramp->Set(-1.0*PICKUP_POWER);
+		//std::cout << "picking up!\n";
 	}
 
 	/**
@@ -98,24 +165,31 @@ public:
 	 */
 	void Unjam()
 	{
-		pickup->Set(-1 * PICKUP_POWER);
+		pickup->Set(PICKUP_POWER * -0.75);
 	}
 
-	void SetPower(float power) {
+	void ShootLow()
+	{
+		pickup->Set(-1);
+	}
+
+	void SetPower(float power) 
+	{
 		pickup->Set(power);
 		launcher->Set(power);
-		std::cout << "setting shooter power" << std::endl;
+		//std::cout << "setting shooter power" << std::endl;
 	}
 private:
 
-	RobotDrive *shooterDrive;
+	//RobotDrive *shooterDrive;
 	CANTalon *launcher;
 	CANTalon *pickup;
 	CANTalon *ramp;
-
+	ShooterState state;
-	RampState rampState;
+	
-	RampState targetState;
+	Timer shotClock;  
-	RampState previousState;
+	bool ready;
+	int fake_position;
 };
 
 #endif /* SRC_SHOOTER_H_ */