Merge branch 'arms'

# Conflicts:
#	Robot2016/src/Robot.cpp
#	Robot2016/src/Shooter.h
This commit is contained in:
dkbug 2016-02-20 16:56:51 -05:00
commit e92e036a5c
2 changed files with 59 additions and 16 deletions

View File

@ -21,7 +21,8 @@ class Robot: public IterativeRobot
{ {
TalonSRX left_drive, right_drive; TalonSRX left_drive, right_drive;
CANTalon shooter1, shooter2, CANTalon shooter1, shooter2,
launch_spinny; ramp,
arms;
RobotDrive drive; RobotDrive drive;
Shooter shooter; Shooter shooter;
Joystick driver_stick, operator_stick; Joystick driver_stick, operator_stick;
@ -31,10 +32,11 @@ public:
right_drive(1), // Right DriveTrain Talons plug // left wheel 2 right_drive(1), // Right DriveTrain Talons plug // left wheel 2
shooter1(11), // shooter drive 1 shooter1(11), // shooter drive 1
shooter2(10), // shooter drive 2 shooter2(10), // shooter drive 2
launch_spinny(12), ramp(12),
arms(13),
drive(&left_drive, &right_drive), drive(&left_drive, &right_drive),
shooter( // initialize Shooter object. shooter( // initialize Shooter object.
&shooter1, &shooter2, &launch_spinny), &shooter1, &shooter2, &ramp),
driver_stick(0), // right stick (operator) driver_stick(0), // right stick (operator)
operator_stick(1) // left stick (driver) operator_stick(1) // left stick (driver)
{ {
@ -48,6 +50,7 @@ private:
bool ramping; bool ramping;
bool shooting; bool shooting;
bool unjamming; bool unjamming;
bool arming;
float shooter_power; float shooter_power;
LiveWindow *lw = LiveWindow::GetInstance(); LiveWindow *lw = LiveWindow::GetInstance();
@ -66,12 +69,13 @@ private:
shooter2.Enable(); shooter2.Enable();
//left_drive.SetInverted(true); //left_drive.SetInverted(true);
//right_drive.SetInverted(true); //right_drive.SetInverted(true);
//launch_spinny.SetInverted(true); //ramp.SetInverted(true);
inverting = false; inverting = false;
pickupRunning = false; pickupRunning = false;
ramping = false; ramping = false;
shooting = false; shooting = false;
unjamming = false; unjamming = false;
arming = false;
shooter_power = 0; shooter_power = 0;
} }
@ -111,21 +115,23 @@ private:
void TeleopPeriodic() void TeleopPeriodic()
{ {
//std::cout << "Ramp position: "<< launch_spinny.GetEncPosition() << std::endl; std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX() * 0.75); std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
drive.ArcadeDrive(&driver_stick, true);
//bool rampDoing = false; //bool rampDoing = false;
// This is shit code for testing. Replace it with real code. // This is shit code for testing. Replace it with real code.
if(!ramping && operator_stick.GetRawButton(RAMP_RAISE)) if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
{ {
std::cout << "Raising Ramp."; std::cout << "Raising Ramp.";
//launch_spinny.Set(1); //ramp.Set(1);
shooter.RaiseRamp(); shooter.RaiseRamp();
ramping =true; ramping =true;
} }
else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER)) else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
{ {
std::cout << "Lowering Ramp."; std::cout << "Lowering Ramp.";
//launch_spinny.Set(-1); //ramp.Set(-1);
shooter.LowerRamp(); shooter.LowerRamp();
ramping = true; ramping = true;
} }
@ -159,7 +165,7 @@ private:
/* /*
* Run the Shooter only while the THUMB button is held down on the operator stick. * 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 * the 'pickupRunning' boolean is there to prevent the shooter from calling PickUp
* every itteration of the TeleopPeriodic method (once every 10ms!) * every iteration of the TeleopPeriodic method (once every 10ms!)
* The pickup is disabled when the thumb button is released, but the code still * The pickup is disabled when the thumb button is released, but the code still
* has 'pickupRunning' as true. * has 'pickupRunning' as true.
*/ */
@ -176,7 +182,7 @@ private:
/* /*
* The 'inverting' variable is used to make sure that the drive train isn't getting * 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. * 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. * This is important because the TeleopPeriodic method executes something like once every 10ms.
* Thus, this if-else if pair make the button a toggle. * Thus, this if-else if pair make the button a toggle.
*/ */
@ -210,6 +216,22 @@ private:
shooter.StopShooter(); 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. // This code will become obsolete after the Shooter logic is complete.
float opThrottle = SaneThrottle(operator_stick.GetThrottle()); float opThrottle = SaneThrottle(operator_stick.GetThrottle());

View File

@ -37,7 +37,7 @@ public:
// shooterDrive = new RobotDrive(s1, s2); // shooterDrive = new RobotDrive(s1, s2);
launcher = s1; launcher = s1;
pickup = s2; pickup = s2;
launch_spinny = r; ramp = r;
ready = true; ready = true;
state = READY; state = READY;
} }
@ -51,19 +51,20 @@ public:
{ {
delete launcher; delete launcher;
delete pickup; delete pickup;
delete launch_spinny; delete ramp;
} }
void StopShooter() void StopShooter()
{ {
ready = true; ready = true;
launch_spinny->Set(0); ramp->Set(0);
launcher->Set(0); launcher->Set(0);
pickup->Set(0); pickup->Set(0);
} }
void LowerRamp() void LowerRamp()
{ {
<<<<<<< HEAD
launch_spinny->Set(-0.5); launch_spinny->Set(-0.5);
if(launch_spinny->Limits::kReverseLimit){ if(launch_spinny->Limits::kReverseLimit){
SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be SmartDashboard::PutNumber("ramp", 2); //going to put a circlar dial to show where the ramp could be
@ -71,22 +72,30 @@ public:
SmartDashboard::PutNumber("ramp", 1); SmartDashboard::PutNumber("ramp", 1);
} }
=======
ramp->Set(-.5);
>>>>>>> arms
} }
void RaiseRamp() void RaiseRamp()
{ {
<<<<<<< HEAD
launch_spinny->Set(0.5); launch_spinny->Set(0.5);
if(launch_spinny->Limits::kForwardLimit){ if(launch_spinny->Limits::kForwardLimit){
SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be
} else { } else {
SmartDashboard::PutNumber("ramp", 1); SmartDashboard::PutNumber("ramp", 1);
} }
=======
ramp->Set(0.5);
>>>>>>> arms
} }
void StopRamp() void StopRamp()
{ {
launch_spinny->Set(0); ramp->Set(0);
} }
<<<<<<< HEAD
void BoostRamp() void BoostRamp()
{ {
@ -98,6 +107,8 @@ public:
} }
} }
=======
>>>>>>> arms
void Shoot() void Shoot()
{ {
// TODO: Shooter Logic should go as follows: // TODO: Shooter Logic should go as follows:
@ -112,9 +123,15 @@ public:
case READY: case READY:
{ {
state = SPINNINGUP; state = SPINNINGUP;
<<<<<<< HEAD
launch_spinny->Set(-1); launch_spinny->Set(-1);
launcher->Set(LAUNCH_POWER); launcher->Set(LAUNCH_POWER);
pickup->Set(LAUNCH_POWER); pickup->Set(LAUNCH_POWER);
=======
ramp->Set(-1);
launcher->Set(PICKUP_POWER);
pickup->Set(PICKUP_POWER);
>>>>>>> arms
shotClock.Reset(); shotClock.Reset();
shotClock.Start(); shotClock.Start();
break; break;
@ -134,7 +151,11 @@ public:
} }
case LAUNCH: case LAUNCH:
{ {
<<<<<<< HEAD
launch_spinny->Set(LAUNCH_POWER); launch_spinny->Set(LAUNCH_POWER);
=======
ramp->Set(1);
>>>>>>> arms
state = LAUNCHING; state = LAUNCHING;
break; break;
} }
@ -168,7 +189,7 @@ public:
{ {
pickup->Set((float) (state * PICKUP_POWER)); pickup->Set((float) (state * PICKUP_POWER));
launcher->Set((float) (state * PICKUP_POWER * -1)); launcher->Set((float) (state * PICKUP_POWER * -1));
//launch_spinny->Set(-1.0*PICKUP_POWER); //ramp->Set(-1.0*PICKUP_POWER);
//std::cout << "picking up!\n"; //std::cout << "picking up!\n";
} }
@ -191,7 +212,7 @@ private:
//RobotDrive *shooterDrive; //RobotDrive *shooterDrive;
CANTalon *launcher; CANTalon *launcher;
CANTalon *pickup; CANTalon *pickup;
CANTalon *launch_spinny; CANTalon *ramp;
ShooterState state; ShooterState state;
Timer shotClock; Timer shotClock;