From 009e3d1443fbb636178f3ce2494f96263cec7a59 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Tue, 5 Mar 2019 19:59:15 -0700 Subject: [PATCH] work on arm pid started on arm pid, as of now code gets to function in comand and passes to mp tallon pid controler, then does ????? --- .../java/org/usfirst/frc4388/robot/OI.java | 6 +++- .../java/org/usfirst/frc4388/robot/Robot.java | 1 + .../frc4388/robot/commands/ArmTest.java | 35 +++++++++++++++++++ .../usfirst/frc4388/robot/subsystems/Arm.java | 17 ++++++--- .../frc4388/robot/subsystems/Climber.java | 2 +- .../frc4388/robot/subsystems/Drive.java | 4 +-- .../frc4388/utility/MPTalonPIDController.java | 2 ++ 7 files changed, 58 insertions(+), 9 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java 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 12290ae..6aa2da9 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -85,11 +85,15 @@ public class OI //Operator Xbox /* JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - openIntake.whenPressed(new IntakePosition(true)); + openIntake.whenPressed(new IntakePosition(true)); + s JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); CloseIntake.whenPressed(new IntakePosition(false)); */ + + SmartDashboard.putData("run arm test", new ArmTest()); + JoystickButton safteySwitch = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.START_BUTTON); safteySwitch.whenPressed(new setClimberSafety(true)); safteySwitch.whenReleased(new setClimberSafety(false)); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index a693693..59c7131 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -145,6 +145,7 @@ public class Robot extends IterativeRobot } public void updateStatus() { + arm.updateStatus(operationMode); drive.updateStatus(operationMode); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java new file mode 100644 index 0000000..28341e2 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj.command.CommandGroup; + +public class ArmTest extends CommandGroup { + /** + * Add your docs here. + */ + public ArmTest() { + addSequential(new ArmSetPositionPID(1110)); + // Add Commands here: + // 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 6dca065..e5b58fa 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 @@ -33,7 +33,7 @@ public class Arm extends Subsystem implements ControlLoopable public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks - public static final double ENCODER_TICKS_TO_INCHES = ((360/4096)/(3))-60; + public static final double ENCODER_TICKS_TO_INCHES = (1); private double periodMs; @@ -52,7 +52,7 @@ public class Arm extends Subsystem implements ControlLoopable public static final double ZERO_POSITION_INCHES = -0.25; public static final double NEAR_ZERO_POSITION_INCHES = 0.0; public static final double MIN_POSITION_INCHES = 0.0; - public static final double MAX_POSITION_INCHES = 83.4; + public static final double MAX_POSITION_INCHES = 4096; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; public static final double SWITCH_POSITION_INCHES = 24.0; @@ -108,7 +108,7 @@ public class Arm extends Subsystem implements ControlLoopable motor1.setInverted(true); motor2.setInverted(true); - + // if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); // } @@ -163,6 +163,7 @@ public class Arm extends Subsystem implements ControlLoopable targetPositionInchesPID = limitPosition(targetPositionInches); double startPositionInches = motor1.getPositionWorld(); mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + System.err.println("it should get here"); } public void setPositionMP(double targetPositionInches) { @@ -259,6 +260,11 @@ public class Arm extends Subsystem implements ControlLoopable if (controlMode == ArmControlMode.MOTION_PROFILE) { isFinished = mpController.controlLoopUpdate(getPositionInches()); } + if (controlMode == ArmControlMode.JOYSTICK_PID){ + controlPidWithJoystick(); + System.err.println("test of pid"); + + } /*else if (controlMode == ArmControlMode.MP_PATH_VELOCITY) { isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); @@ -291,7 +297,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) { @@ -336,7 +342,9 @@ public class Arm extends Subsystem implements ControlLoopable } public void updateStatus(Robot.OperationMode operationMode) { + //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()); @@ -348,6 +356,7 @@ public class Arm extends Subsystem implements ControlLoopable SmartDashboard.putNumber("Elevator 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/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java index c498fd2..00e53d9 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -115,7 +115,7 @@ public class Climber extends Subsystem{ climberFront.set(leftTriggerInput * 0.7); } else { - climberBack.set(rightTriggerInput*.3); + climberBack.set(rightTriggerInput); } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index f404f20..5fdc9b4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -42,9 +42,6 @@ import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DoubleSolenoid; @@ -870,6 +867,7 @@ public class Drive extends Subsystem implements ControlLoopable public void updateStatus(Robot.OperationMode operationMode) { if (operationMode == Robot.OperationMode.TEST) { try { + SmartDashboard.putNumber("Gyro Value", getGyroAngleDeg()); SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); 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 1461505..6e66b74 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java @@ -218,12 +218,14 @@ 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); + } }