diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp index 711de7e..e384335 100644 --- a/Commands/Autonomous/AutoDrive.cpp +++ b/Commands/Autonomous/AutoDrive.cpp @@ -15,6 +15,7 @@ void AutoDrive::Initialize(){ void AutoDrive::Execute(){ //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle) printf("z: %f\n", z); + printf("(x, y, z): (%f, %f, %f)\n", x, y, z); DentRobot::drivetrain->DriveMecanum(x, y, z, 0.9, gyro); } bool AutoDrive::IsFinished(){ diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index 2fe6ed7..0c7d19c 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -16,7 +16,13 @@ Autonomous::Autonomous(int seq){ case 0: // Just for testing // Turn testing - AddSequential(new Turn(3.8)); + //AddSequential(new AutoDrive(2.0, -0.75, 0.0, true)); + //AddSequential(new CollectTote()); + //AddSequential(new CollectTote()); + //AddSequential(new CollectTote()); + //AddSequential(new CollectTote()); + //AddSequential(new Turn(3.8)); + AddSequential(new Turn(SmartDashboard::GetNumber("180Turn"), 0.8)); break; case 1: // Drive to Auto Zone (TM) @@ -44,8 +50,10 @@ Autonomous::Autonomous(int seq){ // Collect one, two, or three totes, drive to Auto Zone (TM), release totes AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn"))); if(SmartDashboard::GetBoolean("Two totes")){ - AddParallel(new Turn(0.81)); + AddParallel(new Turn(SmartDashboard::GetNumber("TwoToteTurn"))); + AddParallel(new RollIn(3.5)); AddSequential(new Raise(3.5)); + AddParallel(new RollIn(SmartDashboard::GetNumber("Two Tote Distance"))); AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75)); AddSequential(new CollectTote()); AddSequential(new Lower(3.0)); @@ -74,6 +82,14 @@ Autonomous::Autonomous(int seq){ // Same as auto 4 with (Three|Two) totes checked, collect bin, drive to Auto Zone (TM), release totes //TODO: Implement this break; + case 8: + AddSequential(new CollectTote(SmartDashboard::GetNumber("CollectToteTurn"))); + AddSequential(new Turn(SmartDashboard::GetNumber("180Turn"), 0.8)); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Two Tote Distance"), 0.0, 0.75)); + AddSequential(new CollectTote()); + AddSequential(new AutoDrive(SmartDashboard::GetNumber("Three Tote Distance"), 0.0, 0.75)); + AddSequential(new CollectTote()); + break; default: printf("Invalid seq: %d\n", seq); break; diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp index 2413e6c..7daa141 100644 --- a/Commands/Autonomous/Turn.cpp +++ b/Commands/Autonomous/Turn.cpp @@ -1,14 +1,15 @@ #include "Turn.h" #include "../../DentRobot.h" -Turn::Turn(double timeout): Command("Turn"){ +Turn::Turn(double timeout, double pwr): Command("Turn"){ Requires(DentRobot::drivetrain); SetTimeout(timeout); + power=pwr; } void Turn::Initialize(){ } void Turn::Execute(){ //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle) - DentRobot::drivetrain->DriveMecanum(0.0, 0.0, 0.6, 0.9); + DentRobot::drivetrain->DriveMecanum(0.0, 0.0, power, 0.9); } bool Turn::IsFinished(){ return IsTimedOut(); diff --git a/Commands/Autonomous/Turn.h b/Commands/Autonomous/Turn.h index 09d0913..b52330b 100644 --- a/Commands/Autonomous/Turn.h +++ b/Commands/Autonomous/Turn.h @@ -13,13 +13,14 @@ */ class Turn: public Command{ private: + double power; //Run(); } void DentRobot::TeleopInit(){ diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index 0b1d89e..c7a7362 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -27,9 +27,9 @@ void Drivetrain::DriveMecanum(double x, double y, double z, double sensitivity, if(DentRobot::oi->GetLeftStick()->GetRawButton(9)){ correctY /= SmartDashboard::GetNumber("DriveSpeedReductionThresh"); } - rightFront->Set((-correctX + correctY - correctZ)); + rightFront->Set(-correctX + correctY - correctZ); leftFront->Set((correctX + correctY + correctZ)*-1); - rightRear->Set((correctX + correctY - correctZ)); + rightRear->Set(correctX + correctY - correctZ); leftRear->Set((-correctX + correctY + correctZ)*-1); } //Used in pretest