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

motion maggic works

Browse Source
This commit is contained in:
lukesta182
2019-03-12 22:25:21 -06:00
parent 4de3492ed0
commit 22a9cfa9b2
6 changed files with 89 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java
+1
View File
@@ -97,6 +97,7 @@ public class OI
SmartDashboard.putData("switch to manuel", new SetManual());
SmartDashboard.putData("run arm test", new ArmTest());
SmartDashboard.putData("run arm2", new ArmTest2());
SmartDashboard.putData("wrist test", new wristTest());
//SmartDashboard.putData("PRE GAME!!!!", new PreGame());
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 ArmSetPositionMM(600));
addSequential(new ArmSetPositionMM(1200));
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java
+34
View File
@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* 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 ArmTest2 extends CommandGroup {
/**
* Add your docs here.
*/
public ArmTest2() {
addSequential(new ArmSetPositionMM(600));
// 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.
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+42 -42
View File
@@ -12,6 +12,7 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder;
import org.usfirst.frc4388.utility.TalonSRXFactory;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
@@ -81,18 +82,21 @@ public class Arm extends Subsystem implements ControlLoopable
private MPTalonPIDController mpController;
public static int PID_SLOT = 0;
public static int MP_SLOT = 1;
public static int MM_SLOT = 1;
public static int MP_SLOT = 2;
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;//0.01;
public static final double KF_UP = 1;//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 P_Value = 0.5;// 2;
public static final double I_Value = 0.0001;// 0.00030;
public static final double D_Value = 0;// 200;
public static final double F_Value = 0.75; // 1023 / 1360 max speed (ticks/100ms)
public static final double maxGravityComp = 0.08;
public static final double RampRate = 0;// 0.0;
public static final int A_value = 25;
public static final int CV_value = 25;
public static final int A_value = 100;
public static final int CV_value = 340;
@@ -128,6 +132,17 @@ public class Arm extends Subsystem implements ControlLoopable
// Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false);
// }
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(MM_SLOT, 0);
motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS);
motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
motor1.setNeutralMode(NeutralMode.Brake);
@@ -174,50 +189,33 @@ public class Arm extends Subsystem implements ControlLoopable
public void setPositionMM(double targetPositionInches){
motor1.set(ControlMode.MotionMagic, targetPositionInches);
System.err.println(motor1.getControlMode());
motor1.selectProfileSlot(MM_SLOT, 0);
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 double calcGravityCompensationAtCurrentPosition() {
int ticks = motor1.getSelectedSensorPosition();
double degreesFromDown = (ticks+920)*(360.0/(4096*3));
double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180);
System.err.println("comp(" + degreesFromDown + "^) = " + compensation);
return compensation;
}
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);
//double startPositionInches = motor1.getPositionWorld();
double compensation = calcGravityCompensationAtCurrentPosition();
//System.err.println("compensation = " + compensation);
// motor1.set(ControlMode.MotionMagic, targetPositionInches);
motor1.set(ControlMode.MotionMagic, targetPositionInches, DemandType.ArbitraryFeedForward, compensation);
//System.err.println(motor1.getControlMode());
motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS);
motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS);
}
@@ -306,7 +304,7 @@ public class Arm extends Subsystem implements ControlLoopable
}
if (armControlMode == ArmControlMode.MOTION_MAGIC){
controlMMWithJoystick();
System.err.println(motor1.getControlMode());
//System.err.println(motor1.getControlMode());
}
/*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) {
@@ -348,7 +346,7 @@ public class Arm extends Subsystem implements ControlLoopable
private void controlManualWithJoystick() {
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
setSpeedJoystick((joyStickSpeed*.30)+.1);
setSpeedJoystick((joyStickSpeed*.3)+.08);
}
/*
public void setShiftState(ElevatorSpeedShiftState state) {
@@ -399,10 +397,12 @@ public class Arm extends Subsystem implements ControlLoopable
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("sensor vel", motor1.getSelectedSensorVelocity());
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("arm 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));
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+5 -5
View File
@@ -879,7 +879,7 @@ public class Drive extends Subsystem implements ControlLoopable
//SmartDashboard.putNumber("Left Pos Inches", encoderLeft.getPosition());//leftDrive1.getPositionWorld());
//SmartDashboard.putNumber("Right Vel Ft-Sec", 0);//rightDrive1.getVelocityWorld() / 12);
//SmartDashboard.putNumber("Left Vel Ft-Sec", 0);//leftDrive1.getVelocityWorld() / 12);
SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD);
//SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD);
//SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg());
SmartDashboard.putNumber("Yaw Angle Deg", Math.abs(getGyroAngleDeg() % 360));
//MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint();
@@ -887,12 +887,12 @@ public class Drive extends Subsystem implements ControlLoopable
//SmartDashboard.putNumber("Gyro Delta", delta);
//SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating);
SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating());
SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg);
SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg());
// SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg);
//SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg());
SmartDashboard.putNumber("Steer Output", m_steerOutput);
SmartDashboard.putNumber("Move Output", m_moveOutput);
SmartDashboard.putNumber("Steer Input", m_steerInput);
SmartDashboard.putNumber("Move Input", m_moveInput);
//SmartDashboard.putNumber("Steer Input", m_steerInput);
//SmartDashboard.putNumber("Move Input", m_moveInput);
SmartDashboard.putString("MODE", "TEST");
//if (left_distance.getPosition() != 0 && right_distance.getPosition() != 0){
// SmartDashboard.putNumber("Distance Inches", (left_distance.getPosition()-right_distance.getPosition())/2);//rightDrive1.getPositionWorld());
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+6 -5
View File
@@ -85,12 +85,12 @@ public class Wrist extends Subsystem implements ControlLoopable
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 = .5;
public static final double I_Value = 0.00300;
public static final double D_Value = 200;
public static final double P_Value = 2.3;
public static final double I_Value = 0.00004;
public static final double D_Value = 96;
public static final double RampRate = 0.0;
private PIDParams wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later
public static final double PID_ERROR_INCHES = 5.0;
public static final double PID_ERROR_INCHES = 150;
LimitSwitchSource limitSwitchSource;
// Pneumatics
private Solenoid speedShift;
@@ -140,6 +140,7 @@ public class Wrist extends Subsystem implements ControlLoopable
wristMotor1.setPosition(0);
}
private synchronized void setWristControlMode(WristControlMode controlMode) {
this.wristControlMode = controlMode;
}
@@ -316,7 +317,7 @@ public class Wrist extends Subsystem implements ControlLoopable
private void controlManualWithJoystick() {
double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
setSpeedJoystick(joyStickSpeed);
setSpeedJoystick(joyStickSpeed*.5);
}
/*
public void setShiftState(ElevatorSpeedShiftState state) {