diff --git a/src/Robot.cpp b/src/Robot.cpp index f735af2..6d0e6b1 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -21,7 +21,8 @@ class Robot: public IterativeRobot { TalonSRX left_drive, right_drive; CANTalon shooter1, shooter2, - launch_spinny; + ramp, + arms; RobotDrive drive; Shooter shooter; Joystick driver_stick, operator_stick; @@ -31,10 +32,11 @@ public: right_drive(1), // Right DriveTrain Talons plug // left wheel 2 shooter1(11), // shooter drive 1 shooter2(10), // shooter drive 2 - launch_spinny(12), + ramp(12), + arms(13), drive(&left_drive, &right_drive), shooter( // initialize Shooter object. - &shooter1, &shooter2, &launch_spinny), + &shooter1, &shooter2, &ramp), driver_stick(0), // right stick (operator) operator_stick(1) // left stick (driver) { @@ -48,6 +50,7 @@ private: bool ramping; bool shooting; bool unjamming; + bool arming; float shooter_power; LiveWindow *lw = LiveWindow::GetInstance(); @@ -66,12 +69,13 @@ private: shooter2.Enable(); //left_drive.SetInverted(true); //right_drive.SetInverted(true); - //launch_spinny.SetInverted(true); + //ramp.SetInverted(true); inverting = false; pickupRunning = false; ramping = false; shooting = false; unjamming = false; + arming = false; shooter_power = 0; } @@ -111,21 +115,23 @@ private: void TeleopPeriodic() { - //std::cout << "Ramp position: "<< launch_spinny.GetEncPosition() << std::endl; - drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX() * 0.75); + std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl; + std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl; + drive.ArcadeDrive(&driver_stick, true); + //bool rampDoing = false; // This is shit code for testing. Replace it with real code. if(!ramping && operator_stick.GetRawButton(RAMP_RAISE)) { std::cout << "Raising Ramp."; - //launch_spinny.Set(1); + //ramp.Set(1); shooter.RaiseRamp(); ramping =true; } else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER)) { std::cout << "Lowering Ramp."; - //launch_spinny.Set(-1); + //ramp.Set(-1); shooter.LowerRamp(); ramping = true; } @@ -159,7 +165,7 @@ private: /* * 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 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 * has 'pickupRunning' as true. */ @@ -176,7 +182,7 @@ private: /* * 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. * Thus, this if-else if pair make the button a toggle. */ @@ -210,6 +216,22 @@ private: 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()); diff --git a/src/Shooter.h b/src/Shooter.h index 17561c4..d8bf4bc 100644 --- a/src/Shooter.h +++ b/src/Shooter.h @@ -37,7 +37,7 @@ public: // shooterDrive = new RobotDrive(s1, s2); launcher = s1; pickup = s2; - launch_spinny = r; + ramp = r; ready = true; state = READY; } @@ -51,19 +51,20 @@ public: { delete launcher; delete pickup; - delete launch_spinny; + delete ramp; } void StopShooter() { ready = true; - launch_spinny->Set(0); + ramp->Set(0); launcher->Set(0); pickup->Set(0); } void LowerRamp() { +<<<<<<< HEAD launch_spinny->Set(-0.5); if(launch_spinny->Limits::kReverseLimit){ 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); } +======= + ramp->Set(-.5); +>>>>>>> arms } void RaiseRamp() { +<<<<<<< HEAD launch_spinny->Set(0.5); if(launch_spinny->Limits::kForwardLimit){ SmartDashboard::PutNumber("ramp", 0); //going to put a circlar dial to show where the ramp could be } else { SmartDashboard::PutNumber("ramp", 1); } +======= + ramp->Set(0.5); +>>>>>>> arms } void StopRamp() { - launch_spinny->Set(0); + ramp->Set(0); } +<<<<<<< HEAD void BoostRamp() { @@ -98,6 +107,8 @@ public: } } +======= +>>>>>>> arms void Shoot() { // TODO: Shooter Logic should go as follows: @@ -112,9 +123,15 @@ public: case READY: { state = SPINNINGUP; +<<<<<<< HEAD launch_spinny->Set(-1); launcher->Set(LAUNCH_POWER); pickup->Set(LAUNCH_POWER); +======= + ramp->Set(-1); + launcher->Set(PICKUP_POWER); + pickup->Set(PICKUP_POWER); +>>>>>>> arms shotClock.Reset(); shotClock.Start(); break; @@ -134,7 +151,11 @@ public: } case LAUNCH: { +<<<<<<< HEAD launch_spinny->Set(LAUNCH_POWER); +======= + ramp->Set(1); +>>>>>>> arms state = LAUNCHING; break; } @@ -168,7 +189,7 @@ public: { pickup->Set((float) (state * PICKUP_POWER)); 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"; } @@ -191,7 +212,7 @@ private: //RobotDrive *shooterDrive; CANTalon *launcher; CANTalon *pickup; - CANTalon *launch_spinny; + CANTalon *ramp; ShooterState state; Timer shotClock;