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
|
// 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;
|
||||||
|
@ -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();
|
||||||
|
@ -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;
|
||||||
|
@ -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();
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user