Skip to content

Fix angles for climbing #94

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
brettle merged 7 commits into from
Mar 16, 2019
Merged

Fix angles for climbing #94

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
cd48a1a
Fix angles for climbing
DriverStationComputer Mar 14, 2019
b02256f
Change climber piston ports
DriverStationComputer Mar 14, 2019
d150708
Use atan for climber angle
DriverStationComputer Mar 15, 2019
0ad84a9
use pitch instead of accel calculations
kevinzwang Mar 15, 2019
34c28bf
Merge pull request #95 from DeepBlueRobotics/climber-ports
dfang314 Mar 15, 2019
71caeab
change rails to toggle
alexander-mcdowell Mar 15, 2019
dee2ba0
Merge branch 'climber' of https://github.com/DeepBlueRobotics/RobotCo…
alexander-mcdowell Mar 15, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions Robot2019/src/main/java/frc/robot/OI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
import edu.wpi.cscore.VideoSink;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import frc.robot.commands.ActuateClimberRails;
import frc.robot.commands.ToggleClimberRails;
import frc.robot.commands.Climb;
import frc.robot.commands.EjectCargo;
import frc.robot.commands.EjectHatch;
Expand Down Expand Up @@ -80,7 +80,7 @@ public class OI {
cargoEjectBtn.whenPressed(new EjectCargo(cargo));

climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder);
climberRailBtn.whenPressed(new ActuateClimberRails(climber));
climberRailBtn.whenPressed(new ToggleClimberRails(climber));

autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger);
autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy));
Expand Down
2 changes: 1 addition & 1 deletion Robot2019/src/main/java/frc/robot/RobotMap.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ public class RobotMap {
// Initialize motors on the climbing mech
climberMotor = new VictorSP(1);
climberEncoder = new Encoder(new DigitalInput(4), new DigitalInput(5));
climberPistons = new DoubleSolenoid(6, 1);
climberPistons = new DoubleSolenoid(5, 2);

// Initialize motors on the cargo mech
cargoRoller = new VictorSP(0);
Expand Down
6 changes: 3 additions & 3 deletions ...c/robot/commands/ActuateClimberRails.java → ...rc/robot/commands/ToggleClimberRails.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,10 @@
import edu.wpi.first.wpilibj.command.InstantCommand;
import frc.robot.subsystems.Climber;

public class ActuateClimberRails extends InstantCommand {
public class ToggleClimberRails extends InstantCommand {
private Climber climber;

public ActuateClimberRails(Climber cl) {
public ToggleClimberRails(Climber cl) {
super();
requires(cl);
climber = cl;
Expand All @@ -15,6 +15,6 @@ public ActuateClimberRails(Climber cl) {
// Called once when the command executes
@Override
protected void initialize() {
climber.actuateRails();
climber.toggleRails();
}
}
18 changes: 8 additions & 10 deletions Robot2019/src/main/java/frc/robot/subsystems/Climber.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.commands.KeepClimber;

public class Climber extends Subsystem {
Expand Down Expand Up @@ -40,8 +41,12 @@ public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) {
enc.reset();
}

public void actuateRails() {
pistons.set(DoubleSolenoid.Value.kForward);
public void toggleRails() {
if (pistons.get() == DoubleSolenoid.Value.kForward) {
pistons.set(DoubleSolenoid.Value.kReverse);
} else {
pistons.set(DoubleSolenoid.Value.kForward);
}
}

public void runClimber(double speed) {
Expand All @@ -53,14 +58,7 @@ public void stopClimber() {
}

public double getAngle() {
double rawAngle = Math.atan2(ahrs.getRawAccelZ(), ahrs.getRawAccelX());
double angle;
if (rawAngle > 0) {
angle = rawAngle - Math.PI;
} else {
angle = rawAngle + Math.PI;
}
return angle * 180 / Math.PI;
return ahrs.getPitch();
}

//We are erring on the side of changing directions too much
Expand Down