Skip to content

Commit

Permalink
Test auto poop
Browse files Browse the repository at this point in the history
  • Loading branch information
@andrewmcalinden @mray190
andrewmcalinden authored and mray190 committed Apr 18, 2022
1 parent d044007 commit bfa6fb9
Show file tree
Hide file tree
Showing 8 changed files with 282 additions and 14 deletions.
58 changes: 57 additions & 1 deletion Competition/.vscode/settings.json
View file
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
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
117 changes: 108 additions & 9 deletions Competition/src/main/cpp/subsystems/Feeder.cpp
View file
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,30 @@ void Feeder::analyzeDashboard()
table->PutNumber("Average Amps", state.instCurrent);
table->PutBoolean("Spiked: ", state.spiked);
table->PutBoolean("Banner: ", state.bannerTripped);
table->PutBoolean("Queue top", state.ballHistory.front());

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 +167,27 @@ 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 = 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 +224,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 +247,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.pop();
}
}
}

void Feeder::resetDeque() {
state.current_cache.clear();
for (int i = 0; i < FeederConstants::CACHE_SIZE; i++) {
Expand All @@ -204,3 +295,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
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: 9 additions & 0 deletions Competition/src/main/cpp/subsystems/TurretTracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,15 @@ void TurretTracker::assignOutputs() {
// 0.75 = limeligh KP
state.target = (-state.cachedTx * 0.75) + turretPos;

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

// state.target = -1 * robotHeading + state.cachedTurretPos;
// atan2(drivetrain->getKinematics().ToChassisSpeeds().vx.to(<double>()), drivetrain->getPose_m().X());

Expand Down
Loading

0 comments on commit bfa6fb9

Please sign in to comment.