From 9cb891b90a51d243709333edc642142fcd383a69 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Sun, 1 Mar 2015 16:48:37 -0500 Subject: [PATCH 1/4] Minor command improvements --- Commands/Autonomous/Turn.cpp | 2 +- Commands/Autonomous/Turn.h | 2 +- Commands/BinElevator/BinCloseArms.cpp | 3 +-- Commands/BinElevator/BinOpenArms.cpp | 1 - 4 files changed, 3 insertions(+), 5 deletions(-) diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp index 11a51d9..848cc6e 100644 --- a/Commands/Autonomous/Turn.cpp +++ b/Commands/Autonomous/Turn.cpp @@ -1,7 +1,7 @@ #include "Turn.h" #include "../../DentRobot.h" // Drive for a short while then stop. Just for testing -Turn::Turn(int k) : Command("Turn"){ +Turn::Turn(double k) : Command("Turn"){ Requires(DentRobot::drivetrain); SetTimeout(k); } diff --git a/Commands/Autonomous/Turn.h b/Commands/Autonomous/Turn.h index b128c20..515ce44 100644 --- a/Commands/Autonomous/Turn.h +++ b/Commands/Autonomous/Turn.h @@ -10,7 +10,7 @@ class Turn: public Command{ private: int degrees; public: - Turn(int); + Turn(double); void Initialize(); void Execute(); bool IsFinished(); diff --git a/Commands/BinElevator/BinCloseArms.cpp b/Commands/BinElevator/BinCloseArms.cpp index 1cb292f..37a4534 100644 --- a/Commands/BinElevator/BinCloseArms.cpp +++ b/Commands/BinElevator/BinCloseArms.cpp @@ -4,11 +4,10 @@ BinCloseArms::BinCloseArms() : Command("BinCloseArms"){ } void BinCloseArms::Initialize(){ - //Should never need to use this SetTimeout(0.5); } void BinCloseArms::Execute(){ - DentRobot::pneumatics->SetOpen(true); + DentRobot::pneumatics->SetOpen(false); } bool BinCloseArms::IsFinished(){ return true; diff --git a/Commands/BinElevator/BinOpenArms.cpp b/Commands/BinElevator/BinOpenArms.cpp index c8936a4..2a415d3 100644 --- a/Commands/BinElevator/BinOpenArms.cpp +++ b/Commands/BinElevator/BinOpenArms.cpp @@ -4,7 +4,6 @@ BinOpenArms::BinOpenArms() : Command("BinOpenArms"){ } void BinOpenArms::Initialize(){ - //Should never need to use this SetTimeout(0.5); } void BinOpenArms::Execute(){ From 63e9adce86ea8a11c7ddf0acd330f346d28f66cd Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Sun, 1 Mar 2015 17:23:23 -0500 Subject: [PATCH 2/4] Use doubles instead of floats (untested) --- Commands/BinElevator/BinLower.cpp | 2 +- Commands/BinElevator/BinLower.h | 4 ++-- Commands/BinElevator/BinRaise.cpp | 2 +- Commands/BinElevator/BinRaise.h | 4 ++-- Subsystems/Drivetrain.cpp | 4 ++-- Subsystems/Drivetrain.h | 6 +++--- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/Commands/BinElevator/BinLower.cpp b/Commands/BinElevator/BinLower.cpp index ee2bc43..336e896 100644 --- a/Commands/BinElevator/BinLower.cpp +++ b/Commands/BinElevator/BinLower.cpp @@ -1,7 +1,7 @@ #include "BinLower.h" #include "../../DentRobot.h" #include "../../OI.h" -BinLower::BinLower(float t) : Command("BinLower"){ +BinLower::BinLower(double t) : Command("BinLower"){ timeout=t; } void BinLower::Initialize(){ diff --git a/Commands/BinElevator/BinLower.h b/Commands/BinElevator/BinLower.h index 49813b0..79e5286 100644 --- a/Commands/BinElevator/BinLower.h +++ b/Commands/BinElevator/BinLower.h @@ -6,9 +6,9 @@ class BinLower: public Command{ private: - float timeout; + double timeout; public: - BinLower(float); + BinLower(double); void Initialize(); void Execute(); bool IsFinished(); diff --git a/Commands/BinElevator/BinRaise.cpp b/Commands/BinElevator/BinRaise.cpp index 21f46f7..2b53e81 100644 --- a/Commands/BinElevator/BinRaise.cpp +++ b/Commands/BinElevator/BinRaise.cpp @@ -1,7 +1,7 @@ #include "BinRaise.h" #include "../../DentRobot.h" #include "../../OI.h" -BinRaise::BinRaise(float t) : Command("BinRaise"){ +BinRaise::BinRaise(double t) : Command("BinRaise"){ timeout=t; } void BinRaise::Initialize(){ diff --git a/Commands/BinElevator/BinRaise.h b/Commands/BinElevator/BinRaise.h index d173430..cdea982 100644 --- a/Commands/BinElevator/BinRaise.h +++ b/Commands/BinElevator/BinRaise.h @@ -6,9 +6,9 @@ class BinRaise: public Command{ private: - float timeout; + double timeout; public: - BinRaise(float); + BinRaise(double); void Initialize(); void Execute(); bool IsFinished(); diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index c56b463..2eff667 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -11,7 +11,7 @@ Drivetrain::Drivetrain() : Subsystem("Drivetrain"){ void Drivetrain::InitDefaultCommand(){ SetDefaultCommand(new Drive()); } -void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){ +void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, double gyro){ double correctX = -(sensitivity*(pow(x, 3))+(1-sensitivity)*x); double correctY = -(sensitivity*(pow(y, 3))+(1-sensitivity)*y); double correctZ = -z * 0.5; @@ -25,7 +25,7 @@ void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, floa } //Used in pretest -void Drivetrain::TestMotor(e_motors motor, float power){ +void Drivetrain::TestMotor(e_motors motor, double power){ switch(motor){ case FRONTRIGHT: rightFront->Set(power); diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h index 4550de8..45fc325 100644 --- a/Subsystems/Drivetrain.h +++ b/Subsystems/Drivetrain.h @@ -15,9 +15,9 @@ class Drivetrain: public Subsystem{ BACKLEFT }; void InitDefaultCommand(); - void DriveMecanum(float, float, float, float, float); - void DriveArcade(float, float); - void TestMotor(e_motors, float); + void DriveMecanum(double, double, double, double, double); + void DriveArcade(double, double); + void TestMotor(e_motors, double); }; #endif // vim: ts=2:sw=2:et From 136a28994e10a7efb011fd7d8a909757c96dff02 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Thu, 5 Mar 2015 20:07:52 -0500 Subject: [PATCH 3/4] Added more autonomous modes (untested). Changed default autonomous timeout to 0.0s from 3.0s --- Commands/Autonomous/Autonomous.cpp | 43 ++++++++++++++++++++++-------- DentRobot.cpp | 6 +++-- 2 files changed, 36 insertions(+), 13 deletions(-) diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index d3339a1..e8f52cd 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -24,7 +24,16 @@ Autonomous::Autonomous(int seq){ AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.8)); break; case 2: - // Collect a tote, turn, drive to Auto Zone (TM) + // Lower BinElevator, collect bin, turn, drive to AutoZone (TM) + AddSequential(new BinLower(0.5)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Bin Distance"), 0, 0.75)); + AddSequential(new BinRaise(1.0)); + AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0.0, -0.75)); + AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); + break; + case 3: + // Collect a tote with BinElevator, turn, drive to Auto Zone (TM) Wait(SmartDashboard::GetNumber("Auto Wait Time")); AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); AddSequential(new BinRaise(1.2)); @@ -32,29 +41,41 @@ Autonomous::Autonomous(int seq){ AddSequential(new BinLower(1.0)); AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); break; - case 3: - // Collect three totes, drive to Auto Zone (TM) + case 4: + // Collect one, two, or three totes, drive to Auto Zone (TM) printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time")); Wait(SmartDashboard::GetNumber("Auto Wait Time")); printf("Done"); - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75)); AddSequential(new CollectTote()); - AddSequential(new Raise()); - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75)); - AddSequential(new CollectTote()); - AddSequential(new Lower()); - AddSequential(new Raise()); - printf("Three totes?: %d\n", SmartDashboard::GetBoolean("Three totes")); - if(SmartDashboard::GetBoolean("Three totes")){ + if(SmartDashboard::GetBoolean("Two totes")){ + AddSequential(new Raise()); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75)); AddSequential(new CollectTote()); AddSequential(new Lower()); AddSequential(new Raise()); + if(SmartDashboard::GetBoolean("Three totes")){ + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), 0, 0.75)); + AddSequential(new CollectTote()); + AddSequential(new Lower()); + AddSequential(new Raise()); + } } AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), 0, 0.75)); AddSequential(new ReleaseTote()); break; + case 5: + // Same as auto 4, but navigate around bins + //TODO: Implement this + break; + case 6: + // Collect 1 bin and 1 tote + //TODO: Implement this + break; + case 7: + // Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM) + //TODO: Implement this + break; default: printf("Invalid seq: %d\n", seq); break; diff --git a/DentRobot.cpp b/DentRobot.cpp index 0bd011e..69632fc 100644 --- a/DentRobot.cpp +++ b/DentRobot.cpp @@ -25,14 +25,16 @@ void DentRobot::RobotInit(){ // Autonomous // Sequence of autonomous command SmartDashboard::PutNumber("Auto Sequence", 2.0); - SmartDashboard::PutNumber("Auto Wait Time", 3.0); + SmartDashboard::PutNumber("Auto Wait Time", 0.0); // If the robot will be picking up three totes in sequence 3 - SmartDashboard::PutBoolean("Three totes", true); + SmartDashboard::PutBoolean("Two totes", true); + SmartDashboard::PutBoolean("Three totes", false); // TODO: Calibrate the following two values // Distance (in time) to auto zone SmartDashboard::PutNumber("Auto Zone Distance", 2.8); // Distance (in time) to auto tote (used in sequence 3) SmartDashboard::PutNumber("Auto Tote Distance", 0.5); + SmartDashboard::PutNumber("Auto Bin Distance", 0.25); SmartDashboard::PutNumber("TurnAmount", 2.0); // Elevators From 2e069a5caddac0302a37b5f05c07541bfe3406c7 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Thu, 5 Mar 2015 20:33:18 -0500 Subject: [PATCH 4/4] Eject totes after auto sequence 4 and 7 --- Commands/Autonomous/Autonomous.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index e8f52cd..b1dc4e0 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -42,7 +42,7 @@ Autonomous::Autonomous(int seq){ AddSequential(new Turn(SmartDashboard::GetNumber("TurnAmount"))); break; case 4: - // Collect one, two, or three totes, drive to Auto Zone (TM) + // Collect one, two, or three totes, drive to Auto Zone (TM), release totes printf("Waiting: %f\n", SmartDashboard::GetNumber("Auto Wait Time")); Wait(SmartDashboard::GetNumber("Auto Wait Time")); printf("Done"); @@ -73,7 +73,7 @@ Autonomous::Autonomous(int seq){ //TODO: Implement this break; case 7: - // Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM) + // Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes //TODO: Implement this break; default: