Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Test auto poop #68

Open
wants to merge 2 commits into
base: dev
Choose a base branch
from
Open

Test auto poop #68

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e961189
Test auto poop
andrewmcalinden Apr 12, 2022
e37a797
empty queue protections
andrewmcalinden Apr 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 57 additions & 1 deletion Competition/.vscode/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,62 @@
"memory_resource": "cpp",
"memory": "cpp",
"*.inc": "cpp",
"cmath": "cpp"
"cmath": "cpp",
"array": "cpp",
"atomic": "cpp",
"hash_map": "cpp",
"hash_set": "cpp",
"cctype": "cpp",
"clocale": "cpp",
"codecvt": "cpp",
"condition_variable": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"deque": "cpp",
"list": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"filesystem": "cpp",
"functional": "cpp",
"iterator": "cpp",
"map": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"set": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"fstream": "cpp",
"future": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cinttypes": "cpp",
"typeinfo": "cpp"
}
}
1 change: 1 addition & 0 deletions Competition/src/main/cpp/RobotContainer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ RobotContainer::RobotContainer() : m_auto(&m_drivetrain, &m_shooter, &m_feeder,
m_shooter.setDrivetrain(&m_drivetrain);
m_turretTracker.setDrivetrain(&m_drivetrain);
m_turretTracker.setShooter(&m_shooter);
m_feeder.setShooter(&m_shooter);
}


Expand Down
125 changes: 116 additions & 9 deletions Competition/src/main/cpp/subsystems/Feeder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,16 +17,25 @@ Feeder::Feeder() : ValorSubsystem(),
operatorController(NULL),
motor_intake(FeederConstants::MOTOR_INTAKE_CAN_ID, "baseCAN"),
motor_stage(FeederConstants::MOTOR_STAGE_CAN_ID, "baseCAN"),
banner(FeederConstants::BANNER_DIO_PORT)
banner(FeederConstants::BANNER_DIO_PORT),
revColorSensor(frc::I2C::kOnboard)

{
frc2::CommandScheduler::GetInstance().RegisterSubsystem(this);
init();
}

void Feeder::setShooter(Shooter *s){
shooter = s;
}

void Feeder::init()
{
initTable("Feeder");
limeTable = nt::NetworkTableInstance::GetDefault().GetTable("limelight");

state.isRedAlliance = fmsTable->GetBoolean("IsRedAlliance", false);

motor_intake.ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10);
motor_intake.SetNeutralMode(ctre::phoenix::motorcontrol::Coast);
motor_intake.SetInverted(false);
Expand All @@ -48,12 +57,18 @@ void Feeder::init()
table->PutNumber("Feeder Forward Speed Shoot", FeederConstants::DEFAULT_FEEDER_SPEED_FORWARD_SHOOT);
table->PutNumber("Intake Spike Current", FeederConstants::JAM_CURRENT);

table->PutNumber("Blue Threshold", FeederConstants::BLUE_THRESHOLD);
table->PutNumber("Red Threshold", FeederConstants::RED_THRESHOLD);

table->PutNumber("Average Amps", 0);
table->PutBoolean("Spiked: ", 0);
table->PutBoolean("Banner: ", 0);


table->PutBoolean("Auto Poop Enabled", false);

state.timing = false;
fmsTable = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
resetState();

}

void Feeder::setControllers(frc::XboxController *controllerO, frc::XboxController *controllerD)
Expand Down Expand Up @@ -82,12 +97,12 @@ void Feeder::assessInputs()

state.operator_leftBumperPressed = operatorController->GetLeftBumper();

if (state.driver_rightTriggerPressed || state.operator_leftBumperPressed) {
state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run
if (state.driver_leftBumperPressed) {
state.feederState = FeederState::FEEDER_REVERSE;
state.spiked = false;
}
else if (state.driver_leftBumperPressed) {
state.feederState = FeederState::FEEDER_REVERSE;
else if (state.driver_rightTriggerPressed || state.operator_leftBumperPressed) {
state.feederState = FeederState::FEEDER_SHOOT; //intake and feeder run
state.spiked = false;
}
else if (state.driver_rightBumperPressed) {
Expand All @@ -114,8 +129,35 @@ void Feeder::analyzeDashboard()
table->PutNumber("Average Amps", state.instCurrent);
table->PutBoolean("Spiked: ", state.spiked);
table->PutBoolean("Banner: ", state.bannerTripped);
if (!(state.ballHistory.empty())){
table->PutBoolean("Queue top", state.ballHistory.front());
}
else{
table->PutBoolean("Queue top", false);
}

if (state.autoPoopEnabled){
if (isRed()){
state.curBall = 1;
}
else if (isBlue()){
state.curBall = 2;
}
else{
state.curBall = 0;
}
updateQueue();
state.prevBall = state.curBall;

state.blueThreshold = table->GetNumber("Blue Threshold", FeederConstants::BLUE_THRESHOLD);
state.redThreshold = table->GetNumber("Red Threshold", FeederConstants::RED_THRESHOLD);

table->PutBoolean("isRed", isRed());
table->PutBoolean("isBlue", isBlue());
}

table->PutNumber("current feeder state", state.feederState);
state.autoPoopEnabled = table->GetBoolean("Auto Poop Enabled", false);
// Calculate instantaneous current
calcCurrent();
}
Expand All @@ -130,8 +172,30 @@ void Feeder::assignOutputs()
motor_stage.Set(0);
}
else if (state.feederState == FeederState::FEEDER_SHOOT) {
motor_intake.Set(state.intakeForwardSpeed);
motor_stage.Set(state.feederForwardSpeedShoot);
if (state.autoPoopEnabled){
bool currentBall = false;
if (!(state.ballHistory.empty())){
currentBall = state.ballHistory.front();
}
if (currentBall){
shooter->state.offsetTurret = true;
state.counter = 16;
}

if (state.counter <= 0){
motor_intake.Set(state.intakeForwardSpeed);
motor_stage.Set(state.feederForwardSpeedShoot);
}
else{
motor_intake.Set(0);
motor_stage.Set(0);
}
state.counter--;
}
else{
motor_intake.Set(state.intakeForwardSpeed);
motor_stage.Set(state.feederForwardSpeedShoot);
}
}
else if (state.feederState == Feeder::FEEDER_REVERSE) {
motor_intake.Set(state.intakeReverseSpeed);
Expand Down Expand Up @@ -168,6 +232,10 @@ void Feeder::assignOutputs()
motor_stage.Set(state.feederForwardSpeedDefault);
}
}
else if (state.feederState == FeederState::FEEDER_FEEDER_ONLY){
motor_intake.Set(0);
motor_stage.Set(state.feederForwardSpeedShoot);
}
else {
motor_intake.Set(0);
motor_stage.Set(0);
Expand All @@ -187,6 +255,37 @@ void Feeder::calcCurrent() {
state.instCurrent = sum / FeederConstants::CACHE_SIZE;
}

void Feeder::updateQueue(){
if (state.curBall != state.prevBall){
if (state.curBall == 1){ //red
if (!state.isRedAlliance){
state.ballHistory.push(true);
}
else{
state.ballHistory.push(false);
}
}
else if (state.curBall == 2){ //blue
if (state.isRedAlliance){
state.ballHistory.push(true);
}
else{
state.ballHistory.push(false);
}
}
}

if (state.feederState == FeederState::FEEDER_SHOOT){
double current = shooter->flywheel_lead.GetSelectedSensorVelocity();
double target = shooter->state.flywheelTarget;

double diff = fabs((current - target) / target);
if (diff > .15 && !(state.ballHistory.empty())){
state.ballHistory.pop();
}
}
}

void Feeder::resetDeque() {
state.current_cache.clear();
for (int i = 0; i < FeederConstants::CACHE_SIZE; i++) {
Expand All @@ -204,3 +303,11 @@ void Feeder::resetState()

resetDeque();
}

bool Feeder::isRed(){
return revColorSensor.GetColor().red > state.redThreshold;
}

bool Feeder::isBlue(){
return revColorSensor.GetColor().blue > state.blueThreshold;
}
53 changes: 53 additions & 0 deletions Competition/src/main/cpp/subsystems/Shooter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@ void Shooter::init()
{
limeTable = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
liftTable = nt::NetworkTableInstance::GetDefault().GetTable("Lift");
fmsTable = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
feederTable = nt::NetworkTableInstance::GetDefault().GetTable("Feeder");
initTable("Shooter");

table->PutBoolean("Zero Turret", false);
Expand Down Expand Up @@ -126,6 +128,30 @@ void Shooter::init()
limelightTrack(true);

setPIDProfile(0);

setupCommands();
}

void Shooter::setupCommands(){
frc2::FunctionalCommand poopShot(
[this]() { //onInit
state.hoodState = HoodState::HOOD_POOP;
state.offsetTurret = true;
//state.flywheelState = FlywheelState::FLYWHEEL_POOP;
},
[this](){
}, //onExecute
[this](bool){ //onEnd
state.hoodState = HoodState::HOOD_TRACK;
state.offsetTurret = false;
//state.flywheelState = FlywheelState::FLYWHEEL_TRACK;
},
[this](){ //isFinished
return state.spiked;
}
);
poopOneBall.AddCommands(frc2::WaitCommand((units::second_t)0.15));
poopOneBall.AddCommands(poopShot);
}

void Shooter::resetState(){
Expand All @@ -145,6 +171,16 @@ void Shooter::resetState(){
state.currentBall = 0;
}

bool Shooter::isOppositeColor(){
if (feederTable->GetBoolean("isRed", false) && !fmsTable->GetBoolean("IsRedAlliance", false)){
return true;
}
if (feederTable->GetBoolean("isBlue", false) && fmsTable->GetBoolean("IsRedAlliance", false)){
return true;
}
return false;
}

void Shooter::resetEncoder(){
turretEncoder.SetPosition(0);
hoodEncoder.SetPosition(0);
Expand Down Expand Up @@ -213,6 +249,10 @@ void Shooter::assessInputs()
state.flywheelState = FlywheelState::FLYWHEEL_TRACK; // Higher speed
}

if (isOppositeColor() && state.autoPoopEnabled && state.tv){
poopOneBall.Schedule();
}

state.trackCorner = false;//state.rightBumper ? true : false;
}

Expand All @@ -238,6 +278,16 @@ void Shooter::analyzeDashboard()
state.flywheelState = FlywheelState::FLYWHEEL_DISABLE;
}

double rpm = state.flywheelTarget * ShooterConstants::falconMaxRPM;
double rp100ms = rpm / 600.0;
double ticsp100ms = rp100ms * ShooterConstants::falconGearRatio * ShooterConstants::ticsPerRev;
if (fabs((flywheel_lead.GetSelectedSensorVelocity() - ticsp100ms) / ticsp100ms) > 0.15){
state.spiked = true;
}
else{
state.spiked = false;
}

// Turret homing and zeroing
if (table->GetBoolean("Zero Turret", false)) {
turret.RestoreFactoryDefaults();
Expand Down Expand Up @@ -306,6 +356,8 @@ void Shooter::analyzeDashboard()
state.powerC_2x = table->GetNumber("Power Y Int 2X", ShooterConstants::cPower_2x);

state.pipeline = limeTable->GetNumber("pipeline", 0);

state.autoPoopEnabled = feederTable->GetBoolean("Auto Poop Enabled", false);
}

//0 is close (1x zoom), 1 is far (2x zoom), 2 is auto (1x zoom)
Expand Down Expand Up @@ -409,6 +461,7 @@ void Shooter::assignOutputs()
double rpm = state.flywheelTarget * ShooterConstants::falconMaxRPM;
double rp100ms = rpm / 600.0;
double ticsp100ms = rp100ms * ShooterConstants::falconGearRatio * ShooterConstants::ticsPerRev;
state.flywheelTarget = ticsp100ms;

table->PutNumber("FlyWheel State", state.flywheelState);
table->PutNumber("FlyWheel Target", ticsp100ms);
Expand Down
9 changes: 7 additions & 2 deletions Competition/src/main/cpp/subsystems/TurretTracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,13 @@ void TurretTracker::assignOutputs() {
// 0.75 = limeligh KP
state.target = (-state.cachedTx * 0.75) + turretPos;

if(shooter-> state.driverBButton){
state.target += 15;
if (shooter->state.offsetTurret){
if (tx > 0){
state.target += 15;
}
else{
state.target -= 15;
}
}

// state.target = -1 * robotHeading + state.cachedTurretPos;
Expand Down
Loading