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
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;

View File

@ -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();

View File

@ -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;

View File

@ -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();

View File

@ -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

View File

@ -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