Skip to content

Commit

Permalink
Merge with tankturn branch
Browse files Browse the repository at this point in the history 
Merge with tankturn
  • Loading branch information
@wusteven815
wusteven815 authored Apr 15, 2022
2 parents 036c335 + 7eddd20 commit 56f6aec
Show file tree
Hide file tree
Showing 12 changed files with 100 additions and 91 deletions.
6 changes: 4 additions & 2 deletions src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
#include <cameraserver/CameraServer.h>

void Robot::RobotInit() {
frc::CameraServer::GetInstance()->StartAutomaticCapture(0);
frc::CameraServer::GetInstance()->StartAutomaticCapture(1);
//frc::CameraServer::GetInstance()->StartAutomaticCapture(0);
//frc::CameraServer::GetInstance()->StartAutomaticCapture(1);
frc::RobotController::SetBrownoutVoltage(6_V);
}

Expand Down Expand Up @@ -45,6 +45,7 @@ void Robot::DisabledPeriodic() {}
*/

void Robot::AutonomousInit() {
//m_container.m_drivetrain.SetIdleMode(rev::CANSparkMax::IdleMode::kBrake);

m_autonomousCommand = m_container.GetAutonomousCommand();

Expand All @@ -57,6 +58,7 @@ void Robot::AutonomousInit() {
void Robot::AutonomousPeriodic() {}

void Robot::TeleopInit() {
//m_container.m_drivetrain.SetIdleMode(rev::CANSparkMax::IdleMode::kCoast);

// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
Expand Down
74 changes: 6 additions & 68 deletions src/main/cpp/RobotContainer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,16 @@

// Testing
#include "commands/TankTurn.h"
#include "commands/common/GrabBall.h"
#include "commands/common/ScoreBall.h"
#include "commands/common/ArmLower.h"
#include "commands/TankMoveGyro.h"

RobotContainer::RobotContainer() {

m_chooser.SetDefaultOption("Only Taxi", &m_only_taxi);
m_chooser.AddOption("2 Ball", &m_ball2);
m_chooser.AddOption("2 Ball b", &m_ball2b);
m_chooser.AddOption("3 Ball", &m_ball3);

frc::SmartDashboard::PutData(&m_chooser);
Expand Down Expand Up @@ -90,54 +95,7 @@ RobotContainer::RobotContainer() {
)
);

/* Me reading the above code:
&&&&&&&&&&&&&##BGPPPGBB#BBGBGJ???J5BB#BBB###BBPYY5Y5B#&&&&GY5PGGGYJYGGPG&&&&&&#BJ:.:?B##BGGGGPY!JGPP
&&###BBBGG##&&##BGGG##BGGBBBG?7???5#&#BBGG###BPYYYP#&##&&&BY5GGGPYJ5GBPB&&#BGPP57. .7GBBGPPPPPY!JPPG
&&#BGPPPGB####&&######BBBBBBG?7?7?5#&#GB#BG##G5GGG&&#GP!5&BYPGBGPYYGBP?7!~~!~77?77!~~^~~~!????7~!???
#&&&#BGB#&&&&#&&&##B########G?!7775##BB##GB##GB&&&&&#!. J@BYPBBGPY5J~^!JP#&@@@@@@@@@@#B5?^:~J5J!?5PP
##&&###&&&&&&&&#BGGB#######BP?!77?5BBGGGBB#GG#&#&&&&B: ?&BYGBBG5!.^5&@@@@@@@@@@@@@@@@@@@@B7.~J7YBBB
#BBBBPG#######BGPGB###BBBGGGBB#BB##########B&&&#&&&&#J. ^B&GGB#5::P@@@@@@@BY7!!!~^!?YB@@@@@@#! ^YB#B
G###GG#&&###BGGGPG#BBBB###&&&&&BB&&&&&&##&&&&&&&&&&&&#J.~##PB#P.~&@@@@@@G~:?5G#B?. .::~G@@@@@@J ?###
!.!?JJ5#&&&&&&&######&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&B#PJ#G5B#^.&@@@@@@5 7#@&&&#Y:.:75! P@@@@@@^.B##
#~ :~?P#&&&&#&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&#&&&&&&&&#BGG5JBB::!??J5P5 7&&&##&#5:.^7GP J@@@@@@? GBB
&G: 7YYP#&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&###&#BBGP##B5Y55YJ7~!#&&&&#&#5^.^?J.~#@@@@@@~.PBB
&&P: ^5BBB#&&&&&&&&&&&&&&&&&&&&&&&&&&##&&&&&&&&&&&&&##&&BPG#&BGB&@@&#GB&&&#BBBGY^.:.:J&@@@@@@J !GGG
#&&G! .P#BB###&&##&&&&&&&&&&&&&&#B&&&&&##&&&&&&&&&&&&&B#&&#G#&BGB&&#BG5GBBGPYJJJ!. :J#@@@@@@#! ^7???
##&&#J.Y&#BBBGB&##&&&&&#B#&&&&&&&G#&&&###&&&&&#&&##&&&#B#&&#BBBGB&&&&#GB&&#BBBP!.!5&@@@@@@#?.~J!?YYY
&&#BB#PYG###BBG#B#&&&&&&B#&&&&&&&GB&###&##&&&&&#B#B#&&&#B&###GGG#&&&&#GB&&&#B7:7B@@@@@@@B7:~YGY!?555
&&&#PG##PJYGB##BG#&&&&&###&&&&&&&B5##B#&&#&&&&&&GBGGB###B##B##PG&&&&&#GB&&&G::G@@@@@@&5~:75GBBY!JPGG
&&&#G5P#&#PJ7YGBGGB########&&&&###5PGGG&&&&&&&&&GB#BB#&&&G##B#BB#&&#&#GB&&#^:#@@@@@B7:!YB#GPBGY!JPPP
&&&#GPG#&&&&#GGBGGG####&&&GB&####BGGGGPB#&&&&&&#GBB#BB&&&GP##B#BB&#&&#GB&@Y 5@@@@@G :P&&#BGPGGY!JPPP
&&&#PGBGBB#&&&&&B#B####&&&BP#&&&&GGB#BGPG###&&&BGBB#GG#&##GG&BBGG###&#PB&&? &@@@@@J !B&&&&#BBGY!JPPP
&&&#5GBB#BBB#&&&######&&&&&PPB&&&PPBBGPPGB#####G###57!?YJ5PPGGGBPB###BPG#&J.777777^ 7B@@@&&&&#Y!JPGG
&&&#5GGG&##BBBBBBB&G#&&&&&&BGBBGBY7!^::^YB##BBBB###P7!^~7Y?PBGGBBGB#BGPPBBPJ????!. .!5GGGGGGGPJ!?5PP
&&&#PPGB&BBB#&##BG&5B&&&&&&#GBP!Y5?7!!J7J####BB#BB##5Y5YJ#PPB#BG#BBBGG55GG7 !777!!!^ 7JJJJ??77!^!777
&&&#PGGB&BBB#&&#&B&55&&&&&&&BGB#B#5Y5P5YG############BGPB#PPBG#BBGG&&#GG#&? &@@@@@@J PBBBBBGGPJ!JPPP
&&&#GGGB###B##&&&##GPB#&&&&&&GG#&##BGGBB##############BBBBG5BB#BGGG###GG#&? &@@@@@@J P#BBGGGBGY!JGGG
&&&#BGGB5#####&&#5BBPB##&&&&&BGB##BBBBBBB#################BPG#&GBGGB&#GB#&? B#####&? 5GGGPPGBBY!YB##
&&&##BGBP#&####&PJGP5P###&@&&&GG###################&#######P5BB##BBB&#GB##Y~!!!!~:..:5BBBBBGGGY!Y#&&
&&&#&#GGB#&#BB#GJY5P55G##&@&&&#P#############GPPPP5PB######P5GGBGGBG##GB####BBBGY^.:7PBBB###&#Y!JB##
&&#&##GGBB&#BBBYY55PP55###&&&#&B###########&B5PPGGP5G#####G55GBBBBGG&#GB&&#GGGGPY^.:!5GBBBB###Y7JB#&
####&#PBBB&#BBPY5P55PG5B&#&&&BB&B#######################BGPP5PB##GPGGGPPBBPYJJJJ7:.:~????JJJJJ7~7Y55
######5BBB&##GY55PP5PGGPB&&&&#P#G5B###&&##############BPPGP555#BBGBGBBGB##BGGGBGY:.:!YPPPPPP55?~7Y5Y
PPPGGPYBBG&&&P555PPP5PPPPB&&&&GPB?JY55PGBB#####&####BP55GPP555&#55GG##B#&&###&##5^.:!5###&&&&#5!Y&&&
YYYJJY?YYG&&G5555PPPYYPP5P#&&&#5BYJYYYYYYYYY555PPP555PPPGP555P&&GPGB##B#&&&###&#5^.:!P##&&&&&#5!Y#&&
YYJJ?J??JG&BP555Y5PPY?5PPYP&&&&PPGJYYYYYYYYYYYYJ55Y5PP5PG555PP&#BBBB&#B#&&&&##&&5^.:7G&&&&&&&#Y~Y#&&
JJJJ?J5GBB#5GP5P5Y55YYJPP5JP&&&BYGJ?YYYYYYYYYY77PP55PP5PP55YY5&#G#GG&#B#&&&###&BJ:.^?P&&&&&&&#Y~Y#&&
YYYYY5BB#BBPBG55PY5B5?7?Y5YJP&&B55? :~7JYYYYJ^ .55Y5P5YJ?!~^.!&#G##B&#B#&&&GYYJ!:..:^7YPBB####Y~J##&
JJJYG####GBGBBP55G#&5 .:^~75&B7:!. .755Y: :..:~~^. 7&YP##GBBGGBBBJ^:......:^~7?Y5YYJ7~7Y5P
7?YG&&&&&BB#GBG5P#&&&Y 7#7 . !Y~ JJ^B#BBBPPPBBGP5Y!^~!~:.:?5PGBBGGP5PP55
J5B&&&&&&#P#BGGBB#&&&&Y. ~! . : 7 7##B#&BBBBBBG5J77JYY?!!7??5BBGGBB##&&
#&&&&&&&&&BPBBB##&&&&&@5. . ^JPY?~. .GBB#&&&###GY?7777??YYYJJJYYPPPGBBB#&&
&&&&&&&&&&BG##########&&P. .~JPGBBBG5J?!^. Y#GB##&#BGGYY7~!!!!!77????JJJJYYYY5PGB
########&&##GB#&&&&#####&G: :5#BBBBBBBBB##G: ~&#B####BGB#55Y???7777777???77!!!!!!!!!
############PB#BB##&&&&&&@B^ ^5#&#########7 !#&&@&BBB####GPBG55PGGPP55YYJJJ??777!!!!
############PB###BBB##&&&&&#! ?B#######&P 7&@&BP5G######BPBB?:!5BBBBBBBBBBGGGPPP55Y
############P#######BBBB##&@&Y. ~G###BB#&? J#G5YJ5B########GP5^ ~PBBBBBBBBBBBBBBBBBB
############G#####B###BGGPPPGBP?!^:. :YPJ~:~?. .:~JPJ!7YG##########BY!.^:^5BBBBBBBBBBBBBBBBBB
######BBBBBBP#####BBB###BGGP?7?5PPP5YJ7~: .:!?JYYYY5J?YB#B#BB######Y .7GBBBBBBBBBBBBBBBBBB
BBBBGGGGBB#BG######BGGGBB###PJJYY55J?JJYYY?!:^7J?7JYJ??JY555GGYP#BBGB######G: .JBBBBBBBBBBBBBBBBBBB
PPPGGGGGBGJ!Y#######BG5P###BGYGBGP555YJ7777!!J????7!?Y5GBBB&&&G5###BPG#####B?.~JBBBBBBBBBBBBBBBBBBBB
/* no u
*/

m_arm.SetDefaultCommand(
Expand Down Expand Up @@ -170,26 +128,6 @@ void RobotContainer::ConfigureButtonBindings() {
IntakeDrive(m_intake, -1)
);

// Tank Turn Tuning
frc2::JoystickButton(&m_stick2, 5).WhenPressed(
TankTurn(m_drivetrain, -45)
);
frc2::JoystickButton(&m_stick2, 3).WhenPressed(
TankTurn(m_drivetrain, -135)
);
frc2::JoystickButton(&m_stick2, 6).WhenPressed(
TankTurn(m_drivetrain, 45)
);
frc2::JoystickButton(&m_stick2, 4).WhenPressed(
TankTurn(m_drivetrain, 135)
);
frc2::JoystickButton(&m_stick3, 5).WhenPressed(
TankTurn(m_drivetrain, -180)
);
frc2::JoystickButton(&m_stick3, 6).WhenPressed(
TankTurn(m_drivetrain, 180)
);

}

frc2::Command* RobotContainer::GetAutonomousCommand() {
Expand Down
5 changes: 3 additions & 2 deletions src/main/cpp/commands/TankMoveGyro.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@ conversion factor around 2.3


TankMoveGyro::TankMoveGyro(Drivetrain& drivetrain, double distance, double speed):
m_distance(distance),
m_drivetrain(&drivetrain),
m_distance(distance),
m_speed(speed) {

SetName("TankMove");
Expand All @@ -25,6 +25,7 @@ void TankMoveGyro::Initialize() {
m_drivetrain->m_leftLeadEncoder.SetPosition(0);
m_drivetrain->m_rightLeadEncoder.SetPosition(0);
m_drivetrain->m_navX.Reset();
m_drivetrain->m_pid_move.SetSetpoint(0);
distanceCounter = 0;
m_drivetrain->Drive(0, 0);
}
Expand All @@ -33,7 +34,7 @@ void TankMoveGyro::Initialize() {
// Called repeatedly when this Command is scheduled to run
void TankMoveGyro::Execute() {
distanceCounter = (-m_drivetrain->m_leftLeadEncoder.GetPosition() + m_drivetrain->m_rightLeadEncoder.GetPosition())/2;
double correction = m_drivetrain->m_pid.Calculate(m_drivetrain->m_navX.GetAngle());
double correction = m_drivetrain->m_pid_move.Calculate(m_drivetrain->m_navX.GetAngle());
if (m_distance >= 0 && distanceCounter < m_distance){
m_drivetrain->Drive(m_speed-correction, m_speed+correction);
}
Expand Down
9 changes: 4 additions & 5 deletions src/main/cpp/commands/autonomous/Ball2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,18 +27,17 @@ Ball2::Ball2(
// Remove ball
IntakeMove(intake, -1, 0.3),
// Taxi out
TankMoveGyro(drivetrain, -0.4, 0.6),
TankMoveGyro(drivetrain, -1.4, 0.63),
// Break tape
ArmLower(arm),
// Turn around to face the ball
TankTurn(drivetrain, 165),
TankTurn(drivetrain, 180),
// Get the ball
GrabBall(arm, drivetrain, intake),
// Turn back around to face hub
TankTurn(drivetrain, -165),
TankStop(drivetrain, 0.2),
TankTurn(drivetrain, 180),
// Move towards hub
TankMoveGyro(drivetrain, 1.8, 0.6),
TankMove(drivetrain, 1.9, 0.63),
// Score ball
ScoreBall(arm, drivetrain, intake)
);
Expand Down
36 changes: 36 additions & 0 deletions src/main/cpp/commands/autonomous/Ball2b.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
#include "commands/autonomous/Ball2b.h"

#include "commands/ArmMove.h"
#include "commands/TankMove.h"
#include "commands/TankMoveGyro.h"
#include "commands/TankTurn.h"
#include "commands/IntakeMove.h"

#include "commands/TankStop.h"
#include "commands/TankRawMove.h"

#include "commands/common/ArmLower.h"
#include "commands/common/GrabBall.h"
#include "commands/common/ScoreBall.h"


Ball2b::Ball2b(
Arm& arm,
Drivetrain& drivetrain,
Intake& intake):
m_arm(&arm),
m_drivetrain(&drivetrain),
m_intake(&intake){

SetName("Ball2b");
AddCommands(
TankMoveGyro(drivetrain, 0.6, 0.5),
GrabBall(arm, drivetrain, intake),
TankMoveGyro(drivetrain, -2.35, 0.5),
TankTurn(drivetrain, 158),
TankMoveGyro(drivetrain, 0.25, 0.45),
// Score ball
ScoreBall(arm, drivetrain, intake)
);

}
2 changes: 1 addition & 1 deletion src/main/cpp/commands/autonomous/OnlyTaxi.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ OnlyTaxi::OnlyTaxi(
// Remove ball
IntakeMove(intake, -1, 0.3),
// Taxi out
TankMove(drivetrain, -2, 0.5)
TankMove(drivetrain, -3, 0.5)
);

}
8 changes: 4 additions & 4 deletions src/main/cpp/commands/common/GrabBall.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,12 @@ GrabBall::GrabBall(

SetName("GrabBall");
AddCommands(
ArmMove(arm, -0.15, 0.6),
ArmMove(arm, -0.23, 0.6),
// Move forward while intake in
frc2::ParallelCommandGroup(
ArmMove(arm, -0.1, 1),
TankRawMove(drivetrain, 0.5 * 1.1, 0.5 * 1, 1),
IntakeMove(intake, 0.5, 1))
ArmMove(arm, -0.1, 1.3),
TankMoveGyro(drivetrain, 0.5, 0.7),
IntakeMove(intake, 0.8, 1.3))
);

}
8 changes: 3 additions & 5 deletions src/main/cpp/commands/common/ScoreBall.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,20 +21,18 @@ ScoreBall::ScoreBall(

SetName("ScoreBall");
AddCommands(
TankStop(drivetrain, 0.1),
// Raise arm
ArmMove(arm, 0.4, 1.5),
ArmMove(arm, 0.3, 0.7),
// Move forward while keeping arm up
frc2::ParallelRaceGroup(
TankMove(drivetrain, 0.3, 0.4),
TankMove(drivetrain, 0.4, 0.4),
ArmMove(arm, 0.1, 5)),
// Dump ball while keeping arm up
frc2::ParallelCommandGroup(
IntakeMove(intake, -1, 0.3),
ArmMove(arm, 0.1, 0.3)),
// Move back
TankStop(drivetrain, 0.4),
TankMove(drivetrain, -2, 0.7),
TankMove(drivetrain, -3, 0.65),
ArmLower(arm)
);

Expand Down
11 changes: 9 additions & 2 deletions src/main/cpp/subsystems/Drivetrain.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,8 @@ Drivetrain::Drivetrain() {
m_rightFollowMotor.Follow(m_rightLeadMotor);

//Conversion factor from rotations to meters
m_leftLeadEncoder.SetPositionConversionFactor(0.0442);
m_rightLeadEncoder.SetPositionConversionFactor(0.0442);
m_leftLeadEncoder.SetPositionConversionFactor(0.0443);
m_rightLeadEncoder.SetPositionConversionFactor(0.0443);

SetName("Drivetrain");

Expand Down Expand Up @@ -48,6 +48,13 @@ void Drivetrain::Log() {
frc::SmartDashboard::PutNumber("Gyro", m_navX.GetYaw());
}

void Drivetrain::SetIdleMode(rev::CANSparkMax::IdleMode mode){
m_leftLeadMotor.SetIdleMode(mode);
m_rightLeadMotor.SetIdleMode(mode);
m_leftFollowMotor.SetIdleMode(mode);
m_rightFollowMotor.SetIdleMode(mode);
}

void Drivetrain::Periodic() {
// Implementation of subsystem periodic method goes here.
Log();
Expand Down
5 changes: 4 additions & 1 deletion src/main/include/RobotContainer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <frc2/command/Command.h>

#include "commands/autonomous/Ball2.h"
#include "commands/autonomous/Ball2b.h"
#include "commands/autonomous/Ball3.h"
#include "commands/autonomous/OnlyTaxi.h"

Expand All @@ -29,17 +30,19 @@ class RobotContainer {
public:
RobotContainer();
frc2::Command* GetAutonomousCommand();
Drivetrain m_drivetrain;

private:
frc::Joystick m_stick1{0};
frc::Joystick m_stick2{1};
frc::Joystick m_stick3{2};

Drivetrain m_drivetrain;

Intake m_intake;
Arm m_arm;

Ball2 m_ball2{m_arm, m_drivetrain, m_intake};
Ball2 m_ball2b{m_arm, m_drivetrain, m_intake};
Ball3 m_ball3{m_arm, m_drivetrain, m_intake};
OnlyTaxi m_only_taxi{m_drivetrain, m_intake};

Expand Down
23 changes: 23 additions & 0 deletions src/main/include/commands/autonomous/Ball2b.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
#pragma once

#include <frc2/command/CommandHelper.h>
#include <frc2/command/SequentialCommandGroup.h>

#include "subsystems/Arm.h"
#include "subsystems/Drivetrain.h"
#include "subsystems/Intake.h"


class Ball2b: public frc2::CommandHelper<frc2::SequentialCommandGroup, Ball2b> {

public:

Ball2b(Arm& arm, Drivetrain& drivetrain, Intake& intake);

private:

Arm* m_arm;
Drivetrain* m_drivetrain;
Intake* m_intake;

};
4 changes: 3 additions & 1 deletion src/main/include/subsystems/Drivetrain.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ class Drivetrain: public frc2::SubsystemBase {
void Drive(double left, double right);
void Log();
void Periodic() override;
void SetIdleMode(rev::CANSparkMax::IdleMode mode);

private:

Expand All @@ -47,6 +48,7 @@ class Drivetrain: public frc2::SubsystemBase {
AHRS m_navX{frc::SPI::Port::kMXP};
rev::SparkMaxRelativeEncoder m_leftLeadEncoder = m_leftLeadMotor.GetEncoder();
rev::SparkMaxRelativeEncoder m_rightLeadEncoder = m_rightLeadMotor.GetEncoder();
frc2::PIDController m_pid{0.005, 0, 0}; // PID Controller
frc2::PIDController m_pid_move{0.005, 0, 0}; // PID Controller
frc2::PIDController m_pid_turn{0.001, 0, 0};

};

0 comments on commit 56f6aec

Please sign in to comment.