From 43dd7c2676d1598d8d1a847be7ea051a158781cd Mon Sep 17 00:00:00 2001 From: Ben Short Date: Thu, 18 Feb 2016 11:43:16 -0500 Subject: [PATCH 1/5] Autonomous Mode Started Can select different autonomous programs from the smartdashboard. The programs don't do anything yet. --- src/AutonomousMode.h | 25 +++++++++++++++++++++++++ src/ModeNum.h | 17 +++++++++++++++++ src/Robot.cpp | 30 ++++++++++++++++++++++++++++++ 3 files changed, 72 insertions(+) create mode 100644 src/AutonomousMode.h create mode 100644 src/ModeNum.h diff --git a/src/AutonomousMode.h b/src/AutonomousMode.h new file mode 100644 index 0000000..cce1409 --- /dev/null +++ b/src/AutonomousMode.h @@ -0,0 +1,25 @@ +/* + * AutonomousMode.h + * + * Created on: Feb 18, 2016 + * Author: Benjamin + */ + +#ifndef AUTONOMOUSMODE_H +#define AUTONOMOUSMODE_H + + +class AutonomousMode{ + +private: + +public: + int slotnum; + enum slot{slot1, slot2, slot3}; + + AutonomousMode(int slot): slotnum(slot){}; +}; + + + +#endif /* SRC_AUTONOMOUSMODES_H_ */ diff --git a/src/ModeNum.h b/src/ModeNum.h new file mode 100644 index 0000000..5d167f2 --- /dev/null +++ b/src/ModeNum.h @@ -0,0 +1,17 @@ +/* + * ModeNum.h + * + * Created on: Feb 18, 2016 + * Author: Benjamin + */ + +#ifndef MODENUM_H +#define MODENUM_H + +enum slot{ + slot1, slot2, slot3 +}; + + + +#endif /* SRC_MODENUM_H_ */ diff --git a/src/Robot.cpp b/src/Robot.cpp index 1837b27..becf47d 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -1,7 +1,10 @@ +#include #include "WPILib.h" #include "XboxController.h" #include "Subsystems/Shooter.h" #include "Subsystems/BallCollector.h" +#include "ModeNum.h" +#include class Robot: public IterativeRobot { private: @@ -15,6 +18,12 @@ class Robot: public IterativeRobot { std::unique_ptr shooter; std::unique_ptr ball_collector; + // Chooosing an autonomous program + SendableChooser* chooser; + int slot_number; + + std::chrono::milliseconds ms; + void RobotInit() { // Robot robot_drive = std::unique_ptr (new RobotDrive(1,2,3,4)); @@ -25,6 +34,12 @@ class Robot: public IterativeRobot { // Subsystems shooter = std::unique_ptr (new Shooter()); ball_collector = std::unique_ptr (new BallCollector()); + + // Choosing an autonomous program + chooser = new SendableChooser(); + chooser->AddDefault("Low Bar", new AutonomousMode(0)); + chooser->AddObject("Not Low Bar", new AutonomousMode(1)); + SmartDashboard::PutData("Autonomous modes:", chooser); } void DisabledInit() { @@ -35,6 +50,21 @@ class Robot: public IterativeRobot { } void AutonomousInit() { + ms = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch() + ); + slot_number = ((AutonomousMode*) chooser->GetSelected())->slotnum; + switch(slot_number){ + case slot1: { + + } + case slot2: { + + } + case slot3: { + + } + } } From 5b7b27762b0f51db1f41fd5e1af22cfe5e452406 Mon Sep 17 00:00:00 2001 From: Ben Short Date: Fri, 19 Feb 2016 09:50:16 -0500 Subject: [PATCH 2/5] Stuff --- src/AutonomousMode.h | 1 - src/ModeNum.h | 2 +- src/Robot.cpp | 12 +++++++++--- src/Subsystems/DriveTrain.h | 2 ++ 4 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/AutonomousMode.h b/src/AutonomousMode.h index cce1409..edb96e9 100644 --- a/src/AutonomousMode.h +++ b/src/AutonomousMode.h @@ -15,7 +15,6 @@ class AutonomousMode{ public: int slotnum; - enum slot{slot1, slot2, slot3}; AutonomousMode(int slot): slotnum(slot){}; }; diff --git a/src/ModeNum.h b/src/ModeNum.h index 5d167f2..c9dee4d 100644 --- a/src/ModeNum.h +++ b/src/ModeNum.h @@ -9,7 +9,7 @@ #define MODENUM_H enum slot{ - slot1, slot2, slot3 + slot1, slot2, slot3, slot4, slot5 }; diff --git a/src/Robot.cpp b/src/Robot.cpp index becf47d..031b68c 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -56,13 +56,19 @@ class Robot: public IterativeRobot { slot_number = ((AutonomousMode*) chooser->GetSelected())->slotnum; switch(slot_number){ case slot1: { - + break; } case slot2: { - + break; } case slot3: { - + break; + } + case slot4: { + break; + } + case slot5: { + break; } } diff --git a/src/Subsystems/DriveTrain.h b/src/Subsystems/DriveTrain.h index 54df8c7..2a3ffcf 100644 --- a/src/Subsystems/DriveTrain.h +++ b/src/Subsystems/DriveTrain.h @@ -13,6 +13,8 @@ class Drivetrain: public Subsystem { Drivetrain(RobotDrive *drive); void InitDefaultCommand(); void drive(float lSpeed, float rSpeed); + void drive(float distance); + void turn(float angle); }; From 14272994c927358136e431cc0278fd3216e8560b Mon Sep 17 00:00:00 2001 From: Ben Short Date: Fri, 19 Feb 2016 10:01:31 -0500 Subject: [PATCH 3/5] Merged from upstream --- src/Robot.cpp | 53 ++++++++++++++++++++++++----------- src/Subsystems/DriveTrain.cpp | 34 +++++++++++++++++++--- src/Subsystems/DriveTrain.h | 21 +++++++++----- 3 files changed, 80 insertions(+), 28 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index 031b68c..d0feb04 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -1,6 +1,10 @@ #include #include "WPILib.h" +#include + +#include "Vector.h" #include "XboxController.h" + #include "Subsystems/Shooter.h" #include "Subsystems/BallCollector.h" #include "ModeNum.h" @@ -8,13 +12,11 @@ class Robot: public IterativeRobot { private: - // Robot - std::unique_ptr robot_drive; - // Input devices std::unique_ptr controller; // Subsystems + std::unique_ptr drive_train; std::unique_ptr shooter; std::unique_ptr ball_collector; @@ -25,13 +27,11 @@ class Robot: public IterativeRobot { std::chrono::milliseconds ms; void RobotInit() { - // Robot - robot_drive = std::unique_ptr (new RobotDrive(1,2,3,4)); - // Input devices controller = std::unique_ptr(new XboxController(0)); // Subsystems + drive_train = std::unique_ptr (new Drivetrain()); shooter = std::unique_ptr (new Shooter()); ball_collector = std::unique_ptr (new BallCollector()); @@ -42,13 +42,27 @@ class Robot: public IterativeRobot { SmartDashboard::PutData("Autonomous modes:", chooser); } + /** + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when + * the robot is disabled. + */ void DisabledInit() { } void DisabledPeriodic() { - + Scheduler::GetInstance()->Run(); } + /** + * This autonomous (along with the chooser code above) shows how to select between different autonomous modes + * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW + * Dashboard, remove all of the chooser code and uncomment the GetString code to get the auto name from the text box + * below the Gyro + * + * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example) + * or additional comparisons to the if-else structure below with additional strings & commands. + */ void AutonomousInit() { ms = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch() @@ -71,33 +85,38 @@ class Robot: public IterativeRobot { break; } } - } void AutonomousPeriodic() { - + Scheduler::GetInstance()->Run(); } void TeleopInit() { - + controller->Calibrate(); } void TeleopPeriodic() { + // Calibrate + if(controller->GetButton(controller->ButtonRightJoystickPress)) { + controller->Calibrate(); + } + // Chases drive - std::unique_ptr left_stick_vector = std::unique_ptr(controller->GetLeftVector()); - std::unique_ptr right_stick_vector = std::unique_ptr(controller->GetRightVector()); + std::unique_ptr left_stick_vector = std::unique_ptr(controller->GetLeftStickVector()); + std::unique_ptr right_stick_vector = std::unique_ptr(controller->GetRightStickVector()); - robot_drive->TankDrive(left_stick_vector->magnitude, right_stick_vector->magnitude, false); + drive_train->TankDrive(left_stick_vector->GetMagnitude(true), + right_stick_vector->GetMagnitude(true)); // Shooter control - if (controller->GetTrigger(controller->RightTrigger,controller->RightTriggerOffset) >= 0.5) { + if (controller->GetRightTrigger() >= 0.5) { shooter->run_shooter(); - } else { + }else{ shooter->stop_shooter(); } // Ball Collector control - if (controller->GetTrigger(controller->LeftTrigger,controller->LeftTriggerOffset) >= 0.5) { + if (controller->GetLeftTrigger() >= 0.5) { ball_collector->Start(); } else { ball_collector->Stop(); @@ -105,7 +124,7 @@ class Robot: public IterativeRobot { } void TestPeriodic() { - + LiveWindow::GetInstance()->Run(); } }; diff --git a/src/Subsystems/DriveTrain.cpp b/src/Subsystems/DriveTrain.cpp index 50bf31a..b94404e 100644 --- a/src/Subsystems/DriveTrain.cpp +++ b/src/Subsystems/DriveTrain.cpp @@ -1,8 +1,26 @@ #include #include "../RobotMap.h" -Drivetrain::Drivetrain(RobotDrive *drive): Subsystem("Drivetrain") { - robot = drive; +Drivetrain::Drivetrain(): Subsystem("Drivetrain") { + // Motors init + left_primary_motor = std::shared_ptr (new CANTalon(RobotMap::LEFT_PRIMARY_MOTOR)); + left_secondary_motor = std::shared_ptr (new CANJaguar(RobotMap::LEFT_SECONDARY_MOTOR)); + + right_primary_motor = std::shared_ptr (new CANTalon(RobotMap::RIGHT_PRIMARY_MOTOR)); + right_secondary_motor = std::shared_ptr (new CANJaguar(RobotMap::RIGHT_SECONDARY_MOTOR)); + + // Motors sync groups + left_primary_motor->Set(0, LEFT_SYNC_GROUP); + left_secondary_motor->Set(0, LEFT_SYNC_GROUP); + + right_primary_motor->Set(0, RIGHT_SYNC_GROUP); + right_primary_motor->Set(0, RIGHT_SYNC_GROUP); + + // Robot Drive init + robot_drive = std::shared_ptr ( + new RobotDrive(left_primary_motor, left_secondary_motor, + right_primary_motor, right_secondary_motor) + ); } void Drivetrain::InitDefaultCommand() { @@ -10,7 +28,15 @@ void Drivetrain::InitDefaultCommand() { //SetDefaultCommand(new MySpecialCommand()); } -void Drivetrain::drive(float lSpeed, float rSpeed) { +void Drivetrain::TankDrive(float lSpeed, float rSpeed) { //should be constantly getting speed inputs from the controller - robot->SetLeftRightMotorOutputs(lSpeed, rSpeed); + robot_drive->TankDrive(lSpeed, rSpeed, true); +} + +void Drivetrain::ArcadeDrive(float magnitude, float rotate) { + robot_drive->ArcadeDrive(magnitude, rotate, true); +} + +void Drivetrain::DriveDistance(float distance) { + //left_primary_motor->GetEncVel() * left_primary_motor->GetNativeUnitsPerRotationScalar(CANTalon::FeedbackDevice::QuadEncoder); } diff --git a/src/Subsystems/DriveTrain.h b/src/Subsystems/DriveTrain.h index 2a3ffcf..db240c6 100644 --- a/src/Subsystems/DriveTrain.h +++ b/src/Subsystems/DriveTrain.h @@ -5,17 +5,24 @@ #include "WPILib.h" class Drivetrain: public Subsystem { - private: - RobotDrive *robot; - + const int LEFT_SYNC_GROUP = 0; + const int RIGHT_SYNC_GROUP = 1; public: - Drivetrain(RobotDrive *drive); + std::shared_ptr left_primary_motor; + std::shared_ptr left_secondary_motor; + + std::shared_ptr right_primary_motor; + std::shared_ptr right_secondary_motor; + + std::shared_ptr robot_drive; + + Drivetrain(); void InitDefaultCommand(); - void drive(float lSpeed, float rSpeed); - void drive(float distance); - void turn(float angle); + void TankDrive(float lSpeed, float rSpeed); + void ArcadeDrive(float magnitude, float rotate); + void DriveDistance(float distance); }; #endif From 507faed695ba38dcd4088d627bb94f90cdcb08e1 Mon Sep 17 00:00:00 2001 From: Ben Short Date: Fri, 19 Feb 2016 11:41:09 -0500 Subject: [PATCH 4/5] Autonomous Mode Movement finished --- src/Robot.cpp | 30 +++++++++++++----------------- src/Subsystems/DriveTrain.h | 1 + 2 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index d0feb04..5dc5c1c 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -26,6 +26,11 @@ class Robot: public IterativeRobot { std::chrono::milliseconds ms; + float distance1[5] = {22.20244104688751*12, 24.60806716851051*12, 17.39119351980729*12, 18.61805812566858*12, 25.83493649053858*12}; + float angle1[5] = {0, 0, -M_PI/6, M_PI/6, 0}; + float distance2[5] = {0, 0, 12*25/3, 12*25/3, 0}; + float angle2[5] = {M_PI/3, M_PI/3, M_PI/2, -M_PI/2, -M_PI/3}; + void RobotInit() { // Input devices controller = std::unique_ptr(new XboxController(0)); @@ -68,23 +73,14 @@ class Robot: public IterativeRobot { std::chrono::system_clock::now().time_since_epoch() ); slot_number = ((AutonomousMode*) chooser->GetSelected())->slotnum; - switch(slot_number){ - case slot1: { - break; - } - case slot2: { - break; - } - case slot3: { - break; - } - case slot4: { - break; - } - case slot5: { - break; - } - } + drive_train->DriveDistance(distance1[slot_number]); + drive_train->TurnAngle(angle1[slot_number]); + drive_train->DriveDistance(distance2[slot_number]); + drive_train->TurnAngle(angle2[slot_number]); + shooter->run_shooter(); + //wait some time + //put ball into shooter + shooter->stop_shooter(); } void AutonomousPeriodic() { diff --git a/src/Subsystems/DriveTrain.h b/src/Subsystems/DriveTrain.h index db240c6..f9a270a 100644 --- a/src/Subsystems/DriveTrain.h +++ b/src/Subsystems/DriveTrain.h @@ -23,6 +23,7 @@ class Drivetrain: public Subsystem { void ArcadeDrive(float magnitude, float rotate); void DriveDistance(float distance); + void TurnAngle(float angle); }; #endif From cc789c6c2b39cf19c4ffa1448c845262f35b1220 Mon Sep 17 00:00:00 2001 From: Ben Short Date: Fri, 19 Feb 2016 12:50:19 -0500 Subject: [PATCH 5/5] Attempt to time the shooter --- src/Robot.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/Robot.cpp b/src/Robot.cpp index 5dc5c1c..5958a9c 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -42,8 +42,11 @@ class Robot: public IterativeRobot { // Choosing an autonomous program chooser = new SendableChooser(); - chooser->AddDefault("Low Bar", new AutonomousMode(0)); - chooser->AddObject("Not Low Bar", new AutonomousMode(1)); + chooser->AddDefault("Slot 1", new AutonomousMode(0)); + chooser->AddObject("Slot 2", new AutonomousMode(1)); + chooser->AddObject("Slot 3", new AutonomousMode(2)); + chooser->AddObject("Slot 4", new AutonomousMode(3)); + chooser->AddObject("Slot 5", new AutonomousMode(4)); SmartDashboard::PutData("Autonomous modes:", chooser); } @@ -69,17 +72,24 @@ class Robot: public IterativeRobot { * or additional comparisons to the if-else structure below with additional strings & commands. */ void AutonomousInit() { - ms = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch() - ); + slot_number = ((AutonomousMode*) chooser->GetSelected())->slotnum; drive_train->DriveDistance(distance1[slot_number]); drive_train->TurnAngle(angle1[slot_number]); drive_train->DriveDistance(distance2[slot_number]); drive_train->TurnAngle(angle2[slot_number]); shooter->run_shooter(); - //wait some time - //put ball into shooter + ms = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch() + ); + while (!ms.count()+500Start(); + while (!ms.count()+1000Stop(); shooter->stop_shooter(); }