Team4388/2019-Hit-or-Miss
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Hit-or-Miss.git synced 2026-06-09 08:38:06 -06:00
Code Issues Packages Projects Releases Wiki Activity

added in motion magic, dose not work

Browse Source 
find missing part in code
This commit is contained in:
lukesta182
2019-03-09 23:03:12 -07:00
parent 9852f11e2c
commit ca321c68fb
4 changed files with 141 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java
+56
View File
@@ -0,0 +1,56 @@
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import edu.wpi.first.wpilibj.command.Command;
/**
*
*/
public class ArmSetPositionMM extends Command {
private double targetPositionInches;
private boolean isAtTarget;
private static final double MIN_DELTA_TARGET = 3;
public ArmSetPositionMM(double targetPositionInches) {
this.targetPositionInches = targetPositionInches;
requires(Robot.arm);
}
// Called just before this Command runs the first time
protected void initialize() {
if (Math.abs(targetPositionInches - Robot.arm.getPositionInches()) < MIN_DELTA_TARGET) {
isAtTarget = true;
}
else {
isAtTarget = false;
Robot.arm.setPositionMM(targetPositionInches);
}
// System.out.println("Arm set MP initialized, target = " + targetPositionInches);
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return isAtTarget || Robot.arm.isFinished();
}
// Called once after isFinished returns true
protected void end() {
Robot.arm.setPositionMM(Robot.arm.getPositionInches());
// System.out.println("Arm set MP end");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
// System.out.println("ArmSetPositionMP interrupted");
Robot.arm.setFinished(true);
Robot.arm.setPositionMM(Robot.arm.getPositionInches());
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java
+1 -1
View File
@@ -14,7 +14,7 @@ public class ArmTest extends CommandGroup {
* Add your docs here.
*/
public ArmTest() {
addSequential(new ArmSetPositionPID(600));
addSequential(new ArmSetPositionMM(600));
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+81 -61
View File
@@ -30,7 +30,7 @@ public class Arm extends Subsystem implements ControlLoopable
{
private static Arm instance;
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC};
// 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 = (1);
@@ -84,12 +84,18 @@ 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.01;
public static final double KF_DOWN = 0.0;
public static final double P_Value = 2;
public static final double I_Value = 0.00030;
public static final double D_Value = 200;
public static final double RampRate = 0.0;
public static final double KF_UP = 0;//0.01;
public static final double KF_DOWN = 0;// 0.0;
public static final double P_Value = 0;// 2;
public static final double I_Value = 0;// 0.00030;
public static final double D_Value = 0;// 200;
public static final double RampRate = 0;// 0.0;
public static final int A_value = 25;
public static final int CV_value = 25;
private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later
public static final double PID_ERROR_INCHES = 5;
LimitSwitchSource limitSwitchSource;
@@ -101,6 +107,7 @@ public class Arm extends Subsystem implements ControlLoopable
private boolean isFinished;
private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL;
private double targetPositionInchesPID = 0;
private double targetPositionInchesMM = 0;
private boolean firstMpPoint;
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
private double p = 0;
@@ -125,6 +132,7 @@ public class Arm extends Subsystem implements ControlLoopable
motor2.setNeutralMode(NeutralMode.Brake);
motor1.enableCurrentLimit(true);
motorControllers.add(motor1);
//motor1.setSelectedSensorPosition(0, , );
}
@@ -161,6 +169,55 @@ public class Arm extends Subsystem implements ControlLoopable
motor1.set(ControlMode.PercentOutput, speed);
setArmControlMode(ArmControlMode.JOYSTICK_MANUAL);
}
public void setPositionMM(double targetPositionInches){
motor1.set(ControlMode.MotionMagic, targetPositionInches);
System.err.println(motor1.getControlMode());
setArmControlMode(ArmControlMode.MOTION_MAGIC);
updatePositionMM(targetPositionInches);
setFinished(false);
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
System.err.println("should get here");
}
public void updatePositionMM(double targetPositionInches){
targetPositionInchesMM = limitPosition(targetPositionInches);
double startPositionInches = motor1.getPositionWorld();
motor1.set(ControlMode.MotionMagic, targetPositionInches);
motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS);
motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS);
motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS);
motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS);
motor1.selectProfileSlot(1, 0);
motor1.config_kF(1, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kP(1, P_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kI(1, I_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kD(1, D_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS);
motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS);
}
public void setPositionPID(double targetPositionInches) {
motor1.set(ControlMode.Position, targetPositionInches);
@@ -212,51 +269,6 @@ public class Arm extends Subsystem implements ControlLoopable
mpController.setPIDSlot(PID_SLOT);
this.periodMs = periodMs;
}
/*@Override
public void onStart(double timestamp) {
mpController = new MPTalonPIDController(periodMs, motorControllers);
mpController.setPID(mpPIDParams, MP_SLOT);
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
mpController.setPIDSlot(PID_SLOT);
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
mpController.setPIDSlot(PID_SLOT);
}
@Override
public void onStop(double timestamp) {
// TODO Auto-generated method stub
}
@Override
public void onLoop(double timestamp) {
synchronized (Arm.this) {
switch( getElevatorControlMode() ) {
case JOYSTICK_PID:
controlPidWithJoystick();
break;
case JOYSTICK_MANUAL:
controlManualWithJoystick();
break;
case MOTION_PROFILE:
if (!isFinished()) {
if (firstMpPoint) {
mpController.setPIDSlot(MP_SLOT);
firstMpPoint = false;
}
setFinished(mpController.controlLoopUpdate());
if (isFinished()) {
mpController.setPIDSlot(PID_SLOT);
}
}
break;
default:
break;
}
}
}
*/
@@ -280,12 +292,12 @@ public class Arm extends Subsystem implements ControlLoopable
}
if (armControlMode == ArmControlMode.JOYSTICK_PID){
//System.err.println(motor1.getControlMode());
if (Robot.oi.getOperatorController().getDpadAngle() == -1){
controlPidWithJoystick();
} else {
}
System.err.println(motor1.getControlMode());
}
if (armControlMode == ArmControlMode.MOTION_MAGIC){
controlMMWithJoystick();
System.err.println(motor1.getControlMode());
}
/*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) {
@@ -311,6 +323,14 @@ public class Arm extends Subsystem implements ControlLoopable
updatePositionPID(targetPositionInchesPID);
}
private void controlMMWithJoystick() {
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
double deltaPosition = joystickPosition * joystickInchesPerMs;
targetPositionInchesMM = targetPositionInchesMM + deltaPosition;
updatePositionMM(targetPositionInchesMM);
}
private void ControlWithJoystickhold(){
@@ -370,19 +390,19 @@ public class Arm extends Subsystem implements ControlLoopable
try {
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 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("Arm Target MM Position", targetPositionInchesMM);
// 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("Arm Target PID Position", targetPositionInchesPID);
}
catch (Exception e) {
System.err.println("Drivetrain update status error" +e.getMessage());
System.err.println("Arm update status error" +e.getMessage());
}
}
2019robot/src/main/java/org/usfirst/frc4388/utility/TalonSRXEncoder.java
+3
View File
@@ -81,4 +81,7 @@ public class TalonSRXEncoder extends WPI_TalonSRX
public double getVelocityWorld() {
return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(PID_IDX) / 0.1);
}
public void configNominalOutputVoltage(float f, float g) {
}
}