diff --git a/.gitignore b/.gitignore index 2af2145..ebc65cf 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ *.o -Makefile -wpilib +Debug bin +wpilib diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index f585686..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "hhlib"] - path = hhlib - url = git@github.com:team2059/hhlib.git diff --git a/CommandBase.cpp b/CommandBase.cpp new file mode 100644 index 0000000..c4defc3 --- /dev/null +++ b/CommandBase.cpp @@ -0,0 +1,22 @@ +#include "CommandBase.h" +#include "Subsystems/Drivetrain.h" +#include "Subsystems/Collector.h" +#include "Subsystems/Elevator.h" +#include "Subsystems/BinElevator.h" +Drivetrain* CommandBase::drivetrain = NULL; +Collector* CommandBase::collector = NULL; +Elevator* CommandBase::elevator = NULL; +BinElevator* CommandBase::binElevator = NULL; +OI* CommandBase::oi = NULL; +CommandBase::CommandBase(char const *name) : Command(name) { +} +CommandBase::CommandBase() : Command() { +} +void CommandBase::init(){ + drivetrain = new Drivetrain(); + collector = new Collector(); + elevator = new Elevator(); + binElevator = new BinElevator(); + oi = new OI(); +} +// vim: ts=2:sw=2:et diff --git a/CommandBase.h b/CommandBase.h new file mode 100644 index 0000000..7443ebd --- /dev/null +++ b/CommandBase.h @@ -0,0 +1,24 @@ +#ifndef COMMAND_BASE_H +#define COMMAND_BASE_H + +#include +#include "Subsystems/Drivetrain.h" +#include "Subsystems/Collector.h" +#include "Subsystems/Elevator.h" +#include "Subsystems/BinElevator.h" +#include "OI.h" +#include "WPILib.h" + +class CommandBase: public Command { + public: + CommandBase(char const *name); + CommandBase(); + static void init(); + static Drivetrain *drivetrain; + static Collector *collector; + static Elevator *elevator; + static BinElevator *binElevator; + static OI *oi; +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Autonomous/AutoDrive.cpp b/Commands/Autonomous/AutoDrive.cpp new file mode 100644 index 0000000..4d46525 --- /dev/null +++ b/Commands/Autonomous/AutoDrive.cpp @@ -0,0 +1,22 @@ +#include "AutoDrive.h" +#include "../../DentRobot.h" +// Drive for a short while then stop. Just for testing +AutoDrive::AutoDrive() : Command("AutoDrive"){ + Requires(DentRobot::drivetrain); + 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.0,-.75,0.0,0.9,0.0); +} +bool AutoDrive::IsFinished(){ + return IsTimedOut(); +} +void AutoDrive::End(){ +} +void AutoDrive::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Autonomous/AutoDrive.h b/Commands/Autonomous/AutoDrive.h new file mode 100644 index 0000000..26f2aa3 --- /dev/null +++ b/Commands/Autonomous/AutoDrive.h @@ -0,0 +1,19 @@ +#ifndef AUTODRIVE_H +#define AUTODRIVE_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class AutoDrive: public Command{ + public: + AutoDrive(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Autonomous/Autonomous.cpp b/Commands/Autonomous/Autonomous.cpp new file mode 100644 index 0000000..bc64654 --- /dev/null +++ b/Commands/Autonomous/Autonomous.cpp @@ -0,0 +1,22 @@ +#include "Autonomous.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 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/Autonomous.h b/Commands/Autonomous/Autonomous.h new file mode 100644 index 0000000..43ca69c --- /dev/null +++ b/Commands/Autonomous/Autonomous.h @@ -0,0 +1,14 @@ +#ifndef AUTONOMOUS_H +#define AUTONOMOUS_H + +#include "Commands/CommandGroup.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class Autonomous: public CommandGroup{ + public: + Autonomous(); +}; +#endif +// 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/BinElevator/BinLower.cpp b/Commands/BinElevator/BinLower.cpp new file mode 100644 index 0000000..158f98a --- /dev/null +++ b/Commands/BinElevator/BinLower.cpp @@ -0,0 +1,26 @@ +#include "BinLower.h" +#include "../../DentRobot.h" +#include "../../OI.h" +BinLower::BinLower() : Command("BinLower"){ +} +void BinLower::Initialize(){ + SetTimeout(2.5); +} +void BinLower::Execute(){ + DentRobot::binElevator->Run(-1.0); +} +bool BinLower::IsFinished(){ + if (!DentRobot::binElevator->GetElevatorBottom()||IsTimedOut()){ + printf("Robot stoped BinLowering. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorBottom()); + return true; + }else{ + return false; + } +} +void BinLower::End(){ + DentRobot::binElevator->Run(0.0f); +} +void BinLower::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/BinElevator/BinLower.h b/Commands/BinElevator/BinLower.h new file mode 100644 index 0000000..a3504b4 --- /dev/null +++ b/Commands/BinElevator/BinLower.h @@ -0,0 +1,17 @@ +#ifndef BINLOWER_H +#define BINLOWER_H + +#include "Commands/Command.h" +#include "WPILib.h" + +class BinLower: public Command{ + public: + BinLower(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/BinElevator/BinRaise.cpp b/Commands/BinElevator/BinRaise.cpp new file mode 100644 index 0000000..aa765a4 --- /dev/null +++ b/Commands/BinElevator/BinRaise.cpp @@ -0,0 +1,26 @@ +#include "BinRaise.h" +#include "../../DentRobot.h" +#include "../../OI.h" +BinRaise::BinRaise() : Command("BinRaise"){ +} +void BinRaise::Initialize(){ + SetTimeout(3.0); +} +void BinRaise::Execute(){ + DentRobot::binElevator->Run(1.0); +} +bool BinRaise::IsFinished(){ + if (!DentRobot::binElevator->GetElevatorTop()||IsTimedOut()){ + printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::binElevator->GetElevatorTop()); + return true; + }else{ + return false; + } +} +void BinRaise::End(){ + DentRobot::binElevator->Run(0.0f); +} +void BinRaise::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/BinElevator/BinRaise.h b/Commands/BinElevator/BinRaise.h new file mode 100644 index 0000000..ea73454 --- /dev/null +++ b/Commands/BinElevator/BinRaise.h @@ -0,0 +1,18 @@ +#ifndef BINRAISE_H +#define BINRAISE_H + +#include "Commands/Command.h" +#include "WPILib.h" + +class BinRaise: public Command{ + public: + BinRaise(); + 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 new file mode 100644 index 0000000..2418732 --- /dev/null +++ b/Commands/Collector/CloseCollector.cpp @@ -0,0 +1,28 @@ +#include "CloseCollector.h" +CloseCollector::CloseCollector() : Command("CloseCollector"){ + Requires(DentRobot::collector); +} +void CloseCollector::Initialize(){ + printf("Initialized collector: 0.5\n"); + SetTimeout(2.5); +} +void CloseCollector::Execute(){ + //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(){ + 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); +} +void CloseCollector::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/CloseCollector.h b/Commands/Collector/CloseCollector.h new file mode 100644 index 0000000..cd26268 --- /dev/null +++ b/Commands/Collector/CloseCollector.h @@ -0,0 +1,20 @@ +#ifndef CLOSECOLLECTOR_H +#define CLOSECOLLECTOR_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class CloseCollector: public Command{ + public: + CloseCollector(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; + +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/CollectTote.cpp b/Commands/Collector/CollectTote.cpp new file mode 100644 index 0000000..acabd4e --- /dev/null +++ b/Commands/Collector/CollectTote.cpp @@ -0,0 +1,22 @@ +#include "CollectTote.h" +CollectTote::CollectTote() : Command("CollectTote"){ + Requires(DentRobot::collector); +} +void CollectTote::Initialize(){ + printf("Initialized CollectTote\n"); + SetTimeout(2.0); +} +void CollectTote::Execute(){ + //TODO check this value to move the motors in the right direction + DentRobot::collector->MoveRollers(-(-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2); +} +bool CollectTote::IsFinished(){ + return DentRobot::collector->BoxCollected()||IsTimedOut(); +} +void CollectTote::End(){ + DentRobot::collector->MoveRollers(0.0); +} +void CollectTote::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/CollectTote.h b/Commands/Collector/CollectTote.h new file mode 100644 index 0000000..03074fd --- /dev/null +++ b/Commands/Collector/CollectTote.h @@ -0,0 +1,20 @@ +#ifndef COLLECTTOTE_H +#define COLLECTTOTE_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class CollectTote: public Command{ + public: + CollectTote(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; + +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/OpenCollector.cpp b/Commands/Collector/OpenCollector.cpp new file mode 100644 index 0000000..5cebc3e --- /dev/null +++ b/Commands/Collector/OpenCollector.cpp @@ -0,0 +1,22 @@ +#include "OpenCollector.h" +OpenCollector::OpenCollector() : Command("OpenCollector"){ + Requires(DentRobot::collector); +} +void OpenCollector::Initialize(){ + SetTimeout(0.5); +} +void OpenCollector::Execute(){ + 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 IsTimedOut(); +} +void OpenCollector::End(){ + DentRobot::collector->MoveArms(0.0f); +} +void OpenCollector::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/OpenCollector.h b/Commands/Collector/OpenCollector.h new file mode 100644 index 0000000..2686251 --- /dev/null +++ b/Commands/Collector/OpenCollector.h @@ -0,0 +1,20 @@ +#ifndef OPENCOLLECTOR_H +#define OPENCOLLECTOR_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class OpenCollector: public Command{ + public: + OpenCollector(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; + +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/ReleaseTote.cpp b/Commands/Collector/ReleaseTote.cpp new file mode 100644 index 0000000..744efcb --- /dev/null +++ b/Commands/Collector/ReleaseTote.cpp @@ -0,0 +1,22 @@ +#include "ReleaseTote.h" +ReleaseTote::ReleaseTote() : Command("ReleaseTote"){ + Requires(DentRobot::collector); +} +void ReleaseTote::Initialize(){ + SetTimeout(2.0); +} +void ReleaseTote::Execute(){ + //TODO check this value to move the motors in the right direction + // Devide by 2 twice because this speed should be half the collector speed + DentRobot::collector->MoveRollers((-DentRobot::oi->GetLeftStick()->GetRawAxis(3)+1.0)/2/2); +} +bool ReleaseTote::IsFinished(){ + return DentRobot::collector->BoxCollected(); +} +void ReleaseTote::End(){ + DentRobot::collector->MoveRollers(0.0f); +} +void ReleaseTote::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Collector/ReleaseTote.h b/Commands/Collector/ReleaseTote.h new file mode 100644 index 0000000..722680f --- /dev/null +++ b/Commands/Collector/ReleaseTote.h @@ -0,0 +1,20 @@ +#ifndef RELEASETOTE_H +#define RELEASETOTE_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class ReleaseTote: public Command{ + public: + ReleaseTote(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; + +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Drivetrain/Drive.cpp b/Commands/Drivetrain/Drive.cpp new file mode 100644 index 0000000..3b91bae --- /dev/null +++ b/Commands/Drivetrain/Drive.cpp @@ -0,0 +1,31 @@ +#include "Drive.h" +#include "../../DentRobot.h" +Drive::Drive() : Command("Drive"){ + Requires(DentRobot::drivetrain); +} +void Drive::Initialize(){ +} +void Drive::Execute(){ + double x,y,z; + //Code to lock the x axis when not holding button 1 + 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; + //} + //X axis, Y axis, Z axis, sensitivity, speed threshold (usually throttle), gyro + DentRobot::drivetrain->DriveMecanum(x,y,z,0.9,0); +} +bool Drive::IsFinished(){ + return false; +} +void Drive::End(){ +} +void Drive::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Drivetrain/Drive.h b/Commands/Drivetrain/Drive.h new file mode 100644 index 0000000..17e0ab8 --- /dev/null +++ b/Commands/Drivetrain/Drive.h @@ -0,0 +1,19 @@ +#ifndef DRIVE_H +#define DRIVE_H + +#include "Commands/Command.h" +#include "../../CommandBase.h" +#include "../../DentRobot.h" +#include "WPILib.h" + +class Drive: public Command{ + public: + Drive(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Elevator/Lower.cpp b/Commands/Elevator/Lower.cpp new file mode 100644 index 0000000..378156d --- /dev/null +++ b/Commands/Elevator/Lower.cpp @@ -0,0 +1,26 @@ +#include "Lower.h" +#include "../../DentRobot.h" +#include "../../OI.h" +Lower::Lower() : Command("Lower"){ +} +void Lower::Initialize(){ + SetTimeout(2.5); +} +void Lower::Execute(){ + DentRobot::elevator->Run(-1.0); +} +bool Lower::IsFinished(){ + if (!DentRobot::elevator->GetElevatorBottom()||IsTimedOut()){ + printf("Robot stoped lowering. Sensor based? %d\n", !DentRobot::elevator->GetElevatorBottom()); + return true; + }else{ + return false; + } +} +void Lower::End(){ + DentRobot::elevator->Run(0.0f); +} +void Lower::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Elevator/Lower.h b/Commands/Elevator/Lower.h new file mode 100644 index 0000000..92c8535 --- /dev/null +++ b/Commands/Elevator/Lower.h @@ -0,0 +1,17 @@ +#ifndef LOWER_H +#define LOWER_H + +#include "Commands/Command.h" +#include "WPILib.h" + +class Lower: public Command{ + public: + Lower(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Commands/Elevator/Raise.cpp b/Commands/Elevator/Raise.cpp new file mode 100644 index 0000000..6f900b4 --- /dev/null +++ b/Commands/Elevator/Raise.cpp @@ -0,0 +1,26 @@ +#include "Raise.h" +#include "../../DentRobot.h" +#include "../../OI.h" +Raise::Raise() : Command("Raise"){ +} +void Raise::Initialize(){ + SetTimeout(3.0); +} +void Raise::Execute(){ + DentRobot::elevator->Run(1.0); +} +bool Raise::IsFinished(){ + if (!DentRobot::elevator->GetElevatorTop()||IsTimedOut()){ + printf("Robot stoped raising. Sensor based? %d\n", !DentRobot::elevator->GetElevatorTop()); + return true; + }else{ + return false; + } +} +void Raise::End(){ + DentRobot::elevator->Run(0.0f); +} +void Raise::Interrupted(){ + End(); +} +// vim: ts=2:sw=2:et diff --git a/Commands/Elevator/Raise.h b/Commands/Elevator/Raise.h new file mode 100644 index 0000000..b19f470 --- /dev/null +++ b/Commands/Elevator/Raise.h @@ -0,0 +1,18 @@ +#ifndef RAISE_H +#define RAISE_H + +#include "Commands/Command.h" +#include "WPILib.h" + +class Raise: public Command{ + public: + Raise(); + void Initialize(); + void Execute(); + bool IsFinished(); + void End(); + void Interrupted(); +}; + +#endif +// vim: ts=2:sw=2:et diff --git a/DentRobot.cpp b/DentRobot.cpp new file mode 100644 index 0000000..f5eafec --- /dev/null +++ b/DentRobot.cpp @@ -0,0 +1,45 @@ +#include "DentRobot.h" +#include "Commands/Autonomous/Autonomous.h" +OI* DentRobot::oi=NULL; +Collector* DentRobot::collector=NULL; +Drivetrain* DentRobot::drivetrain=NULL; +Elevator* DentRobot::elevator=NULL; +BinElevator* DentRobot::binElevator=NULL; +CommandGroup* DentRobot::aut=NULL; +DentRobot::DentRobot(){ + oi=new OI(); + collector=new Collector(); + drivetrain=new Drivetrain(); + elevator=new Elevator(); + binElevator=new BinElevator(); + aut=new Autonomous(); + CameraServer::GetInstance()->SetQuality(25); + CameraServer::GetInstance()->StartAutomaticCapture("cam0"); + printf("Initialized"); +} +void DentRobot::RobotInit(){ + SmartDashboard::PutNumber("CodeVersion",0.001); +} +void DentRobot::DisabledPeriodic(){ + Scheduler::GetInstance()->Run(); +} +void DentRobot::AutonomousInit(){ + if(aut != NULL){ + aut->Start(); + } +} +void DentRobot::AutonomousPeriodic(){ + Scheduler::GetInstance()->Run(); +} +void DentRobot::TeleopInit(){ + //if (aut != NULL){ + // aut->Cancel(); + //} +} +void DentRobot::TeleopPeriodic(){ + Scheduler::GetInstance()->Run(); +} +void DentRobot::TestPeriodic(){ +} +START_ROBOT_CLASS(DentRobot); +// vim: ts=2:sw=2:et diff --git a/DentRobot.h b/DentRobot.h new file mode 100644 index 0000000..68b17ba --- /dev/null +++ b/DentRobot.h @@ -0,0 +1,30 @@ +#ifndef DENTROBOT_H +#define DENTROBOT_H +#include "WPILib.h" +#include "OI.h" +#include "Subsystems/Elevator.h" +#include "Subsystems/BinElevator.h" +#include "Subsystems/Drivetrain.h" +#include "Subsystems/Collector.h" +#include "Commands/Autonomous/Autonomous.h" +class DentRobot: public IterativeRobot { + private: + Command *driveCommand = NULL; + public: + DentRobot(); + static OI* oi; + static Collector* collector; + static Drivetrain* drivetrain; + static Elevator* elevator; + static BinElevator* binElevator; + static CommandGroup* aut; + void RobotInit(); + void DisabledPeriodic(); + void AutonomousInit(); + void AutonomousPeriodic(); + void TeleopInit(); + void TeleopPeriodic(); + void TestPeriodic(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..0eee4bc --- /dev/null +++ b/Makefile @@ -0,0 +1,30 @@ +REMOTEIP=10.20.59.2 +CC=arm-frc-linux-gnueabi-g++ +CFLAGS=-std=c++11 -O0 -g3 -Wall -c -fmessage-length=0 +LDFLAGS=-Wl,-rpath,/opt/GenICam_v2_3/bin/Linux_armv7-a +SOURCES=$(shell find -type f -name "*.cpp") +OBJECTS=$(SOURCES:.cpp=.o) +WPILIB=/var/frc/wpilib +EXEC=bin/FRCUserProgram +CLEANSER=rm -r + +all : $(OBJECTS) + $(CC) -L$(WPILIB)/lib $(LDFLAGS) -o $(EXEC) $(OBJECTS) -lwpi + +%.o : %.cpp + $(CC) $(CFLAGS) -I$(WPILIB)/include $^ -o $@ + +clean: + $(CLEANSER) $(OBJECTS) bin/FRCUserProgram + +deploy: + @cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram' + +debug: + @cat bin/FRCUserProgram | ssh admin@$(REMOTEIP) 'cat > /home/lvuser/FRCUserProgram2&&rm /home/lvuser/FRCUserProgram;mv /home/lvuser/FRCUserProgram2 /home/lvuser/FRCUserProgram&&. /etc/profile.d/natinst-path.sh;chmod a+x /home/lvuser/FRCUserProgram;/home/lvuser/run.sh' + +putkey: + @test -d ~/.ssh||mkdir ~/.ssh;test -f ~/.ssh/id_rsa||ssh-keygen -t rsa -f ~/.ssh/id_rsa -b 4096 -q -N '';cat ~/.ssh/id_rsa.pub|ssh -v admin@$(REMOTEIP) 'cat >> /tmp/key;mkdir -p ~/.ssh;cat /tmp/key >> ~/.ssh/authorized_keys;rm /tmp/key' + +updatemakefile: + @curl -s https://raw.githubusercontent.com/int0x191f2/nameless/master/configure.sh | sh diff --git a/OI.cpp b/OI.cpp new file mode 100644 index 0000000..54c4212 --- /dev/null +++ b/OI.cpp @@ -0,0 +1,48 @@ +#include "OI.h" +#include "Commands/Elevator/Lower.h" +#include "Commands/Elevator/Raise.h" +#include "Commands/Collector/OpenCollector.h" +#include "Commands/Collector/CloseCollector.h" +#include "Commands/Collector/CollectTote.h" +#include "Commands/Collector/ReleaseTote.h" + +OI::OI() { + // Joysticks + leftStick=new Joystick(0); + rightStick=new Joystick(1); + // Collector + JoystickButton *right1=new JoystickButton(rightStick, 1); + JoystickButton *right2=new JoystickButton(rightStick, 2); + 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(); + JoystickButton *right3=new JoystickButton(rightStick, 3); + JoystickButton *right4=new JoystickButton(rightStick, 4); + JoystickButton *right5=new JoystickButton(rightStick, 5); + JoystickButton *right6=new JoystickButton(rightStick, 6); + right3->WhenPressed(lower); + right4->WhenPressed(lower); + right3->CancelWhenPressed(raise); + right4->CancelWhenPressed(raise); + right5->WhenPressed(raise); + right6->WhenPressed(raise); + right5->CancelWhenPressed(lower); + right6->CancelWhenPressed(lower); + // Cancel + JoystickButton *right12=new JoystickButton(rightStick, 12); + right12->CancelWhenPressed(raise); + right12->CancelWhenPressed(lower); +} +Joystick* OI::GetRightStick(){ + return rightStick; +} +Joystick* OI::GetLeftStick(){ + return leftStick; +} +// vim: ts=2:sw=2:et diff --git a/OI.h b/OI.h new file mode 100644 index 0000000..7f0929f --- /dev/null +++ b/OI.h @@ -0,0 +1,16 @@ +#ifndef OI_H +#define OI_H + +#include "WPILib.h" + +class OI +{ + private: + Joystick *leftStick, *rightStick; + public: + OI(); + Joystick* GetRightStick(); + Joystick* GetLeftStick(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/RobotMap.h b/RobotMap.h new file mode 100644 index 0000000..a3abe10 --- /dev/null +++ b/RobotMap.h @@ -0,0 +1,37 @@ +#ifndef ROBOTMAP_H +#define ROBOTMAP_H + +#include "WPILib.h" + +// Elevator +#define ELEVATOR_CAN 1 +#define ELEVATOR_BOTTOM_DIO 0 +#define ELEVATOR_COLELCT_TOTE_DIO 1 +#define ELEVATOR_READY_TOTE_DIO 2 +#define ELEVATOR_TOP_DIO 5 +#define ELEVATOR_ENCODERA 0 +#define ELEVATOR_ENCODERB 1 + +// BinElevator +#define BINELEVATOR_CAN 11 +#define BINELEVATOR_BOTTOM_DIO 6 +#define BINELEVATOR_COLELCT_BIN_DIO 7 +#define BINELEVATOR_TOP_DIO 8 +#define BINELEVATOR_ENCODERA 2 +#define BINELEVATOR_ENCODERB 3 + +// Drivetrain +#define DRIVE_FRONT_LEFT_CAN 2 +#define DRIVE_BACK_LEFT_CAN 3 +#define DRIVE_FRONT_RIGHT_CAN 4 +#define DRIVE_BACK_RIGHT_CAN 5 + +// Collector +#define COLLECTOR_WINDOW_LEFT_CAN 6 +#define COLLECTOR_WINDOW_RIGHT_CAN 7 +#define COLLECTOR_LEFT_CAN 8 +#define COLLECTOR_BOTTOM_CAN 10 +#define COLLECTOR_RIGHT_CAN 9 + +#endif +// vim: ts=2:sw=2:et diff --git a/Subsystems/BinElevator.cpp b/Subsystems/BinElevator.cpp new file mode 100644 index 0000000..daff79b --- /dev/null +++ b/Subsystems/BinElevator.cpp @@ -0,0 +1,26 @@ +#include "BinElevator.h" +#include "../RobotMap.h" +BinElevator::BinElevator(){ + motor=new CANTalon(BINELEVATOR_CAN); + elevatorEncoder=new Encoder(BINELEVATOR_ENCODERA,BINELEVATOR_ENCODERB,false); + elevatorBottom=new DigitalInput(BINELEVATOR_BOTTOM_DIO); + elevatorTop=new DigitalInput(BINELEVATOR_TOP_DIO); +} +void BinElevator::InitDefaultCommand(){ +} +void BinElevator::Run(double power){ + motor->Set(power); +} +void BinElevator::ResetEncoder(){ + elevatorEncoder->Reset(); +} +double BinElevator::GetHeight(){ + return elevatorEncoder->Get(); +} +bool BinElevator::GetElevatorBottom(){ + return elevatorBottom->Get(); +} +bool BinElevator::GetElevatorTop(){ + return elevatorTop->Get(); +} +// vim: ts=2:sw=2:et diff --git a/Subsystems/BinElevator.h b/Subsystems/BinElevator.h new file mode 100644 index 0000000..8814152 --- /dev/null +++ b/Subsystems/BinElevator.h @@ -0,0 +1,22 @@ +#ifndef BINELEVATOR_H +#define BINELEVATOR_H + +#include "WPILib.h" +#include "Commands/PIDSubsystem.h" +class BinElevator{ + private: + CANTalon *motor; + Encoder *elevatorEncoder; + static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2; + DigitalInput *elevatorBottom, *elevatorTop; + public: + BinElevator(); + void InitDefaultCommand(); + void Run(double); + void ResetEncoder(); + double GetHeight(); + bool GetElevatorTop(); + bool GetElevatorBottom(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Subsystems/Collector.cpp b/Subsystems/Collector.cpp new file mode 100644 index 0000000..fcc7959 --- /dev/null +++ b/Subsystems/Collector.cpp @@ -0,0 +1,30 @@ +#include "Collector.h" +#include "../RobotMap.h" + +Collector::Collector() : Subsystem("Collector") { + windowMotorLeft=new CANTalon(COLLECTOR_WINDOW_LEFT_CAN); + windowMotorRight=new CANTalon(COLLECTOR_WINDOW_RIGHT_CAN); + collectorMotorLeft=new CANTalon(COLLECTOR_LEFT_CAN); + collectorMotorBottom=new CANTalon(COLLECTOR_BOTTOM_CAN); + collectorMotorRight=new CANTalon(COLLECTOR_RIGHT_CAN); +} +void Collector::InitDefaultCommand() { +} +void Collector::MoveArms(double a){ + windowMotorLeft->Set(a); + windowMotorRight->Set(-a); +} +void Collector::MoveRollers(double a){ + collectorMotorLeft->Set(a); + collectorMotorBottom->Set(a); + collectorMotorRight->Set(-a); +} +bool Collector::ArmSensor(){ + // TODO: include limit switch code + return false; +} +bool Collector::BoxCollected(){ + return false; + //return boxSwitch->Get(); +} +// vim: ts=2:sw=2:et diff --git a/Subsystems/Collector.h b/Subsystems/Collector.h new file mode 100644 index 0000000..92262ac --- /dev/null +++ b/Subsystems/Collector.h @@ -0,0 +1,18 @@ +#ifndef COLLECTOR_H +#define COLLECTOR_H + +#include "WPILib.h" +class Collector: public Subsystem +{ + private: + CANTalon *windowMotorLeft, *windowMotorRight, *collectorMotorLeft, *collectorMotorBottom, *collectorMotorRight; + public: + Collector(); + void InitDefaultCommand(); + void MoveArms(double); + void MoveRollers(double); + bool ArmSensor(); + bool BoxCollected(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Subsystems/Drivetrain.cpp b/Subsystems/Drivetrain.cpp new file mode 100644 index 0000000..6440679 --- /dev/null +++ b/Subsystems/Drivetrain.cpp @@ -0,0 +1,23 @@ +#include "Drivetrain.h" +#include "../RobotMap.h" +#include "../Commands/Drivetrain/Drive.h" + +Drivetrain::Drivetrain() : Subsystem("Drivetrain"){ + 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); +} +// vim: ts=2:sw=2:et diff --git a/Subsystems/Drivetrain.h b/Subsystems/Drivetrain.h new file mode 100644 index 0000000..3ab69b3 --- /dev/null +++ b/Subsystems/Drivetrain.h @@ -0,0 +1,16 @@ +#ifndef DRIVETRAIN_H +#define DRIVETRAIN_H + +#include "WPILib.h" +class Drivetrain: public Subsystem{ + private: + CANTalon *rightFront, *leftFront, *rightRear, *leftRear; + RobotDrive *drive; + public: + Drivetrain(); + void InitDefaultCommand(); + void DriveMecanum(float,float,float,float,float); + void DriveArcade(float, float); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/Subsystems/Elevator.cpp b/Subsystems/Elevator.cpp new file mode 100644 index 0000000..9264b4a --- /dev/null +++ b/Subsystems/Elevator.cpp @@ -0,0 +1,26 @@ +#include "Elevator.h" +#include "../RobotMap.h" +Elevator::Elevator(){ + motor=new CANTalon(ELEVATOR_CAN); + elevatorEncoder=new Encoder(ELEVATOR_ENCODERA,ELEVATOR_ENCODERB,false); + elevatorBottom=new DigitalInput(ELEVATOR_BOTTOM_DIO); + elevatorTop=new DigitalInput(ELEVATOR_TOP_DIO); +} +void Elevator::InitDefaultCommand(){ +} +void Elevator::Run(double power){ + motor->Set(power); +} +void Elevator::ResetEncoder(){ + elevatorEncoder->Reset(); +} +double Elevator::GetHeight(){ + return elevatorEncoder->Get(); +} +bool Elevator::GetElevatorBottom(){ + return elevatorBottom->Get(); +} +bool Elevator::GetElevatorTop(){ + return elevatorTop->Get(); +} +// vim: ts=2:sw=2:et diff --git a/Subsystems/Elevator.h b/Subsystems/Elevator.h new file mode 100644 index 0000000..bdc0c8d --- /dev/null +++ b/Subsystems/Elevator.h @@ -0,0 +1,22 @@ +#ifndef ELEVATOR_H +#define ELEVATOR_H + +#include "WPILib.h" +#include "Commands/PIDSubsystem.h" +class Elevator{ + private: + CANTalon *motor; + Encoder *elevatorEncoder; + static constexpr double kP_real=4, kI_real=.0f, kP_simulation=18, kI_simulation=.2; + DigitalInput *elevatorBottom, *elevatorTop; + public: + Elevator(); + void InitDefaultCommand(); + void Run(double); + void ResetEncoder(); + double GetHeight(); + bool GetElevatorTop(); + bool GetElevatorBottom(); +}; +#endif +// vim: ts=2:sw=2:et diff --git a/TODO.txt b/TODO.txt new file mode 100644 index 0000000..e69de29 diff --git a/classes/Collector.cpp b/classes/Collector.cpp deleted file mode 100644 index 919158c..0000000 --- a/classes/Collector.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "Collector.h" -DentCollector::DentCollector(int fl, int fr, int rl, int rr){ - collectorLeft = new Talon(fl); - collectorRight = new Talon(fr); - raiserLeft = new Talon(rl); - raiserRight = new Talon(rr); -} -void DentCollector::Collect(float power){ - collectorLeft->Set(power); - collectorRight->Set(power); -} -void DentCollector::Raise(float power){ - raiserLeft->Set(power); - raiserRight->Set(power); -} -void DentCollector::Rest(){ - raiserLeft->Set(0); - raiserRight->Set(0); - collectorLeft->Set(0); - collectorRight->Set(0); -} -// vim: ts=2:sw=2:et diff --git a/classes/Collector.h b/classes/Collector.h deleted file mode 100644 index f112d0a..0000000 --- a/classes/Collector.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef __COLLECTOR_H__ -#include -#define __COLLECTOR_H__ -//#include "" -class DentCollector{ - private: - Talon *collectorLeft, *collectorRight, *raiserLeft, *raiserRight; - public: - DentCollector(int, int, int, int); - void Collect(float); - void Raise(float); - void Rest(); -}; -#endif -// vim: ts=2:sw=2:et diff --git a/hhlib b/hhlib deleted file mode 160000 index 787733b..0000000 --- a/hhlib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 787733b3d4204d061620c7906af0cf4777c47e34