diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp index c3e2e3b..eaf9669 100644 --- a/Commands/Autonomous/Autonomous.cpp +++ b/Commands/Autonomous/Autonomous.cpp @@ -1,20 +1,7 @@ #include "Autonomous.h" -#include "AutoDrive.h" -#include #include "Commands/CommandGroup.h" #include "../../DentRobot.h" -Autonomous::Autonomous() : CommandGroup("Autonomous"){ +#include "" +Autonomous::Autonomous(){ AddSequential(new AutoDrive()); } -void Autonomous::Initialize(){ -} -void Autonomous::Execute(){ -} -bool Autonomous::IsFinished(){ - return false; -} -void Autonomous::End(){ -} -void Autonomous::Interrupted(){ - End(); -} diff --git a/Commands/Test/Check.cpp b/Commands/Test/Check.cpp new file mode 100644 index 0000000..6151d27 --- /dev/null +++ b/Commands/Test/Check.cpp @@ -0,0 +1,11 @@ +#include "Check.h" +#include "Commands/CommandGroup.h" +#include "../../DentRobot.h" +#include "../../RobotMap.h" +#include "CheckDrive.h" +Check::Check(){ + AddSequential(new CheckDrive(DRIVE_FRONT_LEFT_CAN)); + AddSequential(new CheckDrive(DRIVE_FRONT_RIGHT_CAN)); + AddSequential(new CheckDrive(DRIVE_BACK_LEFT_CAN)); + AddSequential(new CheckDrive(DRIVE_BACK_RIGHT_CAN)); +} diff --git a/Commands/Test/Check.h b/Commands/Test/Check.h new file mode 100644 index 0000000..37b14bf --- /dev/null +++ b/Commands/Test/Check.h @@ -0,0 +1,20 @@ +#ifndef CHECK_H +#define CHECK_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class Check: public CommandGroup{ + private: + int motor; + public: + Check(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; +#endif diff --git a/Commands/Test/CheckDrive.cpp b/Commands/Test/CheckDrive.cpp new file mode 100644 index 0000000..73aff31 --- /dev/null +++ b/Commands/Test/CheckDrive.cpp @@ -0,0 +1,37 @@ +#include "CheckDrive.h" +#include +#include "Commands/CommandGroup.h" +#include "../../DentRobot.h" +#include "../../RobotMap.h" +CheckDrive::CheckDrive(int motorID) : CommandGroup("CheckDrive"){ + Requires(DentRobot::drivetrain); +} +void CheckDrive::Initialize(){ + SetTimeout(1.0); +} +void CheckDrive::Execute(){ + switch(motor){ + case DRIVE_FRONT_LEFT_CAN: + DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT,1); + break; + case DRIVE_FRONT_RIGHT_CAN: + DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT,1); + break; + case DRIVE_BACK_LEFT_CAN: + DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT,1); + break; + case DRIVE_BACK_RIGHT_CAN: + DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT,1); + break; + default: + break; + } +} +bool CheckDrive::IsFinished(){ + return false; +} +void CheckDrive::End(){ +} +void CheckDrive::Interrupted(){ + End(); +} diff --git a/Commands/Test/CheckDrive.h b/Commands/Test/CheckDrive.h new file mode 100644 index 0000000..ec76d13 --- /dev/null +++ b/Commands/Test/CheckDrive.h @@ -0,0 +1,20 @@ +#ifndef CHECKDRIVE_H +#define CHECKDRIVE_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class CheckDrive: public CommandGroup{ + private: + int motor; + public: + CheckDrive(int); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; +#endif diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index 80df4e7..cdb094e 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -20,3 +20,23 @@ void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, floa rightRear->Set((correctX + correctY - correctZ)); leftRear->Set((-correctX + correctY + correctZ)*-1); } + +//Used in pretest +void Drivetrain::TestMotor(e_motors motor, float power){ + switch(motor){ + case FRONTRIGHT: + rightFront->Set(power); + break; + case FRONTLEFT: + leftFront->Set(power); + break; + case BACKRIGHT: + rightRear->Set(power); + break; + case BACKLEFT: + leftRear->Set(power); + break; + default: + break; + } +} diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h index 81a5168..d4ca877 100644 --- a/Subsystems/Drivetrain.h +++ b/Subsystems/Drivetrain.h @@ -8,8 +8,15 @@ class Drivetrain: public Subsystem{ RobotDrive *drive; public: Drivetrain(); + enum e_motors{ + FRONTRIGHT, + FRONTLEFT, + BACKRIGHT, + BACKLEFT + }; void InitDefaultCommand(); void DriveMecanum(float,float,float,float,float); void DriveArcade(float, float); + void TestMotor(e_motors,float); }; #endif