Adding CS10 and CS11 code
This commit is contained in:
3
CS10 - Basic Drivetrain/src/main/deploy/example.txt
Normal file
3
CS10 - Basic Drivetrain/src/main/deploy/example.txt
Normal file
@@ -0,0 +1,3 @@
|
||||
Files placed in this directory will be deployed to the RoboRIO into the
|
||||
'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function
|
||||
to get a proper path relative to the deploy directory.
|
||||
15
CS10 - Basic Drivetrain/src/main/java/frc/robot/Main.java
Normal file
15
CS10 - Basic Drivetrain/src/main/java/frc/robot/Main.java
Normal file
@@ -0,0 +1,15 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
|
||||
public final class Main {
|
||||
private Main() {}
|
||||
|
||||
public static void main(String... args) {
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
73
CS10 - Basic Drivetrain/src/main/java/frc/robot/Robot.java
Normal file
73
CS10 - Basic Drivetrain/src/main/java/frc/robot/Robot.java
Normal file
@@ -0,0 +1,73 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
|
||||
private RobotContainer m_robotContainer;
|
||||
|
||||
@Override
|
||||
public void robotInit() {
|
||||
m_robotContainer = new RobotContainer();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void robotPeriodic() {
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledInit() {}
|
||||
|
||||
@Override
|
||||
public void disabledPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void disabledExit() {}
|
||||
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void autonomousPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void autonomousExit() {}
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void teleopPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void teleopExit() {}
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
CommandScheduler.getInstance().cancelAll();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void testPeriodic() {}
|
||||
|
||||
@Override
|
||||
public void testExit() {}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
|
||||
public class RobotContainer {
|
||||
private Drivetrain drivetrain;
|
||||
|
||||
private CommandXboxController driver;
|
||||
|
||||
public RobotContainer() {
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kDriverUSB);
|
||||
|
||||
configureBindings();
|
||||
}
|
||||
|
||||
private void configureBindings() {
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.driveArcade(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return drivetrain.driveTank(() -> .4, () -> -.4);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class DrivetrainConstants {
|
||||
public static final int kLeftFrontPWM = 0;
|
||||
public static final int kLeftRearPWM = 1;
|
||||
public static final int kRightFrontPWM = 2;
|
||||
public static final int kRightRearPWM = 3;
|
||||
}
|
||||
@@ -0,0 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class OIConstants {
|
||||
public static final int kDriverUSB = 0;
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.function.DoubleSupplier;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
|
||||
public class Drivetrain extends SubsystemBase {
|
||||
private VictorSP leftFront;
|
||||
private VictorSP leftRear;
|
||||
private VictorSP rightFront;
|
||||
private VictorSP rightRear;
|
||||
|
||||
private DifferentialDrive drive;
|
||||
|
||||
public Drivetrain() {
|
||||
leftFront = new VictorSP(DrivetrainConstants.kLeftFrontPWM);
|
||||
leftRear = new VictorSP(DrivetrainConstants.kLeftRearPWM);
|
||||
rightFront = new VictorSP(DrivetrainConstants.kRightFrontPWM);
|
||||
rightRear = new VictorSP(DrivetrainConstants.kRightRearPWM);
|
||||
|
||||
leftFront.addFollower(leftRear);
|
||||
rightFront.addFollower(rightRear);
|
||||
rightFront.setInverted(true);
|
||||
|
||||
drive = new DifferentialDrive(leftFront, rightFront);
|
||||
}
|
||||
|
||||
public Command driveArcade(DoubleSupplier xSpeed, DoubleSupplier zRotation) {
|
||||
return run(() -> {
|
||||
drive.arcadeDrive(xSpeed.getAsDouble(), zRotation.getAsDouble());
|
||||
});
|
||||
}
|
||||
|
||||
public Command driveTank(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) {
|
||||
return run(() -> {
|
||||
drive.tankDrive(leftSpeed.getAsDouble(), rightSpeed.getAsDouble());
|
||||
});
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user