Merge pull request #54 from daredevils2512/Worlds

Worlds

CompBot.json

@@ -1,5 +1,5 @@
 {
-  "dividerPosition": 0.01251303441084463,
+  "dividerPosition": 0.01263157894736842,
   "tabPane": [
     {
       "title": "SmartDashboard",

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,39 +96,39 @@ 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 ElevatorRunToHeight(0.7,2.7));
 	AddSequential(new Pause(0.25));
 	AddSequential(new PIDDriveStraight(28));
 	AddSequential(new Pause(0.3)); //0.4
 	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
+	AddSequential(new Pause(0.3)); //0.25
 	if (ourSwitch == 'L') {
-		AddSequential(new PIDTurn(32 * -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(13, 1.25)); //19
+		AddSequential(new PIDDriveStraight(16, 1.25)); //18
 	}else{
 		AddSequential(new PIDDriveStraight(22.5, 1.25)); //18
 	}
 	AddSequential(new CubeRunIntake(0.65,0.5)); //0.8 for power
 	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
+//	AddParallel(new ElevatorRunToBottom(-0.7, 1.5));
 	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') {
@@ -146,9 +147,9 @@ 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(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));
@@ -160,6 +161,10 @@ AutoSelector::AutoSelector(AutonomousSource* autonomousSource) {
 	}
 	AddSequential(new CubeRunIntake(0.65,0.5));
 	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() {
@@ -184,20 +189,20 @@ 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
+	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,8.0)); //0.08
+//	AddSequential(new ElevatorRunToBottom(-0.7,3));
 	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(8,0.6)); //10
 
 //	AddSequential(new Pause(0.3));
 //	AddParallel(new CubeRunIntake(-1.0, 3)); //3 second intake
@@ -212,12 +217,11 @@ 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
-	AddSequential(new Pause(0.3));
-	AddSequential(new PIDDriveStraight(40,4.5,0.9)); //comment out if doing cube placement
+//	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
 //	AddSequential(new Pause(0.3));
@@ -229,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
 
 

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;

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) {

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() {
+
+}

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 <Commands/Command.h>
+
+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;
+};
+

src/Commands/ElevatorRunToHeight.cpp

@@ -1,13 +1,14 @@
 #include <Commands/ElevatorRunToHeight.h>
 #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);
 }

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 = 5.0);
 	void Initialize();
 	void Execute();
 	bool IsFinished();

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

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
@@ -57,16 +57,16 @@ OI::OI() {
 	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
 	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

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());
 
@@ -131,8 +122,9 @@ void Robot::AutonomousInit() {
 	std::cout << "Starting auto..." << std::endl;
 	Robot::drivetrain->ResetEncoders();
 	Robot::elevator->ResetMagneticEncoder();
-	Robot::elevator->GetBottomSwitch();
 	this->PickAuto();
+	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) {
@@ -149,6 +141,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);

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) {

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) {