Skip to content
This repository was archived by the owner on Jul 20, 2025. It is now read-only.

Commit d581e67

Browse files
Create drivetrain (#7)
* Declare Drivetrain Motors Created a subsystem and added drivetrain motors and motor id's. * Added myself as a contributor Co-Authored-By: Evan Grinnell <155865196+S0L0GUY@users.noreply.github.com> * Added Motor Movement Logic Co-Authored-By: Evan Grinnell <155865196+S0L0GUY@users.noreply.github.com> * Added Empty Drive Command * Created Basic Driving Control Sample This code does not work correctly, but that is to be fixed in the next push. * Made drivetrain turn and not angry Co-Authored-By: Evan Grinnell <155865196+S0L0GUY@users.noreply.github.com> --------- Co-authored-by: Evan Grinnell <155865196+S0L0GUY@users.noreply.github.com>
1 parent 18d4c25 commit d581e67

File tree

3 files changed

+75
-7
lines changed

3 files changed

+75
-7
lines changed

src/main/java/frc/robot/RobotContainer.java

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,28 +8,32 @@
88

99
import edu.wpi.first.wpilibj2.command.Command;
1010
import edu.wpi.first.wpilibj2.command.Commands;
11+
import frc.robot.commands.Drive;
1112
import frc.robot.commands.IntakeGround;
1213
import frc.robot.commands.PrepShooter;
1314
import frc.robot.commands.StageGP;
1415
import frc.robot.commands.intakeHopper;
16+
import frc.robot.subsystems.Drivetrain;
1517
import frc.robot.subsystems.Hopper;
1618
import frc.robot.subsystems.Intake;
1719
import frc.robot.subsystems.Shooter;
1820
import frc.robot.subsystems.Stager;
1921

2022
public class RobotContainer {
21-
private final SN_XboxController m_driverController = new SN_XboxController(1);
23+
private final SN_XboxController m_driverController = new SN_XboxController(RobotMap.mapControllers.DRIVER_USB);
24+
private final Drivetrain subDrivetrain = new Drivetrain();
2225
private final Intake subIntake = new Intake();
2326
private final Hopper subHopper = new Hopper();
2427
private final Shooter subShooter = new Shooter();
2528
private final Stager subStager = new Stager();
29+
private final Drive com_Drive = new Drive(subDrivetrain, m_driverController.axis_LeftY, m_driverController.axis_RightX);
2630
private final IntakeGround com_IntakeGround = new IntakeGround(subIntake, subHopper);
2731
private final PrepShooter com_PrepShooter = new PrepShooter(subShooter);
2832
private final StageGP com_StageGP = new StageGP(subStager);
2933

3034
public RobotContainer() {
35+
subDrivetrain.setDefaultCommand(com_Drive);
3136
configureBindings();
32-
3337
}
3438

3539
private void configureBindings() {
@@ -42,4 +46,4 @@ private void configureBindings() {
4246
public Command getAutonomousCommand() {
4347
return Commands.print("No autonomous command configured");
4448
}
45-
}
49+
}
Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.commands;
6+
7+
import java.util.function.DoubleSupplier;
8+
9+
import edu.wpi.first.wpilibj2.command.Command;
10+
import frc.robot.subsystems.Drivetrain;
11+
12+
public class Drive extends Command {
13+
/** Creates a new Drive. */
14+
15+
Drivetrain globalDrivetrain;
16+
DoubleSupplier globalForwardSpeed;
17+
DoubleSupplier globalRotationSpeed;
18+
19+
public Drive(Drivetrain passedDrivetrain, DoubleSupplier passedForwardSpeed, DoubleSupplier passedRotationSpeed) {
20+
// Use addRequirements() here to declare subsystem dependencies.
21+
globalDrivetrain = passedDrivetrain;
22+
globalForwardSpeed = passedForwardSpeed;
23+
globalRotationSpeed = passedRotationSpeed;
24+
25+
addRequirements(globalDrivetrain);
26+
}
27+
28+
// Called when the command is initially scheduled.
29+
@Override
30+
public void initialize() {}
31+
32+
// Called every time the scheduler runs while the command is scheduled.
33+
@Override
34+
public void execute() {
35+
globalDrivetrain.setDrivetrainSpeed(globalForwardSpeed.getAsDouble(), globalRotationSpeed.getAsDouble());
36+
}
37+
38+
// Called once the command ends or is interrupted.
39+
@Override
40+
public void end(boolean interrupted) {}
41+
42+
// Returns true when the command should end.
43+
@Override
44+
public boolean isFinished() {
45+
return false;
46+
}
47+
}

src/main/java/frc/robot/subsystems/Drivetrain.java

Lines changed: 21 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -11,10 +11,10 @@
1111

1212
public class Drivetrain extends SubsystemBase {
1313
/** Creates a new Drivetrain. */
14-
TalonFX frontRightMotor;
15-
TalonFX frontLeftMotor;
16-
TalonFX backRightMotor;
17-
TalonFX backLeftMotor;
14+
private TalonFX frontRightMotor;
15+
private TalonFX frontLeftMotor;
16+
private TalonFX backRightMotor;
17+
private TalonFX backLeftMotor;
1818

1919
public Drivetrain() {
2020
frontRightMotor = new TalonFX(RobotMap.mapDriveTrain.FRONT_RIGHT_MOTOR);
@@ -23,8 +23,25 @@ public Drivetrain() {
2323
backLeftMotor = new TalonFX(RobotMap.mapDriveTrain.BACK_LEFT_MOTOR);
2424
}
2525

26+
/**
27+
* Sets the velocity of the drivetrain motors.
28+
*
29+
* @param forwardVelocity The velocity to set for the forward movement of the drivetrain.
30+
* @param rotationSpeed The rotation speed to apply to the drivetrain.
31+
*/
32+
public void setDrivetrainSpeed(double forwardVelocity, double rotationSpeed) {
33+
// Set right velocity
34+
frontRightMotor.set(forwardVelocity - rotationSpeed);
35+
backRightMotor.set(forwardVelocity - rotationSpeed);
36+
37+
// Set left velocity
38+
frontLeftMotor.set(forwardVelocity + rotationSpeed);
39+
backLeftMotor.set(forwardVelocity + rotationSpeed);
40+
}
41+
2642
@Override
2743
public void periodic() {
2844
// This method will be called once per scheduler run
2945
}
3046
}
47+

0 commit comments

Comments
 (0)