added in motion magic, dose not work

find missing part in code

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java

@@ -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

@@ -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

@@ -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_UP = 0;//0.01;
-	public static final double KF_DOWN = 0.0;
+	public static final double KF_DOWN = 0;// 0.0;
-	public static final double P_Value = 2;
+	public static final double P_Value = 0;// 2;
-	public static final double I_Value = 0.00030;
+	public static final double I_Value = 0;// 0.00030;
-	public static final double D_Value = 200;
+	public static final double D_Value = 0;// 200;
-	public static final double RampRate = 0.0;
+	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 1 Amps", motor1.getOutputCurrent());
-				SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.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

@@ -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) {
+	}
 }