diff --git a/src/org/usfirst/frc/team2059/robot/Robot.java b/src/org/usfirst/frc/team2059/robot/Robot.java index e2fc6c1..11a2942 100644 --- a/src/org/usfirst/frc/team2059/robot/Robot.java +++ b/src/org/usfirst/frc/team2059/robot/Robot.java @@ -15,6 +15,7 @@ public class Robot extends IterativeRobot { oi = new OI(); chooser = new SendableChooser(); SmartDashboard.putData("Auto mode", chooser); + SmartDashboard.putData("MainArm",CommandBase.mainArm.getPIDController()); } public void disabledInit() { } @@ -39,6 +40,7 @@ public class Robot extends IterativeRobot { Scheduler.getInstance().run(); SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw()); SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees()); + System.out.println(CommandBase.mainArm.getDegrees()); } public void testPeriodic() { LiveWindow.run(); diff --git a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java index 25d2f5b..f0c212e 100644 --- a/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java +++ b/src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java @@ -9,19 +9,23 @@ public class MainArm extends PIDSubsystem { CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor); AnalogInput pot = new AnalogInput(RobotMap.armPot); public MainArm(){ - super("MainArm",0.0,0.0,0.0); + super("MainArm",0.06,0.0,0.002); getPIDController().setContinuous(false); + //setSetpoint(15); + enable(); } public void initDefaultCommand() { } public void moveArm(double speed){ - armMotorLeft.set(speed); - armMotorRight.set(-speed); + armMotorLeft.set(-speed); + armMotorRight.set(speed); } protected double returnPIDInput(){ - return pot.getAverageVoltage(); + return getDegrees(); } protected void usePIDOutput(double output){ + armMotorLeft.set(-output); + armMotorRight.set(output); } public double getRaw(){ return pot.getAverageVoltage();