converted to pidsubsystem, builds but pottodegrees doesn't work
This commit is contained in:
parent
35b756efa4
commit
fe6dce9640
@ -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;
|
||||
|
@ -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();
|
||||
|
@ -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;
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user