diff --git a/src/org/usfirst/frc/team2059/robot/OI.java b/src/org/usfirst/frc/team2059/robot/OI.java index 37a9763..45ef8b3 100644 --- a/src/org/usfirst/frc/team2059/robot/OI.java +++ b/src/org/usfirst/frc/team2059/robot/OI.java @@ -25,8 +25,8 @@ public class OI { } // Print log when button 1 pressed joystickButtons[0][0].whenPressed(new LogEncoder()); - joystickButtons[1][0].whenPressed(new MoveArm(0.25)); - joystickButtons[1][1].whenPressed(new MoveArm(-0.25)); + joystickButtons[1][0].whileHeld(new MoveArm(0.5)); + joystickButtons[1][1].whileHeld(new MoveArm(-0.5)); } public Joystick[] getJoysticks() { return joysticks; diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index 82fbf14..e2fc6c1 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -37,6 +37,8 @@ public class Robot extends IterativeRobot { } public void teleopPeriodic() { Scheduler.getInstance().run(); + SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw()); + SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees()); } public void testPeriodic() { LiveWindow.run(); diff --git a/src/org/usfirst/frc/team2059/robot/RobotMap.java b/src/org/usfirst/frc/team2059/robot/RobotMap.java index ece7714..e350471 100644 --- a/src/org/usfirst/frc/team2059/robot/RobotMap.java +++ b/src/org/usfirst/frc/team2059/robot/RobotMap.java @@ -8,8 +8,8 @@ public class RobotMap { public static int driveRightEncoder = 0; public static int driveLeftEncoder = 1; //Arm - public static double zeroDegrees = 0.1; - public static double ninetyDegrees = 0.7; + public static double zeroDegrees = 2.08; + public static double ninetyDegrees = 3.54; public static int armPot = 0; public static int armLeftMotor = 5; public static int armRightMotor = 6; diff --git a/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java b/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java index 9800578..e7bd8d5 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java +++ b/src/org/usfirst/frc/team2059/robot/commands/CommandBase.java @@ -4,9 +4,9 @@ import org.usfirst.frc.team2059.robot.subsystems.EncoderBase; import org.usfirst.frc.team2059.robot.subsystems.MainArm; import edu.wpi.first.wpilibj.command.Command; public abstract class CommandBase extends Command { - protected static EncoderBase encoderBase; - protected static DriveBase driveBase; - protected static MainArm mainArm; + public static EncoderBase encoderBase; + public static DriveBase driveBase; + public static MainArm mainArm; public static void init() { encoderBase = new EncoderBase(); driveBase = new DriveBase(); diff --git a/src/org/usfirst/frc/team2059/robot/commands/MoveArm.java b/src/org/usfirst/frc/team2059/robot/commands/MoveArm.java index 9c5a0f7..5481bd5 100644 --- a/src/org/usfirst/frc/team2059/robot/commands/MoveArm.java +++ b/src/org/usfirst/frc/team2059/robot/commands/MoveArm.java @@ -16,17 +16,20 @@ public class MoveArm extends CommandBase { // Called repeatedly when this Command is scheduled to run protected void execute() { mainArm.moveArm(speed); + System.out.println(speed); } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { - return true; + return false; } // Called once after isFinished returns true protected void end() { + mainArm.moveArm(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() { + end(); } } // vim: sw=2:ts=2:sts=2 diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 54e9c9b..73ce303 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -1,17 +1,41 @@ package org.usfirst.frc.team2059.robot.subsystems; import org.usfirst.frc.team2059.robot.RobotMap; import org.usfirst.frc.team2059.robot.commands.Drive; -import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.CANTalon; -public class MainArm extends Subsystem { +import edu.wpi.first.wpilibj.AnalogInput; +public class MainArm extends PIDSubsystem { CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor); CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor); + AnalogInput pot = new AnalogInput(RobotMap.armPot); + public MainArm(){ + super("MainArm",0.0,0.0,0.0); + getPIDController().setContinuous(false); + } public void initDefaultCommand() { - setDefaultCommand(new Drive()); } public void moveArm(double speed){ armMotorLeft.set(speed); armMotorRight.set(-speed); } + protected double returnPIDInput(){ + return pot.getAverageVoltage(); + } + protected void usePIDOutput(double output){ + } + public double getRaw(){ + return pot.getAverageVoltage(); + } + public double getDegrees(){ + return potToDegrees(getRaw()); + } + private double potToDegrees(double pot){ + double min = RobotMap.zeroDegrees; + double max = RobotMap.ninetyDegrees; + double tmp = pot - max; + min = min - max; + max = max - max; + return 300 - ((tmp + max) / (300/min)); + } } // vim: sw=2:ts=2:sts=2