diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index 6aa2da9..91c431f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -92,6 +92,7 @@ public class OI CloseIntake.whenPressed(new IntakePosition(false)); */ + SmartDashboard.putData("switch to manuel", new SetManual()); SmartDashboard.putData("run arm test", new ArmTest()); JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java new file mode 100644 index 0000000..10f1f3b --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetManual.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class SetManual extends CommandGroup { + /** + * Add your docs here. + */ + public SetManual() { + addSequential(new ArmSetMode(ArmControlMode.JOYSTICK_MANUAL)); + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 0324376..18b1594 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -84,8 +84,10 @@ public class Arm extends Subsystem implements ControlLoopable private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); - public static final double KF_UP = 0.005; - public static final double KF_DOWN = 0.0; + public static final double KF_UP = 0.06; + public static final double KF_DOWN = 0.01; + public static final double P_Value = 0.6; + public static final double I_Value = .0005; public static final double PID_ERROR_INCHES = 1.0; LimitSwitchSource limitSwitchSource; // Pneumatics @@ -98,6 +100,8 @@ public class Arm extends Subsystem implements ControlLoopable private double targetPositionInchesPID = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; + private double p = 0; + public Arm() { try { @@ -117,7 +121,7 @@ public class Arm extends Subsystem implements ControlLoopable motor2.setNeutralMode(NeutralMode.Brake); motorControllers.add(motor1); - + SmartDashboard.putNumber("arm P value", 0); } catch (Exception e) { @@ -152,6 +156,7 @@ public class Arm extends Subsystem implements ControlLoopable } public void setPositionPID(double targetPositionInches) { + mpController.setPIDSlot(PID_SLOT); updatePositionPID(targetPositionInches); setArmControlMode(ArmControlMode.JOYSTICK_PID); @@ -161,8 +166,13 @@ public class Arm extends Subsystem implements ControlLoopable public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); double startPositionInches = motor1.getPositionWorld(); - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - System.err.println("it should get here"); + //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + motor1.set(ControlMode.Position, targetPositionInches); + motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS); + motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); + System.err.println(motor1.getControlMode()); + //System.err.print(motor1.getClosedLoopError()); } public void setPositionMP(double targetPositionInches) { @@ -254,14 +264,17 @@ public class Arm extends Subsystem implements ControlLoopable // Do the update if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); + } else if (!isFinished) { if (armControlMode == ArmControlMode.MOTION_PROFILE) { isFinished = mpController.controlLoopUpdate(getPositionInches()); + } if (armControlMode == ArmControlMode.JOYSTICK_PID){ + System.err.println(motor1.getControlMode()); controlPidWithJoystick(); - System.err.println("test of pid"); + } @@ -286,6 +299,8 @@ public class Arm extends Subsystem implements ControlLoopable double deltaPosition = joystickPosition * joystickInchesPerMs; targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); + + } private void ControlWithJoystickhold(){ @@ -296,7 +311,7 @@ public class Arm extends Subsystem implements ControlLoopable private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick((joyStickSpeed*.30)/*+.1*/); + setSpeedJoystick((joyStickSpeed*.30)+.1); } /* public void setShiftState(ElevatorSpeedShiftState state) { @@ -344,15 +359,18 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println("the encoder is right after this"); try { - SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld()); - SmartDashboard.putNumber("Elevator Motor 1 Amps", motor1.getOutputCurrent()); - SmartDashboard.putNumber("Elevator Motor 2 Amps", motor2.getOutputCurrent()); - SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent()); + p = SmartDashboard.getNumber("arm P value", 0); + SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld()); + SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); + SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); + SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent()); + SmartDashboard.putNumber("arm pid error", motor1.getClosedLoopError()); + SmartDashboard.putNumber("arm motor output", motor1.getMotorOutputPercent()); // SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID)); // SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID)); // SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); - SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID); + SmartDashboard.putNumber("Arm Target PID Position", targetPositionInchesPID); } catch (Exception e) { System.err.println("Drivetrain update status error" +e.getMessage()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 0e1c3f0..8295059 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -102,7 +102,7 @@ public class Wrist extends Subsystem implements ControlLoopable public Wrist() { try { wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); - System.err.println("the tallon shold be made in wrist"); + System.err.println("the talon should be made in wrist"); wristmotor1.setInverted(true); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java index 6e66b74..205e6ad 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java @@ -218,14 +218,13 @@ public class MPTalonPIDController } public void setTarget(double position, double Kf) { - System.err.println("set target" + position); // Kf gets multipled by position in the Talon double KfPerPosition = Math.abs(position) > 0.001 ? Kf / position : 0; for (TalonSRXEncoder motorController : motorControllers) { motorController.config_kF(0, KfPerPosition, TalonSRXEncoder.TIMEOUT_MS); motorController.setWorld(ControlMode.Position, position); - + //System.err.println(motorController.getControlMode()); } }