Skip to content
This repository has been archived by the owner on Apr 17, 2021. It is now read-only.

Commit

Permalink
Google Java Format
Browse files Browse the repository at this point in the history
  • Loading branch information
GitHub Actions committed Mar 10, 2020
1 parent 20c2bdd commit f9e3d7b
Show file tree
Hide file tree
Showing 10 changed files with 41 additions and 53 deletions.
9 changes: 4 additions & 5 deletions RobotCode2020/src/main/java/frc/robot/OI.java
Original file line number Diff line number Diff line change
Expand Up @@ -16,22 +16,21 @@
import frc.robot.commands.RetractClimb;
import frc.robot.commands.ArmPosition;


/**
* This class is the glue that binds the controls on the physical operator interface to the commands
* and command groups that allow control of the robot.
*/
public class OI {
//CONTROLLER 1
// CONTROLLER 1
public Joystick controller = new Joystick(RobotMap.CONTROLLER_1_PORT_ID);
//Co-CONTROLLER
// Co-CONTROLLER
public Joystick coController = new Joystick(RobotMap.CONTROLLER_2_PORT_ID);
public Button aCoButton = new JoystickButton(controller, RobotMap.A_BUTTON_ID);
public Button bCoButton = new JoystickButton(controller, RobotMap.B_BUTTON_ID);
public Button xCoButton = new JoystickButton(controller, RobotMap.X_BUTTON_ID);
public Button leftBumperCoButton = new JoystickButton(controller, RobotMap.LEFT_BUMPER_BUTTON_ID);
public Button rightBumperCoButton = new JoystickButton(controller, RobotMap.RIGHT_BUMPER_BUTTON_ID);

public Button rightBumperCoButton =
new JoystickButton(controller, RobotMap.RIGHT_BUMPER_BUTTON_ID);

public OI() {
aCoButton.whileHeld(new WinchWind());
Expand Down
5 changes: 2 additions & 3 deletions RobotCode2020/src/main/java/frc/robot/RobotMap.java
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,12 @@ public class RobotMap {
public static final int CONTROLLER_1_PORT_ID = 0; // switches temporarily
public static final int CONTROLLER_2_PORT_ID = 1;


// arm ID
public static final int MOTOR_ARM_ID = 20;

// intake ID
public static final int MOTOR_MAILBOX_ID = 31;

// drivetrain motors
public static final int MOTOR_LEFT_BACK_ID = 12;
public static final int MOTOR_RIGHT_BACK_ID = 10;
Expand All @@ -32,7 +31,7 @@ public class RobotMap {

public static final int MOTOR_CLIMB_EXTENDER_ID = 40;
public static final int MOTOR_CLIMB_WINCH_ID = 50;

// left joystick (horizontal)
public static final int JOYSTICK_DRIVE_FORWARDS_ID = 1;
// left joystick (horizontal)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,7 @@ public ArmPosition() {

// Called just before this Command runs the first time
@Override
protected void initialize() {
}
protected void initialize() {}

// Called repeatedly when this Command is scheduled to run
@Override
Expand All @@ -38,8 +37,7 @@ protected boolean isFinished() {

// Called once after isFinished returns true
@Override
protected void end() {
}
protected void end() {}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,10 @@ protected boolean isFinished() {

// Called once after isFinished returns true
@Override
protected void end() {
}
protected void end() {}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {

}
protected void interrupted() {}
}
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,7 @@ public RetractClimb() {

// Called just before this Command runs the first time
@Override
protected void initialize() {
}
protected void initialize() {}

// Called repeatedly when this Command is scheduled to run
@Override
Expand All @@ -37,12 +36,10 @@ protected boolean isFinished() {

// Called once after isFinished returns true
@Override
protected void end() {
}
protected void end() {}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
}
protected void interrupted() {}
}
Original file line number Diff line number Diff line change
Expand Up @@ -12,18 +12,15 @@
import frc.robot.commands.drivetrain.RotateToAngle;

public class Auton extends CommandGroup {
/**
* Hopefully works.
*/
/** Hopefully works. */
public Auton() {
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
// these will run in order.
addSequential(new DriveDistance(5), 10.0);
addSequential(new RotateToAngle(90.0), 10.0);
//addSequential(new DriveDistance(1), 10.0);

addSequential(new DriveDistance(5), 10.0);
addSequential(new RotateToAngle(90.0), 10.0);
// addSequential(new DriveDistance(1), 10.0);

// To run multiple commands at the same time,
// use addParallel()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,11 @@ public class DriveDistance extends Command {
private double D = 0.0055;
private PIDController pidDriveController = new PIDController(P, I, D);
private PIDController pidTurnController = new PIDController(P, I, D);

private double rotationsToTarget;
private double ticksTarget;
private int ticksPerRotation = 42;
private double wheelDiameter = 0.5; //ft
private double wheelDiameter = 0.5; // ft

/**
* Drives robot a set distance w/ PID loop to keep it straight
Expand All @@ -32,7 +32,7 @@ public class DriveDistance extends Command {
public DriveDistance(double distance) {
requires(Robot.drivetrain);
// calculate # of ticks based off distance
rotationsToTarget = distance / (wheelDiameter * Math.PI) * 10.75;
rotationsToTarget = distance / (wheelDiameter * Math.PI) * 10.75;
pidDriveController.setTolerance(1);
pidTurnController.setTolerance(0.05);
}
Expand All @@ -43,19 +43,17 @@ protected void initialize() {
Robot.drivetrain.calibrate();
SmartDashboard.putNumber("Ticks To Target", rotationsToTarget);
SmartDashboard.putString("Command", "DriveDistance");

}

// Called repeatedly when this Command is scheduled to run
@Override
protected void execute() {
double forwardsSpeed = pidDriveController.calculate(Robot.drivetrain.getPosition(), rotationsToTarget);
double forwardsSpeed =
pidDriveController.calculate(Robot.drivetrain.getPosition(), rotationsToTarget);
double turnSpeed = pidTurnController.calculate(Robot.drivetrain.gyro.getAngle(), 0);
Robot.drivetrain.drive(forwardsSpeed, turnSpeed);
}



// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
Expand Down
11 changes: 5 additions & 6 deletions RobotCode2020/src/main/java/frc/robot/subsystems/Arm.java
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,9 @@ public Arm() {
armMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10);
// config PID parameters
armMotor.config_kF(0, 0, 10);
armMotor.config_kP(0, 0.3, 10);
armMotor.config_kI(0, 0, 10);
armMotor.config_kD(0, 0, 10);
armMotor.config_kP(0, 0.3, 10);
armMotor.config_kI(0, 0, 10);
armMotor.config_kD(0, 0, 10);
}

@Override
Expand All @@ -45,14 +45,13 @@ public void periodic() {
}

public void turn(double value) {
armMotor.set(ControlMode.PercentOutput, value);
}
armMotor.set(ControlMode.PercentOutput, value);
}

public boolean atSetpoint() {
return (Math.abs(armMotor.getSelectedSensorPosition() - 4600) < 30);
}


public void setSetpointUp(double setpoint) {
SmartDashboard.putString("setSetpoint called:", "yes :)");
armMotor.set(ControlMode.Position, 4600);
Expand Down
5 changes: 2 additions & 3 deletions RobotCode2020/src/main/java/frc/robot/subsystems/Climb.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,7 @@

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
import frc.robot.commands.*;

import frc.robot.commands.*;

/** Add your docs here. */
public class Climb extends Subsystem {
Expand All @@ -30,7 +29,7 @@ public void extendClimb(double voltage) {
}

public void turnWinch(double voltage) {
winchMotor.set(ControlMode.PercentOutput, voltage);
winchMotor.set(ControlMode.PercentOutput, voltage);
}

@Override
Expand Down
19 changes: 12 additions & 7 deletions RobotCode2020/src/main/java/frc/robot/subsystems/Drivetrain.java
Original file line number Diff line number Diff line change
Expand Up @@ -30,21 +30,26 @@
/** Drivetrain class w/ limelight vision tracking */
public class Drivetrain extends Subsystem {
// motor stuff
private CANSparkMax leftMotorFront = new CANSparkMax(RobotMap.MOTOR_LEFT_FRONT_ID, MotorType.kBrushless);
private CANSparkMax leftMotorBack = new CANSparkMax(RobotMap.MOTOR_LEFT_BACK_ID, MotorType.kBrushless);
private CANSparkMax rightMotorFront = new CANSparkMax(RobotMap.MOTOR_RIGHT_FRONT_ID, MotorType.kBrushless);
private CANSparkMax rightMotorBack = new CANSparkMax(RobotMap.MOTOR_RIGHT_BACK_ID, MotorType.kBrushless);
private CANSparkMax leftMotorFront =
new CANSparkMax(RobotMap.MOTOR_LEFT_FRONT_ID, MotorType.kBrushless);
private CANSparkMax leftMotorBack =
new CANSparkMax(RobotMap.MOTOR_LEFT_BACK_ID, MotorType.kBrushless);
private CANSparkMax rightMotorFront =
new CANSparkMax(RobotMap.MOTOR_RIGHT_FRONT_ID, MotorType.kBrushless);
private CANSparkMax rightMotorBack =
new CANSparkMax(RobotMap.MOTOR_RIGHT_BACK_ID, MotorType.kBrushless);

private SpeedControllerGroup leftMotors = new SpeedControllerGroup(leftMotorFront, leftMotorBack);
private SpeedControllerGroup rightMotors = new SpeedControllerGroup(rightMotorFront, rightMotorBack);
private SpeedControllerGroup rightMotors =
new SpeedControllerGroup(rightMotorFront, rightMotorBack);

// drivetrain
private DifferentialDrive dualDrive = new DifferentialDrive(leftMotors, rightMotors);

// encoders
private CANEncoder lEncoder = new CANEncoder(leftMotorBack);
private CANEncoder rEncoder = new CANEncoder(rightMotorBack);

// gyro
public AHRS gyro;

Expand Down Expand Up @@ -75,7 +80,7 @@ public void calibrate() {

/** Return average position between the encoders */
public double getPosition() {
return ((lEncoder.getPosition() + rEncoder.getPosition()*-1) / 2.0);
return ((lEncoder.getPosition() + rEncoder.getPosition() * -1) / 2.0);
}

/**
Expand Down

0 comments on commit f9e3d7b

Please sign in to comment.