Skip to content
This repository has been archived by the owner on Sep 13, 2023. It is now read-only.

Commit

Permalink
AutoLevel update
Browse files Browse the repository at this point in the history
  • Loading branch information
@Coolearthsky
Coolearthsky committed Jan 23, 2023
1 parent d1d0df3 commit 7ff8335
Show file tree
Hide file tree
Showing 4 changed files with 82 additions and 26 deletions.
17 changes: 12 additions & 5 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,10 +4,15 @@

package frc.robot;

import edu.wpi.first.wpilibj.SerialPort;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
Expand All @@ -28,6 +33,7 @@
import frc.robot.commands.climber.simpleCommands.HookUpLow;
import frc.robot.commands.shooter.*;


/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
Expand All @@ -38,12 +44,13 @@ public class RobotContainer {
// The robot's subsystems and commands are defined here...

// Subsystems
private final Drivetrain drivetrain = new Drivetrain();
private final AHRS m_gyro = new AHRS(SerialPort.Port.kUSB);
private final Drivetrain drivetrain = new Drivetrain(m_gyro);
private final Climber climber = new Climber();
private final Shooter shooter = new Shooter();
private final Intake intake = new Intake();
private final Indexer indexer = new Indexer();

// Auton DIP Switches

private final DigitalInput firstBallOption = new DigitalInput(3);
Expand Down Expand Up @@ -102,7 +109,7 @@ public class RobotContainer {
// Commands
private final Drive driveCommand = new Drive(drivetrain, leftJoystick, rightJoystick);
private final DriveFurious driveFuriousCommand = new DriveFurious(drivetrain, leftJoystick, rightJoystick);
private final autoLevel autoLevel = new autoLevel(drivetrain);
private final autoLevel autoLevelCommand = new autoLevel(m_gyro, drivetrain);
private final DriveSlow driveSlowCommand = new DriveSlow(drivetrain, leftJoystick, rightJoystick);
private final AlignClimber alignCommand = new AlignClimber(climber, drivetrain);
private final IntakeIntake intakeIntakeCommand = new IntakeIntake(intake);
Expand Down Expand Up @@ -155,7 +162,7 @@ public RobotContainer() {
*/
private void configureButtonBindings() {
turboButton.whileHeld(driveFuriousCommand);
levelButton.whileHeld(autoLevel);
levelButton.whileHeld(autoLevelCommand);
alignButton.whileHeld(alignCommand);//new SequentialCommandGroup(new ClimberCenter(climber), alignCommand));
slowButton.whileHeld(driveSlowCommand);
// stop All.whenPressed(new ParallelCommandGroup(new ClimberStop(climber), new IndexerStop(indexer), new BetterIntakeStop(intake), new ShootStop(shooter)));
Expand All @@ -165,7 +172,7 @@ private void configureButtonBindings() {
shootLowButton.whileHeld(new ParallelCommandGroup(shootLowCommand, feedLowCommand));
ejectButton.whileHeld(new ParallelCommandGroup(intakeEjectCommand, indexerEjectCommand, shootEjectCommand));
shootWayDowntownButton.whileHeld(new ParallelCommandGroup(shootWayDowntownCommand, indexWayDowntownCommand));
//hookZeroButton.whileHeld(hookZeroCommand);
//hookZeroB+++++++utton.whileHeld(hookZeroCommand);

// hookDownButton.whenPressed(hookDownCommand);
// hookUpButton.whenPressed(hookUpCommand);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/drivetrain/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ public void execute() {
double limiter = (-rightJoystick.getRawAxis(2)+1)/2;
drivetrain.driveWithRamp((left*limiter), (right*limiter));
SmartDashboard.putNumber("rightJoystick", rightJoystick.getX());
System.out.println(rightJoystick.getX());
//System.out.println(rightJoystick.getX());
}

// Called once the command ends or is interrupted.
Expand Down
60 changes: 50 additions & 10 deletions src/main/java/frc/robot/commands/drivetrain/autoLevel.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,33 +5,73 @@
package frc.robot.commands.drivetrain;


import java.sql.DriverManager;

import javax.xml.namespace.QName;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.Drivetrain;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.spline.PoseWithCurvature;
import edu.wpi.first.wpilibj.SerialPort;

public class autoLevel extends CommandBase {
private final Drivetrain drivetrain;
/** Creates a new autoLevel. */
public autoLevel(Drivetrain we) {
private AHRS m_gyro;
double previousPitch = 0;
int revCount;
boolean isPitchMode;
boolean backwards;
/** Creates a new autoLevel. */
public autoLevel(AHRS gyro, Drivetrain we) {
drivetrain = we;
m_gyro = gyro;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(drivetrain);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}
public void initialize() {
previousPitch = m_gyro.getPitch()- 3.4;
revCount = 12;
isPitchMode = true;
drivetrain.setBrakeMode(true);
previousPitch = 0;
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double driveAmount = 0.02 * drivetrain.getRoll();
drivetrain.driveWithoutRamp(driveAmount, driveAmount);
System.out.println("running");
}

double Pitch = m_gyro.getPitch() - 3.4;
double changeInPitch = Pitch - previousPitch;
previousPitch = Pitch;
double driveAmount = MathUtil.clamp(0.014 * Pitch, -0.15, 0.15);
System.out.println(changeInPitch);
System.out.println(isPitchMode);
System.out.println(m_gyro.getPitch());
System.out.println(revCount);
if (revCount != 0) {
if (isPitchMode == true && changeInPitch > 1){
isPitchMode = false;
backwards = false;
} else if(isPitchMode == true && changeInPitch < -1) {
isPitchMode = false;
backwards = true;
} else if(isPitchMode == false && backwards == true) {
drivetrain.driveVelocity(-0.12, -0.12);
revCount = revCount - 1;
} else if (isPitchMode == false && backwards == false) {
drivetrain.driveVelocity(.12, .12);
revCount = revCount - 1;
} else if (Pitch > 2 && isPitchMode) {
drivetrain.driveVelocity(driveAmount, driveAmount);
} else if (Pitch < -2 && isPitchMode){
drivetrain.driveVelocity(driveAmount, driveAmount);
}
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
Expand Down
29 changes: 19 additions & 10 deletions src/main/java/frc/robot/subsystems/Drivetrain.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,12 @@ public class Drivetrain extends SubsystemBase {
private FRCTalonFX leftMaster, leftFollower, rightMaster, rightFollower;
private double leftSetpoint, rightSetpoint;
private AHRS m_gyro;



/** Creates a new Drivetrain. */
public Drivetrain() {
m_gyro = new AHRS(SerialPort.Port.kUSB);
public Drivetrain(AHRS gyro) {
m_gyro = gyro;
//m_gyro = new AHRS(SerialPort.Port.kUSB);
leftMaster = new FRCTalonFX.FRCTalonFXBuilder(Constants.DrivetrainConstants.DrivetrainMotors.LeftMaster.CAN_ID)
.withKP(Constants.DrivetrainConstants.DrivetrainMotors.LeftMaster.KP)
.withKI(Constants.DrivetrainConstants.DrivetrainMotors.LeftMaster.KI)
Expand Down Expand Up @@ -94,7 +95,7 @@ else if(right<0&&getCurrentEncoderPosition()>expectedStop){
}

driveWithoutRamp(left, right);
System.out.println("left:right " + left + " : " + right);
//System.out.println("left:right " + left + " : " + right);
}

public void driveWithoutRamp(double left, double right) {
Expand All @@ -104,6 +105,13 @@ public void driveWithoutRamp(double left, double right) {
this.rightSetpoint = right;
}

public void driveVelocity(double left, double right) {
this.leftMaster.driveVelocity(left);
this.rightMaster.driveVelocity(right);
this.leftSetpoint = left;
this.rightSetpoint = right;
}

public void driveWithRamp(double left, double right) {
double rampLeft = ramp(left, leftSetpoint);
double rampRight = ramp(right, rightSetpoint);
Expand All @@ -113,6 +121,7 @@ public void driveWithRamp(double left, double right) {
this.leftSetpoint = rampLeft;
this.rightSetpoint = rampRight;
}


private double ramp(double input, double currentSpeed) {
double dv = input - currentSpeed;
Expand Down Expand Up @@ -161,18 +170,18 @@ public void setBrakeMode(boolean status){
leftFollower.motor.setNeutralMode(NeutralMode.Coast);
rightMaster.motor.setNeutralMode(NeutralMode.Coast);
rightFollower.motor.setNeutralMode(NeutralMode.Coast);
System.out.println("coast");
//System.out.println("coast");
}
}


@Override
public void periodic() {
System.out.println(m_gyro.getRoll());
System.out.println(rightFollower.motor.getMotorOutputPercent());
System.out.println(rightMaster.motor.getMotorOutputPercent());
System.out.println(rightFollower.motor.getMotorOutputPercent());
System.out.println(rightFollower.motor.getMotorOutputPercent());
//System.out.println(m_gyro.getPitch());
//System.out.println(rightFollower.motor.getMotorOutputPercent());
//System.out.println(rightMaster.motor.getMotorOutputPercent());
//System.out.println(rightFollower.motor.getMotorOutputPercent());
//System.out.println(rightFollower.motor.getMotorOutputPercent());

// SmartDashboard.putNumber("drivetrain average encoder value", getCurrentEncoderPosition());
// This method will be called once per scheduler run
Expand Down

0 comments on commit 7ff8335

Please sign in to comment.