converted to pidsubsystem, builds but pottodegrees doesn't work

This commit is contained in:
Adam Long 2016-09-11 20:47:05 -04:00
parent 35b756efa4
commit fe6dce9640
6 changed files with 40 additions and 11 deletions

View File

@ -25,8 +25,8 @@ public class OI {
} }
// Print log when button 1 pressed // Print log when button 1 pressed
joystickButtons[0][0].whenPressed(new LogEncoder()); joystickButtons[0][0].whenPressed(new LogEncoder());
joystickButtons[1][0].whenPressed(new MoveArm(0.25)); joystickButtons[1][0].whileHeld(new MoveArm(0.5));
joystickButtons[1][1].whenPressed(new MoveArm(-0.25)); joystickButtons[1][1].whileHeld(new MoveArm(-0.5));
} }
public Joystick[] getJoysticks() { public Joystick[] getJoysticks() {
return joysticks; return joysticks;

View File

@ -37,6 +37,8 @@ public class Robot extends IterativeRobot {
} }
public void teleopPeriodic() { public void teleopPeriodic() {
Scheduler.getInstance().run(); Scheduler.getInstance().run();
SmartDashboard.putNumber("ArmAngleRaw",CommandBase.mainArm.getRaw());
SmartDashboard.putNumber("ArmAngleDegrees",CommandBase.mainArm.getDegrees());
} }
public void testPeriodic() { public void testPeriodic() {
LiveWindow.run(); LiveWindow.run();

View File

@ -8,8 +8,8 @@ public class RobotMap {
public static int driveRightEncoder = 0; public static int driveRightEncoder = 0;
public static int driveLeftEncoder = 1; public static int driveLeftEncoder = 1;
//Arm //Arm
public static double zeroDegrees = 0.1; public static double zeroDegrees = 2.08;
public static double ninetyDegrees = 0.7; public static double ninetyDegrees = 3.54;
public static int armPot = 0; public static int armPot = 0;
public static int armLeftMotor = 5; public static int armLeftMotor = 5;
public static int armRightMotor = 6; public static int armRightMotor = 6;

View File

@ -4,9 +4,9 @@ import org.usfirst.frc.team2059.robot.subsystems.EncoderBase;
import org.usfirst.frc.team2059.robot.subsystems.MainArm; import org.usfirst.frc.team2059.robot.subsystems.MainArm;
import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Command;
public abstract class CommandBase extends Command { public abstract class CommandBase extends Command {
protected static EncoderBase encoderBase; public static EncoderBase encoderBase;
protected static DriveBase driveBase; public static DriveBase driveBase;
protected static MainArm mainArm; public static MainArm mainArm;
public static void init() { public static void init() {
encoderBase = new EncoderBase(); encoderBase = new EncoderBase();
driveBase = new DriveBase(); driveBase = new DriveBase();

View File

@ -16,17 +16,20 @@ public class MoveArm extends CommandBase {
// Called repeatedly when this Command is scheduled to run // Called repeatedly when this Command is scheduled to run
protected void execute() { protected void execute() {
mainArm.moveArm(speed); mainArm.moveArm(speed);
System.out.println(speed);
} }
// Make this return true when this Command no longer needs to run execute() // Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() { protected boolean isFinished() {
return true; return false;
} }
// Called once after isFinished returns true // Called once after isFinished returns true
protected void end() { protected void end() {
mainArm.moveArm(0);
} }
// Called when another command which requires one or more of the same // Called when another command which requires one or more of the same
// subsystems is scheduled to run // subsystems is scheduled to run
protected void interrupted() { protected void interrupted() {
end();
} }
} }
// vim: sw=2:ts=2:sts=2 // vim: sw=2:ts=2:sts=2

View File

@ -1,17 +1,41 @@
package org.usfirst.frc.team2059.robot.subsystems; package org.usfirst.frc.team2059.robot.subsystems;
import org.usfirst.frc.team2059.robot.RobotMap; import org.usfirst.frc.team2059.robot.RobotMap;
import org.usfirst.frc.team2059.robot.commands.Drive; 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; 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 armMotorLeft = new CANTalon(RobotMap.armLeftMotor);
CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor); 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() { public void initDefaultCommand() {
setDefaultCommand(new Drive());
} }
public void moveArm(double speed){ public void moveArm(double speed){
armMotorLeft.set(speed); armMotorLeft.set(speed);
armMotorRight.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 // vim: sw=2:ts=2:sts=2