From c384a0f3c880c7915be1b7bbc2fd90a1a7a5bf4b Mon Sep 17 00:00:00 2001 From: Loki Date: Wed, 25 Apr 2018 22:47:33 -0400 Subject: [PATCH 1/3] Initial commit for D-town 2018. Changes from practice day. --- src/Commands/AutoSelector.cpp | 3 --- src/OI.cpp | 1 + src/Robot.cpp | 1 + src/Subsystems/Cube.cpp | 7 ++----- src/Util.h | 4 ++-- 5 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/Commands/AutoSelector.cpp b/src/Commands/AutoSelector.cpp index 647cd22..38e681c 100644 --- a/src/Commands/AutoSelector.cpp +++ b/src/Commands/AutoSelector.cpp @@ -95,7 +95,6 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { // AddSequential(new Pause(0.3));//t // AddParallel(new ElevatorRunToHeight(0.7 , 2.7)); //0.7,2.6 // AddSequential(new PIDDriveStraight(18)); - AddSequential (new CubeIntakeDeploy (true)); AddParallel(new ElevatorRunToHeight(0.7,2.7)); AddSequential(new Pause(0.25)); AddSequential(new PIDDriveStraight(28)); @@ -184,7 +183,6 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { } void AutoSelector::OutsideStraightScale() { - AddSequential (new CubeIntakeDeploy (true)); AddSequential(new PIDDriveStraight(212,4.0,0.9)); //258, 4.5 timeout //185 inches at 80 AddSequential(new Pause(0.5)); //0.4 AddSequential(new PIDTurn(80 * -directionScale));//turning towards the scale //37 deg @@ -212,7 +210,6 @@ void AutoSelector::OutsideStraightScale() { } void AutoSelector::OutsideOppositeScale() { - AddSequential (new CubeIntakeDeploy (true)); AddSequential(new PIDDriveStraight(156,4.5,0.9)); //zoom to other side //154 AddSequential(new Pause(0.75)); //0.5 AddSequential(new PIDTurn(88 * directionScale,0.75));//turn to go across the back of switch //90 deg diff --git a/src/OI.cpp b/src/OI.cpp index 9dc7e2e..976f4fb 100644 --- a/src/OI.cpp +++ b/src/OI.cpp @@ -52,6 +52,7 @@ OI::OI() { CDR_topLeftBase.WhenPressed(new CubeIntakeDeploy(true)); //deploy intake CDR_topRightBase.WhenPressed(new CubeIntakeDeploy(false)); //back + CDR_middleLeftBase.WhileHeld(new CubeRunIntake(-1.0)); //alt run cube in CDR_middleLeftBase.WhenReleased(new CubeRunIntake(0.0)); //stop intake CDR_middleRightBase.WhileHeld(new CubeRunIntake(1.0)); //alt run cube out diff --git a/src/Robot.cpp b/src/Robot.cpp index 9e4b2d3..598c081 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -133,6 +133,7 @@ void Robot::AutonomousInit() { Robot::elevator->ResetMagneticEncoder(); Robot::elevator->GetBottomSwitch(); this->PickAuto(); +// Robot::cube->ActuateDeploy(frc::DoubleSolenoid::kForward); RobotMap::drivetrainRearLeftMotor->SetNeutralMode(NeutralMode::Brake); RobotMap::drivetrainRearRightMotor->SetNeutralMode(NeutralMode::Brake); if (autonomousCommand.get() != nullptr) { diff --git a/src/Subsystems/Cube.cpp b/src/Subsystems/Cube.cpp index 9efcce0..d31a90d 100644 --- a/src/Subsystems/Cube.cpp +++ b/src/Subsystems/Cube.cpp @@ -15,13 +15,10 @@ void Cube::InitDefaultCommand() { } void Cube::SetIntakeSpeed(double speed) { -#ifdef Alea intakeLeft->Set(speed *-1); //Alea intakeRight->Set(speed); //Alea -#else - intakeLeft->Set(speed * -1); //Atlas - intakeRight->Set(speed * -1); //Atlas -#endif +// intakeLeft->Set(speed * -1); //Atlas +// intakeRight->Set(speed * -1); //Atlas } void Cube::ActuateIntake(frc::DoubleSolenoid::Value direction) { diff --git a/src/Util.h b/src/Util.h index 240e3ac..22af948 100644 --- a/src/Util.h +++ b/src/Util.h @@ -22,8 +22,8 @@ class Util { static const int DRIVETRAIN_FRONT_RIGHT_MOTOR = 3; static const int DRIVETRAIN_REAR_RIGHT_MOTOR = 4; - static const int CLIMBER = 9; - static const int CLIMBER_DEPLOY = 10; + static const int CLIMBER = 9; //winch + static const int CLIMBER_DEPLOY = 10; //hook holders static const int CUBE_INTAKE_LEFT_MOTOR = 6; static const int CUBE_INTAKE_RIGHT_MOTOR = 7; From aa771c5fe86287afc9361f28259526b3b84b0b9f Mon Sep 17 00:00:00 2001 From: Loki Date: Fri, 27 Apr 2018 05:45:37 -0400 Subject: [PATCH 2/3] Day 1 changes Auton tuning and new timeout for elevator run to height commmand. --- src/Commands/AutoSelector.cpp | 30 +++++++++++++++------------- src/Commands/ElevatorRunToHeight.cpp | 9 +++++++-- src/Commands/ElevatorRunToHeight.h | 2 +- src/OI.cpp | 8 ++++---- src/Robot.cpp | 5 +++-- 5 files changed, 31 insertions(+), 23 deletions(-) diff --git a/src/Commands/AutoSelector.cpp b/src/Commands/AutoSelector.cpp index 38e681c..5a4f209 100644 --- a/src/Commands/AutoSelector.cpp +++ b/src/Commands/AutoSelector.cpp @@ -102,19 +102,19 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { AddSequential(new PIDTurn(45 * directionSwitch)); AddSequential(new Pause(0.25)); //0.3 if (ourSwitch == 'L') { - AddSequential(new PIDDriveStraight(56,2.0,1.0)); //66,default power + AddSequential(new PIDDriveStraight(54,2.0,1.0)); //66,default power//56 }else{ - AddSequential(new PIDDriveStraight(37,2.0,1.0)); //51.5, default power + AddSequential(new PIDDriveStraight(43,2.0,1.0)); //51.5, default power//37 } AddSequential(new Pause(0.3)); //0.3 if (ourSwitch == 'L') { - AddSequential(new PIDTurn(32 * -directionSwitch)); //45 + AddSequential(new PIDTurn(37 * -directionSwitch)); //45 }else{ AddSequential(new PIDTurn(38 * -directionSwitch)); //45 } AddSequential(new Pause(0.25)); //0.3 if (ourSwitch == 'L') { - AddSequential(new PIDDriveStraight(13, 1.25)); //19 + AddSequential(new PIDDriveStraight(14, 1.25)); //13 }else{ AddSequential(new PIDDriveStraight(22.5, 1.25)); //18 } @@ -122,11 +122,11 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { AddSequential(new CubeIntakeActuateOpen()); AddSequential(new CubeIntakeActuateClose()); AddSequential(new Pause(0.1)); - AddParallel(new ElevatorRunToHeight(0.5 , 0.08)); + AddParallel(new ElevatorRunToHeight(0.5 , 0.00)); //used to be 0.08, change to help combat the elevator getting stuck if (ourSwitch == 'L') { AddSequential(new AutoStraightDriveBackward(5,0.7)); }else{ - AddSequential(new AutoStraightDriveBackward(8.75,0.7)); //5 inches + AddSequential(new AutoStraightDriveBackward(5.75,0.7)); //5 inches } AddSequential(new Pause(0.2)); //0.3 if (ourSwitch == 'L') { @@ -147,7 +147,7 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { if (ourSwitch == 'L') { AddSequential(new PIDTurn(67 * directionSwitch,0.9)); //72.5 }else{ - AddSequential(new PIDTurn(66 * directionSwitch,0.9)); + AddSequential(new PIDTurn(68 * directionSwitch,0.9)); } AddSequential(new Pause(0.2)); //0.3 AddSequential(new ElevatorRunToHeight(0.75 , 2.5)); @@ -159,6 +159,8 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { } AddSequential(new CubeRunIntake(0.65,0.5)); AddSequential(new CubeIntakeActuateOpen()); + AddSequential(new Pause(0.75)); + AddSequential(new CubeIntakeActuateClose()); } void AutoSelector::SameOutsideSwitch() { @@ -185,17 +187,17 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { void AutoSelector::OutsideStraightScale() { AddSequential(new PIDDriveStraight(212,4.0,0.9)); //258, 4.5 timeout //185 inches at 80 AddSequential(new Pause(0.5)); //0.4 - AddSequential(new PIDTurn(80 * -directionScale));//turning towards the scale //37 deg + AddSequential(new PIDTurn(60 * -directionScale));//turning towards the scale //37 deg AddSequential(new Pause(0.2)); AddSequential(new AutoStraightDriveBackward(12,0.7)); AddSequential(new ElevatorRunToHeight(1.0, scaleHeight)); //Gonna be talller thane the scale AddSequential(new Pause(0.2)); AddSequential(new CubeRunIntake(1.0,0.5)); //bye bye cube //1.0 speed + AddSequential(new Pause(0.5)); //0.2 + AddSequential(new ElevatorRunToHeight(0.3, 0.0,4.0)); //0.08 AddSequential(new Pause(0.2)); - AddSequential(new ElevatorRunToHeight(0.3, 0.08)); //0.08 - AddSequential(new Pause(0.2)); - AddSequential(new PIDTurn(-90 * -directionScale,0.8)); //76 - AddSequential(new AutoStraightDriveForward(10,0.7)); + AddSequential(new PIDTurn(-75 * -directionScale,0.8)); //76 + AddSequential(new AutoStraightDriveForward(10,0.6)); // AddSequential(new Pause(0.3)); // AddParallel(new CubeRunIntake(-1.0, 3)); //3 second intake @@ -213,8 +215,8 @@ void AutoSelector::OutsideOppositeScale() { AddSequential(new PIDDriveStraight(156,4.5,0.9)); //zoom to other side //154 AddSequential(new Pause(0.75)); //0.5 AddSequential(new PIDTurn(88 * directionScale,0.75));//turn to go across the back of switch //90 deg - AddSequential(new Pause(0.3)); - AddSequential(new PIDDriveStraight(40,4.5,0.9)); //comment out if doing cube placement +// AddSequential(new Pause(0.3)); +// AddSequential(new PIDDriveStraight(40,4.5,0.9)); //comment out if doing cube placement // AddSequential(new PIDDriveStraight(139, 4.5,0.9)); //zoom to other side //160 // AddSequential(new Pause(0.3)); diff --git a/src/Commands/ElevatorRunToHeight.cpp b/src/Commands/ElevatorRunToHeight.cpp index 44374cd..d85bf6b 100644 --- a/src/Commands/ElevatorRunToHeight.cpp +++ b/src/Commands/ElevatorRunToHeight.cpp @@ -1,13 +1,14 @@ #include #include "../Robot.h" -ElevatorRunToHeight::ElevatorRunToHeight(double speed, double encPos) { +ElevatorRunToHeight::ElevatorRunToHeight(double speed, double encPos,double timeout) { // Use Requires() here to declare subsystem dependencies // eg. Requires(Robot::chassis.get()); Requires(Robot::elevator.get()); m_speed = speed; m_encPos = encPos; m_runDown = false; + SetTimeout(timeout); } // Called just before this Command runs the first time @@ -29,10 +30,13 @@ void ElevatorRunToHeight::Execute() { // Make this return true when this Command no longer needs to run execute() bool ElevatorRunToHeight::IsFinished() { +// std::cout << Robot::elevator->GetLiftMagneticEncoder() << std::endl; +// std::cout << Robot::elevator->GetBottomSwitch() << std::endl; +// std::cout << m_runDown << std::endl; return (Util::IsInTolerance(0.08, Robot::elevator->GetLiftMagneticEncoder(), m_encPos) || Robot::elevator->GetLiftMagneticEncoder() >= Util::ELEVATOR_MAX_ENCODER_HEIGHT || //Robot::elevator->GetLiftMagneticEncoder() < 0 || - (Robot::elevator->GetBottomSwitch() && m_runDown) ) ; + (Robot::elevator->GetBottomSwitch() && m_runDown) || IsTimedOut()) ; } // Called once after isFinished returns true @@ -43,5 +47,6 @@ void ElevatorRunToHeight::End() { // Called when another command which requires one or more of the same // subsystems is scheduled to run void ElevatorRunToHeight::Interrupted() { + std::cout<< "Elevator run to height interrupted. Goal: " << m_encPos << " (not reached)" << std::endl; Robot::elevator->RunLift(0.0); } diff --git a/src/Commands/ElevatorRunToHeight.h b/src/Commands/ElevatorRunToHeight.h index eacd61f..f8ae703 100644 --- a/src/Commands/ElevatorRunToHeight.h +++ b/src/Commands/ElevatorRunToHeight.h @@ -5,7 +5,7 @@ class ElevatorRunToHeight : public frc::Command { public: - ElevatorRunToHeight(double speed, double encPos); + ElevatorRunToHeight(double speed, double encPos, double timeout = 2.0); void Initialize(); void Execute(); bool IsFinished(); diff --git a/src/OI.cpp b/src/OI.cpp index 976f4fb..e27a9eb 100644 --- a/src/OI.cpp +++ b/src/OI.cpp @@ -32,8 +32,8 @@ OI::OI() { DRC_leftTrigger.WhileHeld(new CubeRunIntake(-1.0)); //pull me in dad DRC_leftTrigger.WhenReleased(new CubeRunIntake(0.0)); - DRC_rightTrigger.WhileHeld(new LowGear()); //drop a gear - DRC_rightTrigger.WhenReleased(new HighGear()); //and disappear + DRC_rightTrigger.WhileHeld(new HighGear()); //drop a gear + DRC_rightTrigger.WhenReleased(new LowGear()); //and disappear DRC_leftBumper.WhenPressed(new CubeIntakeActuateOpen()); //spread DRC_rightBumper.WhenPressed(new CubeIntakeActuateClose()); //retract DRC_xButton.WhileHeld(new CubeRunIntake(1.0)); //thanks for flying air 2512 @@ -65,9 +65,9 @@ OI::OI() { CDB_green.WhenPressed(new ElevatorResetEncoder()); //manually reset encoder CDB_yellow.WhileHeld(new ClimberRunWinch(1.0)); //run winch in, robot up CDB_yellow.WhenReleased(new ClimberRunWinch(0.0)); - CDB_topWhite.WhileHeld(new ClimberRunDeploy(0.75)); //hooks up + CDB_topWhite.WhileHeld(new ClimberRunDeploy(1.0)); //hooks up CDB_topWhite.WhenReleased(new ClimberRunDeploy(0.0)); - CDB_topRed.WhileHeld(new ClimberRunDeploy(-0.5)); //hooks down + CDB_topRed.WhileHeld(new ClimberRunDeploy(-0.75)); //hooks down CDB_topRed.WhenReleased(new ClimberRunDeploy(0.0)); CDB_middleWhite.WhenPressed(new ElevatorRunToHeight(0.7, 5.5)); //middle scale CDB_middleRed.WhenPressed(new ElevatorRunToHeight(0.7, 2.6)); //switch diff --git a/src/Robot.cpp b/src/Robot.cpp index 598c081..6fee40b 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -131,9 +131,9 @@ void Robot::AutonomousInit() { std::cout << "Starting auto..." << std::endl; Robot::drivetrain->ResetEncoders(); Robot::elevator->ResetMagneticEncoder(); - Robot::elevator->GetBottomSwitch(); this->PickAuto(); -// Robot::cube->ActuateDeploy(frc::DoubleSolenoid::kForward); + Robot::drivetrain->Shifter(frc::DoubleSolenoid::kForward); + Robot::cube->ActuateDeploy(frc::DoubleSolenoid::kForward); RobotMap::drivetrainRearLeftMotor->SetNeutralMode(NeutralMode::Brake); RobotMap::drivetrainRearRightMotor->SetNeutralMode(NeutralMode::Brake); if (autonomousCommand.get() != nullptr) { @@ -150,6 +150,7 @@ void Robot::TeleopInit() { Robot::drivetrain->ResetEncoders(); Robot::elevator->ResetMagneticEncoder(); cube->SetIntakeSpeed(0.0); + Robot::drivetrain->Shifter(frc::DoubleSolenoid::kReverse); RobotMap::drivetrainRearLeftMotor->SetNeutralMode(NeutralMode::Coast); RobotMap::drivetrainRearRightMotor->SetNeutralMode(NeutralMode::Coast); compressor->SetClosedLoopControl(true); From 00f55759698e2226f5405db4ff330cbc539aade4 Mon Sep 17 00:00:00 2001 From: Loki Date: Fri, 27 Apr 2018 21:50:43 -0400 Subject: [PATCH 3/3] Commit for day 2 of worlds. Got left side 2 cube switch running reliably. Fixed elevator not dropping to bottom by increasing the default timeout in the constructor for the command. --- CompBot.json | 2 +- src/Commands/AutoSelector.cpp | 27 ++++++++++-------- src/Commands/AutoSelector.h | 2 +- src/Commands/ElevatorManualRun.cpp | 4 +-- src/Commands/ElevatorRunToBottom.cpp | 42 ++++++++++++++++++++++++++++ src/Commands/ElevatorRunToBottom.h | 24 ++++++++++++++++ src/Commands/ElevatorRunToHeight.h | 2 +- src/Commands/ElevatorSafety.cpp | 1 + src/OI.cpp | 3 +- src/Robot.cpp | 19 ++++--------- src/Subsystems/Elevator.cpp | 6 +--- 11 files changed, 95 insertions(+), 37 deletions(-) create mode 100644 src/Commands/ElevatorRunToBottom.cpp create mode 100644 src/Commands/ElevatorRunToBottom.h diff --git a/CompBot.json b/CompBot.json index cd7c38c..6827b07 100644 --- a/CompBot.json +++ b/CompBot.json @@ -1,5 +1,5 @@ { - "dividerPosition": 0.01251303441084463, + "dividerPosition": 0.01263157894736842, "tabPane": [ { "title": "SmartDashboard", diff --git a/src/Commands/AutoSelector.cpp b/src/Commands/AutoSelector.cpp index 5a4f209..4166957 100644 --- a/src/Commands/AutoSelector.cpp +++ b/src/Commands/AutoSelector.cpp @@ -25,6 +25,7 @@ #include "Commands/ElevatorSafety.h" #include "Commands/AutoStraightDriveBackward.h" #include "Commands/CubeIntakeDeploy.h" +#include "Commands/ElevatorRunToBottom.h" AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { std::string gameMessage = @@ -95,7 +96,7 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { // AddSequential(new Pause(0.3));//t // AddParallel(new ElevatorRunToHeight(0.7 , 2.7)); //0.7,2.6 // AddSequential(new PIDDriveStraight(18)); - AddParallel(new ElevatorRunToHeight(0.7,2.7)); + AddSequential(new ElevatorRunToHeight(0.7,2.7)); AddSequential(new Pause(0.25)); AddSequential(new PIDDriveStraight(28)); AddSequential(new Pause(0.3)); //0.4 @@ -106,15 +107,15 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { }else{ AddSequential(new PIDDriveStraight(43,2.0,1.0)); //51.5, default power//37 } - AddSequential(new Pause(0.3)); //0.3 + AddSequential(new Pause(0.3)); //0.25 if (ourSwitch == 'L') { - AddSequential(new PIDTurn(37 * -directionSwitch)); //45 + AddSequential(new PIDTurn(34 * -directionSwitch)); //45 }else{ AddSequential(new PIDTurn(38 * -directionSwitch)); //45 } - AddSequential(new Pause(0.25)); //0.3 + AddSequential(new Pause(0.3)); //0.3 if (ourSwitch == 'L') { - AddSequential(new PIDDriveStraight(14, 1.25)); //13 + AddSequential(new PIDDriveStraight(16, 1.25)); //18 }else{ AddSequential(new PIDDriveStraight(22.5, 1.25)); //18 } @@ -123,6 +124,7 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { AddSequential(new CubeIntakeActuateClose()); AddSequential(new Pause(0.1)); AddParallel(new ElevatorRunToHeight(0.5 , 0.00)); //used to be 0.08, change to help combat the elevator getting stuck +// AddParallel(new ElevatorRunToBottom(-0.7, 1.5)); if (ourSwitch == 'L') { AddSequential(new AutoStraightDriveBackward(5,0.7)); }else{ @@ -145,7 +147,7 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { } AddSequential(new Pause(0.1)); if (ourSwitch == 'L') { - AddSequential(new PIDTurn(67 * directionSwitch,0.9)); //72.5 + AddSequential(new PIDTurn(66 * directionSwitch,0.9)); //72.5 }else{ AddSequential(new PIDTurn(68 * directionSwitch,0.9)); } @@ -161,6 +163,8 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) { AddSequential(new CubeIntakeActuateOpen()); AddSequential(new Pause(0.75)); AddSequential(new CubeIntakeActuateClose()); + AddSequential(new AutoStraightDriveBackward(10,0.8)); + AddSequential(new ElevatorRunToHeight(0.3, 0.0)); } void AutoSelector::SameOutsideSwitch() { @@ -189,15 +193,16 @@ void AutoSelector::OutsideStraightScale() { AddSequential(new Pause(0.5)); //0.4 AddSequential(new PIDTurn(60 * -directionScale));//turning towards the scale //37 deg AddSequential(new Pause(0.2)); - AddSequential(new AutoStraightDriveBackward(12,0.7)); + AddSequential(new AutoStraightDriveBackward(9,0.7)); AddSequential(new ElevatorRunToHeight(1.0, scaleHeight)); //Gonna be talller thane the scale AddSequential(new Pause(0.2)); AddSequential(new CubeRunIntake(1.0,0.5)); //bye bye cube //1.0 speed AddSequential(new Pause(0.5)); //0.2 - AddSequential(new ElevatorRunToHeight(0.3, 0.0,4.0)); //0.08 + AddSequential(new ElevatorRunToHeight(0.3, 0.0,8.0)); //0.08 +// AddSequential(new ElevatorRunToBottom(-0.7,3)); AddSequential(new Pause(0.2)); AddSequential(new PIDTurn(-75 * -directionScale,0.8)); //76 - AddSequential(new AutoStraightDriveForward(10,0.6)); + AddSequential(new AutoStraightDriveForward(8,0.6)); //10 // AddSequential(new Pause(0.3)); // AddParallel(new CubeRunIntake(-1.0, 3)); //3 second intake @@ -215,7 +220,7 @@ void AutoSelector::OutsideOppositeScale() { AddSequential(new PIDDriveStraight(156,4.5,0.9)); //zoom to other side //154 AddSequential(new Pause(0.75)); //0.5 AddSequential(new PIDTurn(88 * directionScale,0.75));//turn to go across the back of switch //90 deg -// AddSequential(new Pause(0.3)); +// AddSequential(new Pause(0.5)); // AddSequential(new PIDDriveStraight(40,4.5,0.9)); //comment out if doing cube placement // AddSequential(new PIDDriveStraight(139, 4.5,0.9)); //zoom to other side //160 @@ -228,7 +233,7 @@ void AutoSelector::OutsideOppositeScale() { // AddSequential(new CubeRunIntake(1.0,0.5)); //bye bye cube // AddSequential(new ElevatorRunToHeight(0.5, 0.00)); //0.08 // AddSequential(new Pause(0.2)); -// AddSequential(new AutoStraightDriveBackward(22, 0.8)); +// AddSequential(new AutoStraightDriveBackward(18, 0.7)); // AddSequential(new PIDTurn(-110)); //82 diff --git a/src/Commands/AutoSelector.h b/src/Commands/AutoSelector.h index 550ce3a..20aa27d 100644 --- a/src/Commands/AutoSelector.h +++ b/src/Commands/AutoSelector.h @@ -21,7 +21,7 @@ class AutoSelector : public frc::CommandGroup { //TODO Add in functions for doing alternate switch autos private: - static constexpr double scaleHeight = 6.8; + static constexpr double scaleHeight = 7.2; //6.8(not high enough for tipped scale) static constexpr double carryHeight = 0.35; bool doScale = false; bool doSwitch = false; diff --git a/src/Commands/ElevatorManualRun.cpp b/src/Commands/ElevatorManualRun.cpp index 87216c2..53d9f72 100644 --- a/src/Commands/ElevatorManualRun.cpp +++ b/src/Commands/ElevatorManualRun.cpp @@ -34,8 +34,8 @@ void ElevatorManualRun::Execute() { Robot::elevator->RunLift(0.0); } - } else if (Robot::oi->GetLiftControl() < -0.75) { - Robot::elevator->RunLift(Robot::oi->GetLiftControl() * 0.75); //0.5 + } else if (Robot::oi->GetLiftControl() < -0.8) { + Robot::elevator->RunLift(Robot::oi->GetLiftControl() * 0.8); //0.5 //run lift off of joystick } else if (Robot::oi->GetLiftControl() != 0) { diff --git a/src/Commands/ElevatorRunToBottom.cpp b/src/Commands/ElevatorRunToBottom.cpp new file mode 100644 index 0000000..1927a92 --- /dev/null +++ b/src/Commands/ElevatorRunToBottom.cpp @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "ElevatorRunToBottom.h" +#include "../Robot.h" + +ElevatorRunToBottom::ElevatorRunToBottom(double speed = -0.7, double timeout = 3.0) { + // Use Requires() here to declare subsystem dependencies + // eg. Requires(Robot::chassis.get()); + SetTimeout(timeout); + m_speed = speed; +} + +// Called just before this Command runs the first time +void ElevatorRunToBottom::Initialize() { + +} + +// Called repeatedly when this Command is scheduled to run +void ElevatorRunToBottom::Execute() { + Robot::elevator->RunLift(m_speed); +} + +// Make this return true when this Command no longer needs to run execute() +bool ElevatorRunToBottom::IsFinished() { + return Robot::elevator->GetBottomSwitch() || IsTimedOut(); +} + +// Called once after isFinished returns true +void ElevatorRunToBottom::End() { + +} + +// Called when another command which requires one or more of the same +// subsystems is scheduled to run +void ElevatorRunToBottom::Interrupted() { + +} diff --git a/src/Commands/ElevatorRunToBottom.h b/src/Commands/ElevatorRunToBottom.h new file mode 100644 index 0000000..a620ddf --- /dev/null +++ b/src/Commands/ElevatorRunToBottom.h @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +class ElevatorRunToBottom : public frc::Command { +public: + ElevatorRunToBottom(double speed, double timeout); + void Initialize() override; + void Execute() override; + bool IsFinished() override; + void End() override; + void Interrupted() override; + +private: + double m_speed = 0.0; +}; + diff --git a/src/Commands/ElevatorRunToHeight.h b/src/Commands/ElevatorRunToHeight.h index f8ae703..5631f4e 100644 --- a/src/Commands/ElevatorRunToHeight.h +++ b/src/Commands/ElevatorRunToHeight.h @@ -5,7 +5,7 @@ class ElevatorRunToHeight : public frc::Command { public: - ElevatorRunToHeight(double speed, double encPos, double timeout = 2.0); + ElevatorRunToHeight(double speed, double encPos, double timeout = 5.0); void Initialize(); void Execute(); bool IsFinished(); diff --git a/src/Commands/ElevatorSafety.cpp b/src/Commands/ElevatorSafety.cpp index a2e7fac..733d506 100644 --- a/src/Commands/ElevatorSafety.cpp +++ b/src/Commands/ElevatorSafety.cpp @@ -32,6 +32,7 @@ bool ElevatorSafety::IsFinished() { // Called once after isFinished returns true void ElevatorSafety::End() { Robot::elevator->RunLift(0); + Robot::elevator->ResetMagneticEncoder(); } // Called when another command which requires one or more of the same diff --git a/src/OI.cpp b/src/OI.cpp index e27a9eb..5f32cf9 100644 --- a/src/OI.cpp +++ b/src/OI.cpp @@ -52,13 +52,12 @@ OI::OI() { CDR_topLeftBase.WhenPressed(new CubeIntakeDeploy(true)); //deploy intake CDR_topRightBase.WhenPressed(new CubeIntakeDeploy(false)); //back - CDR_middleLeftBase.WhileHeld(new CubeRunIntake(-1.0)); //alt run cube in CDR_middleLeftBase.WhenReleased(new CubeRunIntake(0.0)); //stop intake CDR_middleRightBase.WhileHeld(new CubeRunIntake(1.0)); //alt run cube out CDR_middleRightBase.WhenReleased(new CubeRunIntake(0.0)); //stop intake CDR_bottomLeftBase.WhileHeld(new ClimberRunWinch(-1.0)); //run winch out, robot down - CDR_bottomLeftBase.WhenReleased(new ClimberRunDeploy(0.0)); + CDR_bottomLeftBase.WhenReleased(new ClimberRunWinch(0.0)); CDB_bigWhite.WhileHeld(new ElevatorRunLift (0.7)); //run lift up CDB_bigRed.WhileHeld(new ElevatorRunLift (-0.50)); //run lift down diff --git a/src/Robot.cpp b/src/Robot.cpp index 6fee40b..c015cc9 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -77,12 +77,12 @@ void Robot::RobotPeriodic() { SmartDashboard::PutNumber("Elevator Encoder" , Robot::elevator->GetLiftMagneticEncoder()); // SmartDashboard::PutNumber("Drivetrain PID", Robot::drivetrain->GetPIDOutput()); - SmartDashboard::PutNumber("Drivetrain Front Left Current" , RobotMap::drivetrainFrontLeftMotor->GetOutputCurrent()); - SmartDashboard::PutNumber("Drivetrain Front Right Current" , RobotMap::drivetrainFrontRightMotor->GetOutputCurrent()); - SmartDashboard::PutNumber("Drivetrain Rear Left Current" , RobotMap::drivetrainRearLeftMotor->GetOutputCurrent()); - SmartDashboard::PutNumber("Drivetrain Rear Right Current" , RobotMap::drivetrainRearRightMotor->GetOutputCurrent()); +// SmartDashboard::PutNumber("Drivetrain Front Left Current" , RobotMap::drivetrainFrontLeftMotor->GetOutputCurrent()); +// SmartDashboard::PutNumber("Drivetrain Front Right Current" , RobotMap::drivetrainFrontRightMotor->GetOutputCurrent()); +// SmartDashboard::PutNumber("Drivetrain Rear Left Current" , RobotMap::drivetrainRearLeftMotor->GetOutputCurrent()); +// SmartDashboard::PutNumber("Drivetrain Rear Right Current" , RobotMap::drivetrainRearRightMotor->GetOutputCurrent()); - SmartDashboard::PutNumber("Climber Winch" , RobotMap::climber->GetOutputCurrent()); +// SmartDashboard::PutNumber("Climber Winch" , RobotMap::climber->GetOutputCurrent()); // PrintFaults(RobotMap::drivetrainFrontLeftMotor.get(), "FL"); // PrintFaults(RobotMap::drivetrainFrontRightMotor.get(), "FR"); @@ -92,15 +92,6 @@ void Robot::RobotPeriodic() { // PrintFaults(RobotMap::cubeIntakeLeftMotor.get(), "IL"); // PrintFaults(RobotMap::cubeIntakeRightMotor.get(), "IR"); -// SmartDashboard::PutNumber("Front Left Ultrasonic distance", RobotMap::ultrasonicFrontLeft->GetDistance()); -// SmartDashboard::PutNumber("Rear Left Ultrasonic distance", RobotMap::ultrasonicRearLeft->GetDistance()); -// SmartDashboard::PutNumber("Front Right Ultrasonic distance", RobotMap::ultrasonicFrontRight->GetDistance()); -// SmartDashboard::PutNumber("Rear Right Ultrasonic distance", RobotMap::ultrasonicRearRight->GetDistance()); -// SmartDashboard::PutNumber("Average Distance Away", Robot::ultrasonicSubsystem->GetAverageDistance(Util::RobotSide::leftSide)); -// SmartDashboard::PutNumber ("Voltage Returned Front", RobotMap::ultrasonicFrontLeft->GetAnalogInput()->GetAverageVoltage()); -// SmartDashboard::PutNumber ("Voltage Returned Rear", RobotMap::ultrasonicRearLeft->GetAnalogInput()->GetAverageVoltage()); -// SmartDashboard::PutNumber("Starting Distance", Robot::ultrasonicSubsystem->m_startingDistance); - SmartDashboard::PutBoolean("Bottom Limit Switch" , RobotMap::elevatorBottomSwitch->Get()); // SmartDashboard::PutNumber("Elevator Current" , RobotMap::elevatorMotor->GetOutputCurrent()); diff --git a/src/Subsystems/Elevator.cpp b/src/Subsystems/Elevator.cpp index e127a34..4ffb737 100644 --- a/src/Subsystems/Elevator.cpp +++ b/src/Subsystems/Elevator.cpp @@ -19,11 +19,7 @@ void Elevator::InitDefaultCommand() { // here. Call these from Commands. bool Elevator::GetBottomSwitch() { - if (bottom->Get()) { - return true; - }else{ - return false; - } + return bottom->Get(); } void Elevator::RunLift(double speed) {