Bradley/2024_Programming_Training_Demos
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Partially complete CS18

Browse Source
This commit is contained in:
Bradley Bickford
2024-06-22 15:25:05 -04:00
parent f828ecd6f6
commit e1cf549dd8
21 changed files with 1074 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
CS18 - Sensors with Advanced Controls/src/main/deploy/example.txt Normal file
View 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
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/Main.java Normal file
View 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
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/Robot.java Normal file
View 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() {}
}

20
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/RobotContainer.java Normal file
View File

@@ -0,0 +1,20 @@
// 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.Commands;
public class RobotContainer {
public RobotContainer() {
configureBindings();
}
private void configureBindings() {}
public Command getAutonomousCommand() {
return Commands.print("No autonomous command configured");
}
}

16
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/constants/DrivetrainConstants.java Normal file
View File

@@ -0,0 +1,16 @@
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;
public static final double kTurnP = 0;
public static final double kTurnI = 0;
public static final double kTurnD = 0;
public static final double kPIDTolerance = 1;
public static final boolean kInvertGyro = false;
}

5
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/constants/OIConstants.java Normal file
View File

@@ -0,0 +1,5 @@
package frc.robot.constants;
public class OIConstants {
public static final int kDriverUSB = 0;
}

19
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/constants/ShooterConstants.java Normal file
View File

@@ -0,0 +1,19 @@
package frc.robot.constants;
public class ShooterConstants {
public static final int kMotor1PWMID = 4;
public static final int kMotor2PWMID = 5;
public static final int kMotor1EncoderChannelA = 0;
public static final int kMotor1EncoderChannelB = 1;
public static final int kMotor2EncoderChannelA = 2;
public static final int kMotor2EncoderChannelB = 3;
public static final double kEncoderConversionFactor = 0;
public static final double kBangBangTolerance = 0;
public static final double kFFS = 0;
public static final double kFFV = 0;
public static final double kFFA = 0;
}

77
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/subsystems/Drivetrain.java Normal file
View File

@@ -0,0 +1,77 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
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;
private PIDController turnController;
private SimpleMotorFeedforward turnFF;
private ADXRS450_Gyro gyro;
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);
turnController = new PIDController(
DrivetrainConstants.kTurnP,
DrivetrainConstants.kTurnI,
DrivetrainConstants.kTurnD
);
turnController.setTolerance(DrivetrainConstants.kPIDTolerance);
gyro = new ADXRS450_Gyro();
}
public double getGyroAngle() {
return gyro.getAngle() * (DrivetrainConstants.kInvertGyro ? -1 : 1);
}
public Command goToAngle(double angle, double timeout) {
return run(() -> {
double output = turnController.calculate(
getGyroAngle(),
angle
);
drive.tankDrive(-output, output);
}).until(turnController::atSetpoint).withTimeout(timeout);
}
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());
});
}
}

71
CS18 - Sensors with Advanced Controls/src/main/java/frc/robot/subsystems/Shooter.java Normal file
View File

@@ -0,0 +1,71 @@
package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
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.ShooterConstants;
public class Shooter extends SubsystemBase {
private VictorSP motor1;
private VictorSP motor2;
private BangBangController controller;
private SimpleMotorFeedforward ff;
private Encoder motor1Encoder;
private Encoder motor2Encoder;
public Shooter() {
motor1 = new VictorSP(ShooterConstants.kMotor1PWMID);
motor2 = new VictorSP(ShooterConstants.kMotor2PWMID);
motor1.addFollower(motor2);
controller = new BangBangController(ShooterConstants.kBangBangTolerance);
ff = new SimpleMotorFeedforward(
ShooterConstants.kFFS,
ShooterConstants.kFFV,
ShooterConstants.kFFA
);
motor1Encoder = new Encoder(
ShooterConstants.kMotor1EncoderChannelA,
ShooterConstants.kMotor1EncoderChannelB
);
motor1Encoder.setDistancePerPulse(ShooterConstants.kEncoderConversionFactor);
motor2Encoder = new Encoder(
ShooterConstants.kMotor2EncoderChannelA,
ShooterConstants.kMotor2EncoderChannelB
);
motor2Encoder.setDistancePerPulse(ShooterConstants.kEncoderConversionFactor);
}
public Command runAutomaticSpeedControl(double speed) {
return run(() -> {
double output = controller.calculate(
(motor1Encoder.getRate() + motor2Encoder.getRate()) / 2,
speed
) + ff.calculate(speed);
motor1.setVoltage(output);
});
}
public Command setSpeed(DoubleSupplier speed) {
return run(() -> {
motor1.set(speed.getAsDouble());
});
}
public Command stop() {
return setSpeed(() -> 0);
}
}