Skip to content
This repository has been archived by the owner on Jan 10, 2024. It is now read-only.

Commit

Permalink
do stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
TheGamer1002 committed Apr 5, 2023
1 parent e6f3122 commit bc328e3
Show file tree
Hide file tree
Showing 8 changed files with 91 additions and 30 deletions.
60 changes: 60 additions & 0 deletions src/main/java/edu/bearbots/BearLib/drivebase/Drivebase.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
package edu.bearbots.BearLib.drivebase;

import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.CANSparkMax.IdleMode;

import java.util.Map;

/**
* Abstract class to handle common drivebase motor controller functions.
*/
public abstract class Drivebase {
// for all drive bases? don't implement motors
// odometry
// driving
// move to

/**
* Sets the current limit for Spark Max motor controllers, given a free limit
* and a stall limit (in amps) as well as any number of Spark Maxes.
*
* @param freeLimit The free limit (in amps)
* @param stallLimit The stall limit (in amps)
* @param motor The Spark Maxes
* @return An array of errors, if any
*/
public REVLibError[] setCurrentLimit(int freeLimit, int stallLimit, CANSparkMax... motor) {
REVLibError[] errors = new REVLibError[motor.length];
int i = 0;

for (CANSparkMax m : motor) {
errors[i] = m.setSmartCurrentLimit(freeLimit, stallLimit);
i++;
}

return errors;
}


/**
* Sets the idle mode for Spark Max motor controllers, given an idle mode and
* any number of Spark Maxes.
*
* @param mode The idle mode
* @param motor The Spark Maxes
* @return An array of errors, if any
*/
public REVLibError[] setIdleMode(IdleMode mode, CANSparkMax... motor) {
REVLibError[] errors = new REVLibError[motor.length];
int i = 0;

for (CANSparkMax m : motor) {
errors[i] = m.setIdleMode(mode);
i++;
}

return errors;
}
}
// inclue
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
package edu.bearbots.BearLib.drivebase;

public abstract class MecanumDrivebase extends Drivebase {
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
package edu.bearbots.BearLib.drivebase;

public abstract class SwerveDrivebase extends Drivebase {
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
package edu.bearbots.BearLib.drivebase;

public abstract class TankDrivebase extends Drivebase {
}
14 changes: 3 additions & 11 deletions src/main/java/frc/robot/RebindHat.java
Original file line number Diff line number Diff line change
@@ -1,19 +1,11 @@
package frc.robot;

/** @deprecated */
// dude come onthis is the deffinition fo unnesseary complecity
@Deprecated
public class RebindHat {
public static double JoystickToYAxis() {
if (RobotContainer.m_armController.getPOV() == -1
|| RobotContainer.m_armController.getPOV() == 90
|| RobotContainer.m_armController.getPOV() == 270) {
return 0;
}
if (RobotContainer.m_armController.getPOV() > 270
|| RobotContainer.m_armController.getPOV() < 90) {
return 1;
} else {
return -1;
}

}

public static double JoystickToXAxis() {
Expand Down
14 changes: 9 additions & 5 deletions src/main/java/frc/robot/commands/BalanceCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,9 @@
import frc.robot.subsystems.PID;
import frc.robot.subsystems.Tank;

/** This command balances the robot on the charge station. Default auto command. */
/**
* This command balances the robot on the charge station. Default auto command.
*/
public class BalanceCommand extends CommandBase {
@SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
private final PID pidLoop;
Expand Down Expand Up @@ -53,17 +55,18 @@ public void execute() {
SmartDashboard.putNumber("max offset", maxPitch);
SmartDashboard.putNumber(
"motor speed", Constants.OperatorConstants.Pconstant * (pitchOffset));
if (pitchOffset > maxPitch) {
maxPitch = pitchOffset;
}
/*
* if (pitchOffset > maxPitch) {
* maxPitch = pitchOffset;
* }
*/

// set boolean to true when robot is on ramp to run p loop
if (!onRamp && pitchOffset >= 16) {
onRamp = true;
}

if (onRamp) {
onRamp = true;
// dead zone of tilt
if (Math.abs(pitchOffset) < 2) {
driveBase.setAllMotors(0);
Expand All @@ -76,6 +79,7 @@ public void execute() {

@Override
public void end(boolean interrupted) {
// not used
}

@Override
Expand Down
19 changes: 6 additions & 13 deletions src/main/java/frc/robot/subsystems/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -50,28 +50,21 @@ public Arm() {

@Override
public void periodic() {
SmartDashboard.putBoolean("zeroDeg", allTheWayDownRear.get());
// SmartDashboard.putBoolean("zeroDeg", allTheWayDownRear.get());

encoderWidget.setDouble(armMotor.getEncoder().getPosition());
}

public void moveArm(double leftStickYaxis) {
/**
* uses input double value to set the motor speed of the arm
*/
public void moveArm(double value) {
double speed = 0.8;
double motorDrive = leftStickYaxis * speed;
double motorDrive = value * speed;
armMotor.set(motorDrive);
}

public void moveArmToZeroDeg() {
double speedY = 0.2;
while (allTheWayDownRear
.get()) { // the '== true' is implied, because the if statement is looking for the
// expression to be true. If it is false, it will not run the code inside the if
// statement, so we don't need to write it.
armMotor.set(-1 * speedY);
}
armMotor.set(0);
armMotor.getEncoder().setPosition(0);
currentArmAngle = 0;
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
// license that can be found in the LICENSE file at
// the root directory of this project.

package frc.robot;
package frc.robot.subsystems;

import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
Expand Down

0 comments on commit bc328e3

Please sign in to comment.