From 9f6bfd3c101bf8f6d505131aeac6502d45797061 Mon Sep 17 00:00:00 2001 From: Adam Goldsmith Date: Thu, 10 Mar 2016 09:04:45 -0500 Subject: [PATCH] Add gimble control code Untested, but did previously work --- Robot2016/src/Robot.cpp | 36 +++++++++++++++++++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/Robot2016/src/Robot.cpp b/Robot2016/src/Robot.cpp index a5a88c5..6da8c07 100644 --- a/Robot2016/src/Robot.cpp +++ b/Robot2016/src/Robot.cpp @@ -18,6 +18,13 @@ #define RAMP_RAISE 5 // Button 3 for Raising Ramp #define RAMP_LOWER 3 // Button 4 to lower ramp. #define UNJAM 11 +#define GIM_UP 7 +#define GIM_DOWN 9 +#define GIM_ZERO 10 +#define GIM_SHOOT 8 + +#define GIMB_DEFAULT 768 //Default position of the gimble (*1000) +#define GIMB_DELTA 2 //Amount to increment by #define ARMS_SCALE 0.75 @@ -47,6 +54,8 @@ private: Shooter shooter; Joystick driver_stick, operator_stick; Timer auto_clock; + Servo gimbs; + int gimba; // angle of the gimble as a value of from 0 to 1000 // instance variables bool pickupRunning; // don't want to spam the Talon with set messages. Toggle the pickup when a button is pressed or released. @@ -92,6 +101,8 @@ private: SmartDashboard::PutBoolean("Arms Limit F", !arms.IsFwdLimitSwitchClosed()); SmartDashboard::PutBoolean("Arms Limit R", !arms.IsRevLimitSwitchClosed()); + SmartDashboard::PutNumber("angle of camera", gimba); + if (!logA.is_open() && !logB.is_open() && !logC.is_open()) { std::fstream logNumFile; @@ -200,7 +211,9 @@ public: 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) + gimbs(3), + gimba(GIMB_DEFAULT) { } @@ -228,6 +241,9 @@ public: shooter_power = 0; arcade = false; + // Initialize the number so we can get it later + SmartDashboard::PutNumber("shooting angle", GIMB_DEFAULT); + } void AutonomousInit() @@ -277,6 +293,7 @@ public: break; } } + gimbs.Set(gimba/1000.0); } void TeleopInit() @@ -359,6 +376,23 @@ public: shooter.PickUp(false); unjamming = false; } + + /* + * This is for controlling the gimbal. + */ + if(operator_stick.GetRawButton(GIM_UP)) + { + gimba += GIMB_DELTA; + } + else if(operator_stick.GetRawButton(GIM_DOWN)){ + gimba -= GIMB_DELTA; + } + else if(operator_stick.GetRawButton(GIM_SHOOT)) + { + gimba = SmartDashboard::GetNumber("shooting angle", GIMB_DEFAULT); + } + gimbs.Set(gimba/1000.0); + /* * 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