diff --git a/CommandBase.cpp b/CommandBase.cpp index ed78e06..a517bee 100644 --- a/CommandBase.cpp +++ b/CommandBase.cpp @@ -9,7 +9,6 @@ #include "Commands/OpenCollector.h" #include "Commands/CollectTote.h" #include "Commands/ReleaseTote.h" -#include "Commands/Eject.h" #include "Commands/Raise.h" #include "Commands/Lower.h" #include "Commands/Calibrate.h" @@ -23,8 +22,7 @@ CommandBase::CommandBase(char const *name) : Command(name) { } CommandBase::CommandBase() : Command() { } -void CommandBase::init() -{ +void CommandBase::init(){ drivetrain = new Drivetrain(); collector = new Collector(); elevator = new Elevator(); diff --git a/Commands/CloseCollector.cpp b/Commands/CloseCollector.cpp index 94e4c5d..b415152 100644 --- a/Commands/CloseCollector.cpp +++ b/Commands/CloseCollector.cpp @@ -3,12 +3,13 @@ CloseCollector::CloseCollector() : Command("CloseCollector"){ Requires(DentRobot::collector); } void CloseCollector::Initialize(){ + SetTimeout(0.5); } void CloseCollector::Execute(){ DentRobot::collector->MoveArms(.1); } bool CloseCollector::IsFinished(){ - return DentRobot::collector->ArmsDoneMoving(); + return DentRobot::collector->ArmSensor(); } void CloseCollector::End(){ } diff --git a/Commands/Compressor/StartCompressing.cpp b/Commands/Compressor/StartCompressing.cpp index 8e3b047..b71d975 100644 --- a/Commands/Compressor/StartCompressing.cpp +++ b/Commands/Compressor/StartCompressing.cpp @@ -1,13 +1,12 @@ #include "StartCompressing.h" -#include #include "../../DentRobot.h" StartCompressing::StartCompressing() : Command("StartCompressing"){ - Requires(DentRobot::airCompressor); + Requires(DentRobot::airCompressor); } void StartCompressing::Initialize(){ } void StartCompressing::Execute(){ - DentRobot::airCompressor->CreateCompressedAir(); + DentRobot::airCompressor->StartCompressing(); } bool StartCompressing::IsFinished(){ return false; diff --git a/Commands/Compressor/StopCompressing.cpp b/Commands/Compressor/StopCompressing.cpp index 7d2120a..009ae4f 100644 --- a/Commands/Compressor/StopCompressing.cpp +++ b/Commands/Compressor/StopCompressing.cpp @@ -1,13 +1,12 @@ #include "StopCompressing.h" -#include #include "../../DentRobot.h" StopCompressing::StopCompressing() : Command("StopCompressing"){ - Requires(DentRobot::airCompressor); + Requires(DentRobot::airCompressor); } void StopCompressing::Initialize(){ } void StopCompressing::Execute(){ - DentRobot::airCompressor->StopCreatingCompressedAir(); + DentRobot::airCompressor->StopCompressing(); } bool StopCompressing::IsFinished(){ return false; diff --git a/Commands/Eject.cpp b/Commands/Eject.cpp deleted file mode 100644 index 17ca579..0000000 --- a/Commands/Eject.cpp +++ /dev/null @@ -1,16 +0,0 @@ -#include "Eject.h" -#include "../DentRobot.h" -Eject::Eject() : Command("Eject"){ - Requires(DentRobot::collector); -} -void Eject::Initialize(){ -} -void Eject::Execute(){ -} -bool Eject::IsFinished(){ - return false; -} -void Eject::End(){ -} -void Eject::Interrupted(){ -} diff --git a/Commands/Eject.h b/Commands/Eject.h deleted file mode 100644 index 8793be9..0000000 --- a/Commands/Eject.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef EJECT_H -#define EJECT_H - -#include "Commands/Command.h" -#include "WPILib.h" - -class Eject: public Command{ - public: - Eject(); - void Initialize(); - void Execute(); - bool IsFinished(); - void End(); - void Interrupted(); -}; - -#endif diff --git a/Commands/OpenCollector.cpp b/Commands/OpenCollector.cpp index 8538ec6..bbd00c5 100644 --- a/Commands/OpenCollector.cpp +++ b/Commands/OpenCollector.cpp @@ -1,15 +1,15 @@ #include "OpenCollector.h" OpenCollector::OpenCollector() : Command("OpenCollector"){ - Requires(DentRobot::collector); + Requires(DentRobot::collector); } void OpenCollector::Initialize(){ } void OpenCollector::Execute(){ - //TODO check this value to move the motors in the right direction - DentRobot::collector->MoveArms(-.1); + //TODO check this value to move the motors in the right direction + DentRobot::collector->MoveArms(-.1); } bool OpenCollector::IsFinished(){ - return DentRobot::collector->ArmsDoneMoving(); + return DentRobot::collector->ArmSensor(); } void OpenCollector::End(){ } diff --git a/OI.cpp b/OI.cpp index bf1225f..a2c9e66 100644 --- a/OI.cpp +++ b/OI.cpp @@ -1,7 +1,6 @@ #include "OI.h" #include "Commands/Lower.h" #include "Commands/Raise.h" -#include "Commands/Eject.h" #include "Commands/OpenCollector.h" #include "Commands/CloseCollector.h" #include "Commands/CollectTote.h" @@ -13,7 +12,6 @@ OI::OI() { leftStick=new Joystick(0); rightStick=new Joystick(1); //TODO name these buttons to their functions rather to their number - JoystickButton *left10=new JoystickButton(leftStick, 10); JoystickButton *right1=new JoystickButton(rightStick, 1); JoystickButton *right2=new JoystickButton(rightStick, 2); JoystickButton *right3=new JoystickButton(rightStick, 3); @@ -22,7 +20,6 @@ OI::OI() { right2->WhenPressed(new CloseCollector()); right3->WhenPressed(new CollectTote()); right4->WhenPressed(new ReleaseTote()); - left10->WhenPressed(new Eject()); } Joystick* OI::GetRightStick(){ return rightStick; diff --git a/RobotMap.h b/RobotMap.h index 11d078f..294feab 100644 --- a/RobotMap.h +++ b/RobotMap.h @@ -3,4 +3,23 @@ #include "WPILib.h" +// Elevator +#define ELEVATOR_CAN 0 + +// Drivetrain +#define DRIVE_FRONT_LEFT_CAN 41 +#define DRIVE_BACK_LEFT_CAN 43 +#define DRIVE_FRONT_RIGHT_CAN 40 +#define DRIVE_BACK_RIGHT_CAN 42 + +// Collector +#define COLLECTOR_WINDOW_LEFT_CAN 50 +#define COLLECTOR_WINDOW_RIGHT_CAN 51 +#define COLLECTOR_LEFT_CAN 52 +#define COLLECTOR_RIGHT_CAN 53 +#define COLLECTOR_BOXSWITCH_DIO 9 + +// Compressor +#define COMPRESSOR_CAN 31 + #endif diff --git a/Subsystems/AirCompressor.cpp b/Subsystems/AirCompressor.cpp index cda5a76..b813d9b 100644 --- a/Subsystems/AirCompressor.cpp +++ b/Subsystems/AirCompressor.cpp @@ -2,17 +2,15 @@ #include "../RobotMap.h" AirCompressor::AirCompressor() : Subsystem("AirCompressor") { - compressher = new Compressor(31); + compressor = new Compressor(COMPRESSOR_CAN); } void AirCompressor::InitDefaultCommand() { } -int AirCompressor::CreateCompressedAir() { - printf("compressing and stuff\n"); - compressher->Start(); - return 0; +void AirCompressor::StartCompressing() { + printf("Starting compressor\n"); + compressor->Start(); } -int AirCompressor::StopCreatingCompressedAir() { - printf("not compressing and stuff\n"); - compressher->Stop(); - return 0; +void AirCompressor::StopCompressing() { + printf("Stopping compressor\n"); + compressor->Stop(); } diff --git a/Subsystems/AirCompressor.h b/Subsystems/AirCompressor.h index c57296d..ca48f25 100644 --- a/Subsystems/AirCompressor.h +++ b/Subsystems/AirCompressor.h @@ -5,11 +5,11 @@ class AirCompressor: public Subsystem { private: - Compressor *compressher; + Compressor *compressor; public: AirCompressor(); void InitDefaultCommand(); - int CreateCompressedAir(); - int StopCreatingCompressedAir(); + void StartCompressing(); + void StopCompressing(); }; #endif diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp index 4e39880..563eb54 100644 --- a/Subsystems/Collector.cpp +++ b/Subsystems/Collector.cpp @@ -2,31 +2,25 @@ #include "../RobotMap.h" Collector::Collector() : Subsystem("Collector") { - windowMotorLeft=new CANTalon(50); - windowMotorRight=new CANTalon(51); - collectorMotorLeft=new CANTalon(52); - collectorMotorRight=new CANTalon(53); - boxSwitch=new DigitalInput(9); + windowMotorLeft=new CANTalon(COLLECTOR_WINDOW_LEFT_CAN); + windowMotorRight=new CANTalon(COLLECTOR_WINDOW_RIGHT_CAN); + collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN); + collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN); + boxSwitch=new DigitalInput(COLLECTOR_BOXSWITCH_DIO); } void Collector::InitDefaultCommand() { } void Collector::MoveArms(float a){ windowMotorLeft->Set(a); windowMotorRight->Set(-a); - a++; } void Collector::MoveRollers(float a){ collectorMotorLeft->Set(a); collectorMotorRight->Set(-a); - r++; } -bool Collector::ArmsDoneMoving(){ - //TODO calibrate these values or find a sensor to use - if(a>=200){ - return true; - }else{ - return false; - } +bool Collector::ArmSensor(){ + // TODO: include limit switch code + return false; } bool Collector::BoxCollected(){ return boxSwitch->Get(); diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h index c6e85a9..f5bc658 100644 --- a/Subsystems/Collector.h +++ b/Subsystems/Collector.h @@ -7,13 +7,12 @@ class Collector: public Subsystem private: CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorRight; DigitalInput *boxSwitch; - int a,r; public: Collector(); void InitDefaultCommand(); void MoveArms(float); void MoveRollers(float); - bool ArmsDoneMoving(); + bool ArmSensor(); bool BoxCollected(); }; #endif diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp index 17f690c..1ba7c9b 100644 --- a/Subsystems/Drivetrain.cpp +++ b/Subsystems/Drivetrain.cpp @@ -3,20 +3,20 @@ #include "../Commands/Drive.h" Drivetrain::Drivetrain() : Subsystem("Drivetrain"){ - rightFront = new CANTalon(40); - leftFront = new CANTalon(41); - rightRear = new CANTalon(42); - leftRear = new CANTalon(43); + rightFront = new CANTalon(DRIVE_FRONT_RIGHT_CAN); + leftFront = new CANTalon(DRIVE_FRONT_LEFT_CAN); + rightRear = new CANTalon(DRIVE_BACK_RIGHT_CAN); + leftRear = new CANTalon(DRIVE_BACK_LEFT_CAN); } void Drivetrain::InitDefaultCommand(){ SetDefaultCommand(new Drive()); } void Drivetrain::DriveMecanum(float x, float y, float z, float sensitivity, float gyro){ - double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y); - double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x); - double correctZ = -z *.5; - rightFront->Set((-correctX + correctY - correctZ)); - leftFront->Set((correctX + correctY + correctZ)*-1); - rightRear->Set((correctX + correctY - correctZ)); - leftRear->Set((-correctX + correctY + correctZ)*-1); + double correctY = (sensitivity*(pow(y,3))+(1-sensitivity)*y); + double correctX = -(sensitivity*(pow(x,3))+(1-sensitivity)*x); + double correctZ = -z *.5; + rightFront->Set((-correctX + correctY - correctZ)); + leftFront->Set((correctX + correctY + correctZ)*-1); + rightRear->Set((correctX + correctY - correctZ)); + leftRear->Set((-correctX + correctY + correctZ)*-1); } diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp index a4637ee..d32943b 100644 --- a/Subsystems/Elevator.cpp +++ b/Subsystems/Elevator.cpp @@ -1,7 +1,7 @@ #include "Elevator.h" #include "../RobotMap.h" Elevator::Elevator()/* : PIDSubsystem("Elevator", kP_real, kI_real, 0.0)*/{ - motor=new CANTalon(0); + motor=new CANTalon(ELEVATOR_CAN); elevatorEncoder=new Encoder(0,1,false); offset=0; height=0;