From 267a720b83d2245660d1f726b68006f5448e0ac8 Mon Sep 17 00:00:00 2001 From: Austen Adler Date: Sun, 8 Feb 2015 16:51:35 -0500 Subject: [PATCH] Default value changes, more working on auto --- Commands/Autonomous/AutoDrive.cpp | 4 ++-- Commands/Autonomous/Autonomous.cpp | 17 +++++++++++++++-- Commands/Autonomous/Turn.cpp | 22 ++++++++++++++++++++++ Commands/Autonomous/Turn.h | 19 +++++++++++++++++++ Commands/Collector/CloseCollector.cpp | 14 +++++++++++--- Commands/Collector/CollectTote.cpp | 3 ++- Commands/Collector/OpenCollector.cpp | 7 ++++--- Commands/Drivetrain/Drive.cpp | 12 ++++++------ Commands/Elevator/Lower.cpp | 2 +- Commands/Elevator/Raise.cpp | 2 +- OI.cpp | 12 ++++++------ 11 files changed, 89 insertions(+), 25 deletions(-) create mode 100644 Commands/Autonomous/Turn.cpp create mode 100644 Commands/Autonomous/Turn.h diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp index b5e65df..b7833ec 100644 --- a/Commands/Autonomous/AutoDrive.cpp +++ b/Commands/Autonomous/AutoDrive.cpp @@ -4,13 +4,13 @@ // Drive for a short while then stop. Just for testing AutoDrive::AutoDrive() : Command("AutoDrive"){ Requires(DentRobot::drivetrain); - SetTimeout(1.0); + SetTimeout(0.5); } void AutoDrive::Initialize(){ } void AutoDrive::Execute(){ //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro - DentRobot::drivetrain->DriveMecanum(0.5,0,0,0.9,0); + DentRobot::drivetrain->DriveMecanum(0.0,-.75,0.0,0.9,0.0); } bool AutoDrive::IsFinished(){ return IsTimedOut(); diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index 33bfe6e..bc64654 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -1,9 +1,22 @@ #include "Autonomous.h" -#include "AutoDrive.h" #include "../../DentRobot.h" #include "../Elevator/Raise.h" +#include "../Elevator/Lower.h" +#include "AutoDrive.h" +#include "Turn.h" +#include "../Collector/CloseCollector.h" +#include "../Collector/OpenCollector.h" +#include "../Collector/CollectTote.h" Autonomous::Autonomous(){ - AddSequential(new AutoDrive()); + //AddSequential(new Raise()); + //AddSequential(new Lower()); + //AddSequential(new AutoDrive()); AddSequential(new Raise()); + AddSequential(new Lower()); + AddSequential(new Turn()); + AddParallel(new AutoDrive()); + AddParallel(new CloseCollector()); + AddParallel(new CollectTote()); + AddSequential(new Turn()); } // vim: ts=2:sw=2:et diff --git a/Commands/Autonomous/Turn.cpp b/Commands/Autonomous/Turn.cpp new file mode 100644 index 0000000..174f6cc --- /dev/null +++ b/Commands/Autonomous/Turn.cpp @@ -0,0 +1,22 @@ +#include "Turn.h" +#include "../../DentRobot.h" +// Drive for a short while then stop. Just for testing +Turn::Turn() : Command("Turn"){ + Requires(DentRobot::drivetrain); + SetTimeout(0.25); +} +void Turn::Initialize(){ +} +void Turn::Execute(){ + //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro + DentRobot::drivetrain->DriveMecanum(0.0,0.0,0.5,0.9,0.0); +} +bool Turn::IsFinished(){ + return IsTimedOut(); +} +void Turn::End(){ +} +void Turn::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Autonomous/Turn.h b/Commands/Autonomous/Turn.h new file mode 100644 index 0000000..2adc701 --- /dev/null +++ b/Commands/Autonomous/Turn.h @@ -0,0 +1,19 @@ +#ifndef TURN_H +#define TURN_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class Turn: public Command{ + public: + Turn(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/CloseCollector.cpp b/Commands/Collector/CloseCollector.cpp index cd95a99..2418732 100644 --- a/Commands/Collector/CloseCollector.cpp +++ b/Commands/Collector/CloseCollector.cpp @@ -3,13 +3,21 @@ CloseCollector::CloseCollector() : Command("CloseCollector"){ Requires(DentRobot::collector); } void CloseCollector::Initialize(){ - SetTimeout(0.5); + printf("Initialized collector: 0.5\n"); + SetTimeout(2.5); } void CloseCollector::Execute(){ - DentRobot::collector->MoveArms(0.2f); + //printf("Closing collector: -0.5f\n"); + DentRobot::collector->MoveArms(-0.5); + //DentRobot::collector->MoveArms(-(-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5); } bool CloseCollector::IsFinished(){ - return DentRobot::collector->ArmSensor(); + if(DentRobot::collector->ArmSensor()||IsTimedOut()){ + printf("Stopped Closing: %d, %d\n",DentRobot::collector->ArmSensor(), IsTimedOut()); + return true; + }else{ + return false; + } } void CloseCollector::End(){ DentRobot::collector->MoveArms(0.0f); diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp index 13c5bcc..acabd4e 100644 --- a/Commands/Collector/CollectTote.cpp +++ b/Commands/Collector/CollectTote.cpp @@ -3,6 +3,7 @@ CollectTote::CollectTote() : Command("CollectTote"){ Requires(DentRobot::collector); } void CollectTote::Initialize(){ + printf("Initialized CollectTote\n"); SetTimeout(2.0); } void CollectTote::Execute(){ @@ -10,7 +11,7 @@ void CollectTote::Execute(){ DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2); } bool CollectTote::IsFinished(){ - return DentRobot::collector->BoxCollected(); + return DentRobot::collector->BoxCollected()||IsTimedOut(); } void CollectTote::End(){ DentRobot::collector->MoveRollers(0.0); diff --git a/Commands/Collector/OpenCollector.cpp b/Commands/Collector/OpenCollector.cpp index 93deb00..5cebc3e 100644 --- a/Commands/Collector/OpenCollector.cpp +++ b/Commands/Collector/OpenCollector.cpp @@ -6,11 +6,12 @@ void OpenCollector::Initialize(){ SetTimeout(0.5); } void OpenCollector::Execute(){ - //TODO check this value to move the motors in the right direction - DentRobot::collector->MoveArms(-0.2f); + DentRobot::collector->MoveArms(0.35); + //DentRobot::collector->MoveArms((-DentRobot::oi->GetRightStick()->GetRawAxis(3)+1)/2*.3/.5); } bool OpenCollector::IsFinished(){ - return DentRobot::collector->ArmSensor(); + //return DentRobot::collector->ArmSensor(); + return IsTimedOut(); } void OpenCollector::End(){ DentRobot::collector->MoveArms(0.0f); diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp index 3fe46f9..60fa59c 100644 --- a/Commands/Drivetrain/Drive.cpp +++ b/Commands/Drivetrain/Drive.cpp @@ -12,12 +12,12 @@ void Drive::Execute(){ x = DentRobot::oi->GetLeftStick()->GetRawAxis(0); z = DentRobot::oi->GetLeftStick()->GetRawAxis(2); y = DentRobot::oi->GetLeftStick()->GetRawAxis(1); - if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){ - x=0; - } - if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){ - y=0; - } + //if (DentRobot::oi->GetLeftStick()->GetRawButton(1)){ + // x=0; + //} + //if (DentRobot::oi->GetLeftStick()->GetRawButton(2)){ + // y=0; + //} //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0); } diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp index c29c16f..378156d 100644 --- a/Commands/Elevator/Lower.cpp +++ b/Commands/Elevator/Lower.cpp @@ -4,7 +4,7 @@ Lower::Lower() : Command("Lower"){ } void Lower::Initialize(){ - SetTimeout(2.0); + SetTimeout(2.5); } void Lower::Execute(){ DentRobot::elevator->Run(-1.0); diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp index 92eecc9..6f900b4 100644 --- a/Commands/Elevator/Raise.cpp +++ b/Commands/Elevator/Raise.cpp @@ -4,7 +4,7 @@ Raise::Raise() : Command("Raise"){ } void Raise::Initialize(){ - SetTimeout(2.5); + SetTimeout(3.0); } void Raise::Execute(){ DentRobot::elevator->Run(1.0); diff --git a/OI.cpp b/OI.cpp index f2c7400..54c4212 100644 --- a/OI.cpp +++ b/OI.cpp @@ -13,12 +13,12 @@ OI::OI() { // Collector JoystickButton *right1=new JoystickButton(rightStick, 1); JoystickButton *right2=new JoystickButton(rightStick, 2); - JoystickButton *left3=new JoystickButton(leftStick, 3); - JoystickButton *left4=new JoystickButton(leftStick, 4); - right1->WhileHeld(new OpenCollector()); - right2->WhileHeld(new CloseCollector()); - left3->WhileHeld(new CollectTote()); - left4->WhileHeld(new ReleaseTote()); + JoystickButton *left1=new JoystickButton(leftStick, 1); + JoystickButton *left2=new JoystickButton(leftStick, 2); + right1->WhileHeld(new CloseCollector()); + right2->WhileHeld(new OpenCollector()); + left1->WhileHeld(new CollectTote()); + left2->WhileHeld(new ReleaseTote()); // Elevator Raise* raise=new Raise(); Lower* lower=new Lower();