Skip to content

Commit

Permalink
LEFT_BUMPER is now changed to drive straight tankDrive
Browse files Browse the repository at this point in the history
  • Loading branch information
@moso1
moso1 committed Mar 2, 2022
1 parent 169deeb commit 0252e6e
Show file tree
Hide file tree
Showing 3 changed files with 50 additions and 17 deletions.
1 change: 1 addition & 0 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,7 @@ private Constants() {}
public static final double DRIVE_GEAR_RATIO = 10.71;
public static final double DRIVE_WHEEL_DIAMETER = 6;

public static final int GYRO_NOTUSED = -1;

//PID Value
public static final double DRIVETRAIN_KP=0.06;
Expand Down
12 changes: 9 additions & 3 deletions src/main/java/frc/robot/oi/DriverOI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,11 @@
/*----------------------------------------------------------------------------*/

package frc.robot.oi;
//import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc.robot.Constants;
import frc.robot.RobotContainer;
import frc.robot.subsystems.ChassisSubsystem;

/**
* Driver OI Controls
Expand All @@ -18,10 +19,15 @@ public class DriverOI extends OI {

public DriverOI(int channel, RobotContainer robotContainer) {
super(channel);

a = new JoystickButton(joystick, Constants.A);
y = new JoystickButton(joystick, Constants.Y);
b = new JoystickButton(joystick, Constants.B);
a = new JoystickButton(joystick, Constants.A);
x = new JoystickButton(joystick, Constants.X);

ChassisSubsystem chassisSubsystem = robotContainer.getChassisSubsystem();
leftBumper = new JoystickButton(joystick, Constants.LEFT_BUMPER);
leftBumper.whenPressed(new InstantCommand(chassisSubsystem::startDriveStraight, chassisSubsystem));
leftBumper.whenReleased(new InstantCommand(chassisSubsystem::endDriveStraight, chassisSubsystem));
}
}
54 changes: 40 additions & 14 deletions src/main/java/frc/robot/subsystems/ChassisSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.RelativeEncoder;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand All @@ -25,9 +27,10 @@ public enum RobotSide {
private CANSparkMax frontLeftMotor = new CANSparkMax(Constants.FRONT_LEFT_MOTOR, MotorType.kBrushless);
private CANSparkMax frontRightMotor = new CANSparkMax(Constants.FRONT_RIGHT_MOTOR, MotorType.kBrushless);
private CANSparkMax rearLeftMotor = new CANSparkMax(Constants.REAR_LEFT_MOTOR, MotorType.kBrushless);
private CANSparkMax rearRightMotor = new CANSparkMax(Constants.REAR_RIGHT_MOTOR, MotorType.kBrushless);
private CANSparkMax rearRightMotor = new CANSparkMax(Constants.REAR_RIGHT_MOTOR, MotorType.kBrushless);
// The gyro sensor
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
private final PIDController controller = new PIDController(Constants.DRIVETRAIN_KP, Constants.DRIVETRAIN_KI, Constants.DRIVETRAIN_KD);

// Encoder declarations
private RelativeEncoder frontLeftEncoder;
Expand All @@ -36,7 +39,8 @@ public enum RobotSide {
private RelativeEncoder rearRightEncoder;

private DifferentialDrive driveTrain;

private double m_gyroAngle = Constants.GYRO_NOTUSED;

//drive constants
/**
* The scaling factor between the joystick value and the speed controller
Expand All @@ -51,7 +55,7 @@ public enum RobotSide {
/**
* The scale factor for crawl mode
*/
private static final double crawl = 0.3;
private static final double crawl = 0.3;

/**
* The minimum (closest to 0) speed controller command for the right side of the drive train to start moving forward. Must be empirically derived.
Expand Down Expand Up @@ -82,7 +86,7 @@ public enum RobotSide {

//arcade drive constant
private double turnGain = 0.75;

/**
* The direction which is "forward"; 1 represents the hatch side and -1 represents the cargo side.
*/
Expand Down Expand Up @@ -115,7 +119,8 @@ public ChassisSubsystem() {
rearRightEncoder = rearRightMotor.getEncoder();

driveTrain = new DifferentialDrive(frontLeftMotor, frontRightMotor);
frontRightMotor.setInverted(true);
frontRightMotor.setInverted(true);
m_gyroAngle = Constants.GYRO_NOTUSED;
}

@Override
Expand All @@ -128,7 +133,7 @@ public void periodic() {
SmartDashboard.putNumber("Rear Right Motor RPM", rearRightEncoder.getVelocity());
SmartDashboard.putNumber("Left POS", frontLeftEncoder.getPosition());
SmartDashboard.putNumber("Right POS", frontRightEncoder.getPosition());
SmartDashboard.putNumber("Gyro angle", m_gyro.getAngle());
SmartDashboard.putNumber("Gyro angle", m_gyro.getAngle());
}

/**
Expand All @@ -149,19 +154,29 @@ public void arcadeDrive(double speed, double rotation) {
driveTrain.arcadeDrive(speed, rotation);
}

public void startDriveStraight() {
resetGyro();
m_gyroAngle = m_gyro.getAngle();
controller.setTolerance(1, 5);
}
public void endDriveStraight() {
m_gyroAngle = Constants.GYRO_NOTUSED;
}

/**
* drive - set speed to chassis with the joystick input with a scale
* @param driverOI
* @param scale
*/
public void drive(RobotContainer robotContainer, DriverOI driverOI, int scale) {
speedMultiplier = driverOI.getJoystick().getRawButton(Constants.RIGHT_BUMPER) ? crawl : normal;
dir = driverOI.getJoystick().getRawButton(Constants.LEFT_BUMPER) ? Constants.OUT : Constants.IN;

// LEFT_BUMPER is now changed to drive straight tankDrive.
//dir = driverOI.getJoystick().getRawButton(Constants.LEFT_BUMPER) ? Constants.OUT : Constants.IN;

RobotContainer.DriveTrainType driveTrainType = robotContainer.getDriveTrainType();

if (driveTrainType == RobotContainer.DriveTrainType.TANK) {
double rightVal;
double rightVal;
double leftVal;
if(dir == Constants.IN) {
rightVal = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.RIGHT_JOYSTICK_Y), scale, RobotSide.RIGHT);
Expand All @@ -170,9 +185,21 @@ public void drive(RobotContainer robotContainer, DriverOI driverOI, int scale) {
rightVal = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.LEFT_JOYSTICK_Y), scale, RobotSide.RIGHT);
leftVal = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.RIGHT_JOYSTICK_Y), scale, RobotSide.LEFT);
}
SmartDashboard.putNumber("Left Speed", leftVal);
SmartDashboard.putNumber("Right Speed", rightVal);
tankDrive(leftVal * speedMultiplier * dir, rightVal * speedMultiplier * dir);
if(m_gyroAngle != Constants.GYRO_NOTUSED)
{
var pidOutput = controller.calculate(getGyroAngle(), 0) / 10;

var speed = leftVal * speedMultiplier * dir;
SmartDashboard.putNumber("Left Speed", speed+pidOutput);
SmartDashboard.putNumber("Right Speed", speed-pidOutput);
tankDrive(speed + pidOutput, speed - pidOutput);
}
else
{
SmartDashboard.putNumber("Left Speed", leftVal);
SmartDashboard.putNumber("Right Speed", rightVal);
tankDrive(leftVal * speedMultiplier * dir, rightVal * speedMultiplier * dir);
}
} else if (driveTrainType == RobotContainer.DriveTrainType.ARCADE) {
double speed = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.LEFT_JOYSTICK_Y), scale, RobotSide.LEFT) * dir;
double rotation = getScaledValue(driverOI.getJoystick().getRawAxis(Constants.LEFT_JOYSTICK_X), scale, RobotSide.RIGHT);
Expand All @@ -184,7 +211,7 @@ public void drive(RobotContainer robotContainer, DriverOI driverOI, int scale) {

/**
* Find the scale factor based on the input value and scale on each side of the Robot's motors
*
*
* @param val
* @param scale
* @param side
Expand Down Expand Up @@ -247,7 +274,6 @@ public void resetEncoders() {

public double getLeftDistanceInch() {
return frontLeftEncoder.getPosition() / Constants.DRIVE_GEAR_RATIO * Math.PI * Constants.DRIVE_WHEEL_DIAMETER;

}

public double getRightDistanceInch() {
Expand Down

0 comments on commit 0252e6e

Please sign in to comment.