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

work on arm pid

Browse Source 
started on arm pid, as of now code gets to function in comand and passes to mp tallon pid controler, then does ?????
This commit is contained in:
lukesta182
2019-03-05 19:59:15 -07:00
parent ba554eb1b4
commit 009e3d1443
7 changed files with 58 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java
+5 -1
View File
@@ -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));
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+1
View File
@@ -145,6 +145,7 @@ public class Robot extends IterativeRobot
}
public void updateStatus() {
arm.updateStatus(operationMode);
drive.updateStatus(operationMode);
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java
+35
View File
@@ -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.
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+13 -4
View File
@@ -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());
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java
+1 -1
View File
@@ -115,7 +115,7 @@ public class Climber extends Subsystem{
climberFront.set(leftTriggerInput * 0.7);
}
else {
climberBack.set(rightTriggerInput*.3);
climberBack.set(rightTriggerInput);
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+1 -3
View File
@@ -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);
2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java
+2
View File
@@ -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);
}
}