diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp index 9a46f5c..937ede2 100644 --- a/Commands/Autonomous/AutoDrive.cpp +++ b/Commands/Autonomous/AutoDrive.cpp @@ -1,16 +1,17 @@ #include "AutoDrive.h" #include "../../DentRobot.h" // Drive for a short while then stop. Just for testing -AutoDrive::AutoDrive(double duration, double p=-0.75) : Command("AutoDrive"){ +AutoDrive::AutoDrive(double duration, double xtmp=-0.75, double ytmp=0) : Command("AutoDrive"){ Requires(DentRobot::drivetrain); SetTimeout(duration); - power=p; + speed=xtmp; + strafe=ytmp; } void AutoDrive::Initialize(){ } void AutoDrive::Execute(){ //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro - DentRobot::drivetrain->DriveMecanum(0.0,power,0.0,0.9,0.0); + DentRobot::drivetrain->DriveMecanum(strafe,speed,0.0,0.9,0.0); } bool AutoDrive::IsFinished(){ return IsTimedOut(); diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h index f2d4785..03ce3b1 100644 --- a/Commands/Autonomous/AutoDrive.h +++ b/Commands/Autonomous/AutoDrive.h @@ -8,10 +8,10 @@ class AutoDrive: public Command{ private: - double power; + double speed, strafe; public: AutoDrive(double); - AutoDrive(double, double); + AutoDrive(double, double,double); void Initialize(); void Execute(); bool IsFinished(); diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index f12ace6..a5bbe2d 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -13,13 +13,12 @@ Autonomous::Autonomous(int seq){ switch(seq){ case 0: // Just for testing - AddSequential(new CollectTote()); - AddSequential(new Turn(90)); + AddSequential(new AutoDrive(.5,0,.25)); break; case 1: // Wait a desigated value, drive to Auto Zone (TM) Wait(SmartDashboard::GetNumber("Auto Wait Time")); - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75,0)); break; case 2: // Get one tote and go to Auto Zone (TM) @@ -32,22 +31,22 @@ Autonomous::Autonomous(int seq){ 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.75)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0)); AddSequential(new CollectTote()); AddSequential(new Raise()); - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0)); AddSequential(new CollectTote()); AddSequential(new Lower()); AddSequential(new Raise()); printf("Three totes?: %d\n",SmartDashboard::GetBoolean("Three totes")); if(SmartDashboard::GetBoolean("Three totes")){ - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Tote Distance"), -0.75,0)); AddSequential(new CollectTote()); AddSequential(new Lower()); AddSequential(new Raise()); } AddSequential(new Turn(90)); - AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Auto Zone Distance"), -0.75,0)); AddSequential(new ReleaseTote()); break; default: diff --git a/Commands/Autonomous/CollectTote.cpp b/Commands/Autonomous/CollectTote.cpp index f596853..7f89be3 100644 --- a/Commands/Autonomous/CollectTote.cpp +++ b/Commands/Autonomous/CollectTote.cpp @@ -3,7 +3,7 @@ #include "AutoDrive.h" #include "../Collector/RollIn.h" CollectTote::CollectTote(){ - AddParallel(new AutoDrive(1.0, -0.75)); + AddParallel(new AutoDrive(1.0, -0.75,0)); AddSequential(new RollIn(1.0)); } // vim: ts=2:sw=2:et diff --git a/Commands/Autonomous/ReleaseTote.cpp b/Commands/Autonomous/ReleaseTote.cpp index 6672dd1..846985d 100644 --- a/Commands/Autonomous/ReleaseTote.cpp +++ b/Commands/Autonomous/ReleaseTote.cpp @@ -4,6 +4,6 @@ #include "../Collector/RollOut.h" ReleaseTote::ReleaseTote(){ AddParallel(new RollOut()); - AddParallel(new AutoDrive(0.5, 0.75)); + AddParallel(new AutoDrive(0.5, 0.75,0)); } // vim: ts=2:sw=2:et