Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Fix Shooter PID

Browse Source
This commit is contained in:
ryan123rudder
2020-01-31 21:39:51 -07:00
parent 8a95c8a8cc
commit 42db77fe6d
6 changed files with 16 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Elevator.java
-78
View File
@@ -1,78 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Gains;
import frc4388.robot.Constants.ElevatorConstants;
public class Elevator extends SubsystemBase {
public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1);
public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2);
public static Gains m_elevatorGains = ElevatorConstants.ELEVATOR_GAINS;
/**
* Creates a new Elevator.
*/
public Elevator() {
//resets motors
m_talon1.configFactoryDefault();
m_talon2.configFactoryDefault();
//config following settings
m_talon2.follow(m_talon1);
m_talon1.setNeutralMode(NeutralMode.Brake);
m_talon2.setNeutralMode(NeutralMode.Brake);
m_talon1.setInverted(false);
m_talon2.setInverted(false);
m_talon1.setInverted(InvertType.FollowMaster);
m_talon2.setInverted(InvertType.FollowMaster);
setElevatorGains();
m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
int closedLoopTimeMs = 1;
m_talon1.configClosedLoopPeriod(0, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.configClosedLoopPeriod(1, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
public void moveElevator(double speed) {
m_talon1.set(speed);
m_talon2.set(speed);
}
public void setElevatorGains(){
m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX);
m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX);
m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS);
}
public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) {
talon.set(ControlMode.Position, targetPos);
}
}
src/main/java/frc4388/robot/subsystems/Shooter.java
+10 -3
View File
@@ -25,6 +25,7 @@ public class Shooter extends SubsystemBase {
public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS;
double velP;
/**
* Creates a new Shooter.
*/
@@ -76,7 +77,13 @@ public class Shooter extends SubsystemBase {
* @param falcon Motor to use
* @param targetVel Target velocity to run motor at
*/
public void runDrumShooterVelocityPID(double targetVel) {
m_shooterFalcon.set(TalonFXControlMode.Velocity, m_targetVel*ShooterConstants.ENCODER_TICKS_PER_REV/600);
public void runDrumShooterVelocityPID(double targetVel, double actualVel) {
velP = actualVel/targetVel;
if(velP < 0.1){
velP = 0.1;
}
double runSpeed = velP*(1-velP);
System.err.println(runSpeed);
m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/);
}
}
}