Merge pull request #48 from bearbotics2358/shooter-speed

Shooter speed
bearbotics2358 · Apr 3, 2024 · 5bc7237 · 5bc7237
2 parents e6feb1c + d5d043a
commit 5bc7237
Show file tree

Hide file tree

Showing 8 changed files with 76 additions and 17 deletions.
diff --git a/src/main/cpp/AmpTrap.cpp b/src/main/cpp/AmpTrap.cpp
@@ -17,7 +17,8 @@ a_BeamBreak(AMP_BEAM_BREAK_PORT) ,
 // m_rotationMotorSignal(rotationMotor.GetPosition()),
 // m_extensionMotorSignal(extensionMotor.GetPosition()),
 
-a_ArmAngle(1)
+a_ArmAngle(1),
+extensionLimitSwitch(7)
 {
     rotationPID.SetTolerance(3.0);
     rotationMotor.SetNeutralMode(1);
@@ -71,7 +72,14 @@ double AmpTrap::GetExtensionPosition(){
 
 }
 void AmpTrap::setPosition(){
-    extensionMotor.SetPosition(units::angle::turn_t{0.0});
+    if(extensionLimitSwitch.limitSwitchPressed()){
+        if(!extensionAlreadyZeroed){
+            extensionMotor.SetPosition(zeroExtension);
+            extensionAlreadyZeroed = true;
+        }
+    } else{
+        extensionAlreadyZeroed = false;
+    }
 }
 bool AmpTrap::moveToPosition(double desiredaAngle){
     rotationPID.SetSetpoint(desiredaAngle);//neeed to change from 0.0

diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
@@ -73,7 +73,12 @@ double Climber::GetClimberPosition(){
 }
 void Climber::setPosition(){
     if(topLimitSwitch.limitSwitchPressed()){
-        climberMotor.SetPosition(units::angle::turn_t{0.0});
+        if(!climberAlreadyZeroed){
+            climberMotor.SetPosition(zeroClimber);
+            climberAlreadyZeroed = true;
+        }
+    } else{
+        climberAlreadyZeroed = false;
     }
 }
 
diff --git a/src/main/cpp/Collector.cpp b/src/main/cpp/Collector.cpp
@@ -29,7 +29,7 @@ void Collector::indexToShoot() {
 }
 void Collector::indexToAmp() {
     std::cout << "Shooter Angle True?" << std::endl;
-    indexerMotor.Set(-.2);
+    indexerMotor.Set(.2);
 }
 void Collector::stopIndexer() {
     indexerMotor.StopMotor();

diff --git a/src/main/cpp/NoteHandler.cpp b/src/main/cpp/NoteHandler.cpp
@@ -119,14 +119,14 @@ bool NoteHandler::armToPose(double angle){
 void NoteHandler::setRotPID(double p, double i, double d){
     a_AmpTrap.setPID(p, i, d);
 }
-void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState, bool toDefaultPositionButtonState, bool shooterButtonState, bool driverShootNote, bool collectorButton, bool hooksUp, bool finishClimb) {
+void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState, bool toDefaultPositionButtonState, bool shooterButtonState, bool driverShootNote, bool collectorButton, bool hooksUp, bool finishClimb, bool ejectCollector, bool ejectFromAmp) {
     frc::SmartDashboard::PutNumber("Current Amp State", currentAmpLoadState);
     switch(currentAmpLoadState){
         case IDLE:
             released = false;
             if (shooterButtonState) {
-                double rpm = 3500;
-                double angle = 40.0;
+                double rpm = 2500;
+                double angle = 42.5;
                 startShooter(rpm, angle);
             } 
             else {
@@ -136,6 +136,9 @@ void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState,
 
             if (collectorButton) {
                 collectNote(-0.4, true);
+            }
+            else if(ejectCollector){
+                dispenseNote();
             } 
             else if (driverShootNote) {
         // give note to shooter
@@ -163,6 +166,12 @@ void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState,
             break;
 
         case LOADING:
+            if(ejectFromAmp){
+                if(armToPose(150.0)){
+                    state_time = misc::gettime_d();
+                    currentAmpLoadState = RESETAMP;
+                }
+            }
             if(!transferButtonState){
                 state_time = misc::gettime_d();
                 currentAmpLoadState = IDLE;
@@ -173,6 +182,12 @@ void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState,
             }
            break;
         case HOLDING:
+            if(ejectFromAmp){
+                if(armToPose(150.0)){
+                    state_time = misc::gettime_d();
+                    currentAmpLoadState = RESETAMP;
+                }
+            }
             stopShooter();
             moveShooterToAngle(0.0);
             if(intoAmpButtonState){
@@ -184,6 +199,12 @@ void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState,
             }
                 break;
         case SCORE:
+            if(ejectFromAmp){
+                if(armToPose(150.0)){
+                    state_time = misc::gettime_d();
+                    currentAmpLoadState = RESETAMP;
+                }
+            }
             armToPose(135.0);
             if(misc::gettime_d() > state_time + 0.5){
                             state_time = misc::gettime_d();
@@ -192,7 +213,12 @@ void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState,
             }
             break;
         case AWAYFROMAMP:
-
+            if(ejectFromAmp){
+                if(armToPose(150.0)){
+                    state_time = misc::gettime_d();
+                    currentAmpLoadState = RESETAMP;
+                }
+            }
             if(toDefaultPositionButtonState){
                 if(a_AmpTrap.moveToPosition(7.5)){
                     state_time = misc::gettime_d();
@@ -215,6 +241,16 @@ void NoteHandler::shootToAmp(bool transferButtonState, bool intoAmpButtonState,
         case CLIMBTRAP:
             climbControl(hooksUp, finishClimb);
             break;
+        case RESETAMP:
+            runArmRoller(-45);
+            if(misc::gettime_d() > 1.0 + state_time){
+                a_AmpTrap.stopRoller();
+                if(armToPose(7.5)){
+                    state_time = misc::gettime_d();
+                    currentAmpLoadState = IDLE;
+                }
+            }
+            break;
     }
 }
 void NoteHandler::climbControl(bool climbButton, bool finishClimbButton){

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
@@ -64,8 +64,6 @@ a_Autonomous(&a_Gyro, &a_SwerveDrive, &a_NoteHandler)
 }
 
 void Robot::RobotInit() {
-    a_NoteHandler.setExtensionPosition();
-    a_NoteHandler.setClimberPosition();
     aprilTagFieldLayout.SetOrigin(frc::Pose3d(units::meter_t(-0.038), units::meter_t(5.55), units::meter_t(1.45), frc::Rotation3d(units::radian_t(0.0), units::radian_t(90.0), units::radian_t(0.0))));
     frc::SmartDashboard::init();
 
@@ -106,9 +104,8 @@ void Robot::RobotPeriodic() {
     //a_LED.Update();
 
     a_NoteHandler.UpdateSensors();
-
-
-
+    a_NoteHandler.setExtensionPosition();
+    a_NoteHandler.setClimberPosition();
 
 
     if(!frc::DriverStation::IsDSAttached()){
@@ -403,12 +400,15 @@ void Robot::TeleopPeriodic() {
     a_NoteHandler.shootToAmp(
         a_DriverXboxController.GetRightTriggerAxis() > .75, //transfer to amp
         a_DriverXboxController.GetAButton(), //shoot into amp
-        a_DriverXboxController.GetLeftBumper(), //bring to default amp
+        a_Gamepad.GetRawButton(4), //bring to default amp
         a_Gamepad.GetRawButton(3), //run shooter
         a_DriverXboxController.GetRightBumper(), //shoot note
         a_Gamepad.GetRawButton(1),//run collector
         a_DriverXboxController.GetBButton(),// hooks up
-        a_DriverXboxController.GetXButton());//finishclimb
+        a_DriverXboxController.GetXButton(),//finishclimb
+        a_Gamepad.GetRawButton(11),// eject from collector
+        a_Gamepad.GetRawButton(5) // eject from amp
+        );
                                             // eject from amp
                                             //eject from collector
 

diff --git a/src/main/include/AmpTrap.h b/src/main/include/AmpTrap.h
@@ -3,6 +3,7 @@
 #include <frc/controller/PIDController.h>
 #include "ArmAngle.h"
 #include "BeamBreak.h"
+#include "LimitSwitch.h"
 
 
 class AmpTrap {
@@ -39,5 +40,10 @@ class AmpTrap {
         ArmAngle a_ArmAngle;
 
         ctre::phoenix6::controls::VelocityVoltage m_request = ctre::phoenix6::controls::VelocityVoltage{0_tps}.WithSlot(0);
+
+        units::angle::turn_t zeroExtension{0.0};
+        bool extensionAlreadyZeroed = false;
+
+        LimitSwitch extensionLimitSwitch;
 
 };
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
@@ -22,5 +22,8 @@ class Climber {
 
         frc::PIDController climberPID;
         LimitSwitch topLimitSwitch; 
+
+        units::angle::turn_t zeroClimber{0.0};
+        bool climberAlreadyZeroed = false;
 
 };
diff --git a/src/main/include/NoteHandler.h b/src/main/include/NoteHandler.h
@@ -15,7 +15,8 @@ enum AmpLoadState { // Encoders
             TOAMP,
             AWAYFROMAMP,
             DONE,
-            CLIMBTRAP
+            CLIMBTRAP,
+            RESETAMP
 };
 enum ClimbingState { // Encoders
             CLIMBIDLE = 0,
@@ -74,7 +75,7 @@ class NoteHandler {
 
         void setRotPID(double p, double i, double d);
 
-        void shootToAmp(bool transferButtonState, bool intoAmpButtonState, bool toDefaultPositionButtonState, bool shooterButtonState, bool driverShootNote, bool collectorButton, bool hooksUp, bool finishClimb);
+        void shootToAmp(bool transferButtonState, bool intoAmpButtonState, bool toDefaultPositionButtonState, bool shooterButtonState, bool driverShootNote, bool collectorButton, bool hooksUp, bool finishClimb, bool ejectCollector, bool ejectFromAmp);
         void climbControl(bool climbButton, bool finishClimbButton);
 
         void feedToAmp(double speed);