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

Revert "Merge branch 'Wrist' of https://github.com/Team4388/2019-Hit-or-Miss into Wrist"

Browse Source 
This reverts commit 47fa9bbf87, reversing
changes made to eebb5ecedf.
This commit is contained in:
Keenan D. Buckley
2019-02-15 17:06:04 -07:00
parent 47fa9bbf87
commit 1dab4878f0
13 changed files with 126 additions and 533 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+7 -7
View File
@@ -12,7 +12,7 @@ import org.usfirst.frc4388.robot.subsystems.Pnumatics;
import org.usfirst.frc4388.robot.subsystems.Wrist;
//import org.usfirst.frc4388.utility.ControlLooper;
import org.usfirst.frc4388.utility.Looper;
//import org.usfirst.frc4388.utility.control.RobotStateEstimator;
import org.usfirst.frc4388.utility.control.RobotStateEstimator;
import org.usfirst.frc4388.utility.math.RigidTransform2d;
import org.usfirst.frc4388.utility.control.RobotState;
@@ -32,7 +32,7 @@ public class Robot extends TimedRobot
public static OI oi;
//public static final Drive drive = Drive.getInstance();
public static final Drive drive = Drive.getInstance();
public static final Arm arm = Arm.getInstance();
@@ -71,9 +71,9 @@ public class Robot extends TimedRobot
setPeriod(Constants.kLooperDt * 2);
System.out.println("Main loop period = " + getPeriod());
oi = OI.getInstance();
//controlLoop.register(drive);
controlLoop.register(drive);
controlLoop.register(arm);
//controlLoop.register(RobotStateEstimator.getInstance());
controlLoop.register(RobotStateEstimator.getInstance());
operationModeChooser = new SendableChooser<OperationMode>();
operationModeChooser.addDefault("Competition", OperationMode.COMPETITION);
@@ -93,7 +93,7 @@ public class Robot extends TimedRobot
@Override
public void disabledInit() {
//drive.setLimeLED(false);
drive.setLimeLED(false);
}
@Override
public void disabledPeriodic() {
@@ -103,7 +103,7 @@ public class Robot extends TimedRobot
@Override
public void autonomousInit() {
controlLoop.start();
//drive.setIsRed(getAlliance().equals(Alliance.Red));
drive.setIsRed(getAlliance().equals(Alliance.Red));
arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES);
updateChoosers();
drive.endGyroCalibration();
@@ -171,7 +171,7 @@ public class Robot extends TimedRobot
public void updateStatus()
{
//drive.updateStatus(operationMode);
drive.updateStatus(operationMode);
arm.updateStatus(operationMode);
robotState.updateStatus(operationMode);
2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java
+9 -3
View File
@@ -2,8 +2,7 @@
package org.usfirst.frc4388.robot;
public class RobotMap
{
public class RobotMap {
// USB Port IDs
public static final int DRIVER_JOYSTICK_1_USB_ID = 0;
public static final int OPERATOR_JOYSTICK_1_USB_ID = 1;
@@ -12,14 +11,16 @@ public class RobotMap
public static final int copilot = 1;
// MOTORS
public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2;
public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3;
public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4;
public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5;
//carriage motors
public static final int WRIST_RIGHT_MOTOR_CAN_ID = 6;
public static final int WRIST_LEFT_MOTOR_CAN_ID = 6;
public static final int INTAKE_MOTOR = 7;
//Arm Motors
@@ -30,6 +31,8 @@ public class RobotMap
public static final int CLIMBER_LEFT = 12;
public static final int CLIMBER_RIGHT = 13;
// Pneumatics
public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0;
public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1;
@@ -39,4 +42,7 @@ public class RobotMap
public static final int LOWER_HATCH_INTAKE_PCM_ID = 5;
public static final int END_EFECTOR_EXPAND_PCM_ID = 6;
public static final int END_EFECTOR_CONTRACT_PCM_ID = 7;
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java
-53
View File
@@ -1,53 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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 org.usfirst.frc4388.robot.*;
import edu.wpi.first.wpilibj.command.Command;
public class FlipIntake extends Command
{
public FlipIntake()
{
requires(Robot.wrist);
}
// Called just before this Command runs the first time
@Override
protected void initialize()
{
Robot.wrist.controlPIDFlipIntake();
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute()
{
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished()
{
return false;
}
// Called once after isFinished returns true
@Override
protected void end()
{
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted()
{
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java
+18 -24
View File
@@ -7,35 +7,29 @@
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.WaitCommand;
/**
* Add your docs here.
*/
public class GrabBallOutOfRobot extends CommandGroup
{
public class GrabBallOutOfRobot extends CommandGroup {
/**
* Add your docs here.
*/
public GrabBallOutOfRobot()
{
//Add rest of sequentials for this command group
// Add Commands here:
// e.g. addSequential(new Command1());
// addSequential(new Command2());
// these will run in order.
addSequential(new FlipIntake());
//Move Arm until bar jump angle
// 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.
//Jump bar in robot
if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle)
{
addParallel(new WaitCommand(2));
addSequential(new RunWristMotorJumpBar());
addSequential(new StopWristMotor());
}
//continue arm movement until ball is in intake ///SENSOR for ball in robot
//Move ball out of robot to 360 degrees
//move arm and wrist 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/commands/GrabHatchOffWall.java
-36
View File
@@ -1,36 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* Add your docs here.
*/
public class GrabHatchOffWall extends CommandGroup
{
public GrabHatchOffWall() {
// 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/commands/GrabHatchOutOfFloorIntake.java
-37
View File
@@ -1,37 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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;
/**
* Add your docs here.
*/
public class GrabHatchOutOfFloorIntake extends CommandGroup
{
public GrabHatchOutOfFloorIntake()
{
// 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/commands/RunWristMotorJumpBar.java
-53
View File
@@ -1,53 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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 org.usfirst.frc4388.robot.*;
import edu.wpi.first.wpilibj.command.Command;
public class RunWristMotorJumpBar extends Command
{
public RunWristMotorJumpBar()
{
requires(Robot.wrist);
}
// Called just before this Command runs the first time
@Override
protected void initialize()
{
Robot.wrist.jumpBar();
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute()
{
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished()
{
return false;
}
// Called once after isFinished returns true
@Override
protected void end()
{
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted()
{
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java
-52
View File
@@ -1,52 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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 org.usfirst.frc4388.robot.*;
import edu.wpi.first.wpilibj.command.Command;
public class StopWristMotor extends Command
{
public StopWristMotor()
{
requires(Robot.wrist);
}
// Called just before this Command runs the first time
@Override
protected void initialize()
{
Robot.wrist.stopMotor();
}
// Called repeatedly when this Command is scheduled to run
@Override
protected void execute()
{
}
// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished()
{
return false;
}
// Called once after isFinished returns true
@Override
protected void end()
{
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
@Override
protected void interrupted() {
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+79 -82
View File
@@ -1,7 +1,6 @@
package org.usfirst.frc4388.robot.subsystems;
import java.util.ArrayList;
import org.usfirst.frc4388.controller.XboxController;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.RobotMap;
@@ -12,15 +11,6 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder;
import org.usfirst.frc4388.utility.TalonSRXFactory;
import com.ctre.phoenix.motorcontrol.ControlMode;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
@@ -30,9 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Arm extends Subsystem implements Loop
{
//PID encoder and motor
private CANTalonEncoder armMotor;
//private WPI_TalonSRX ArmLeft;
private static Arm instance;
public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL };
@@ -70,11 +58,10 @@ public class Arm extends Subsystem implements Loop
public static final double MP_T1 = 400; // Fast = 300
public static final double MP_T2 = 150; // Fast = 150
//PID controller Max Scale
private SoftwarePIDPositionController pidPositionControllerLowest;
//private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0);
private PIDParams PositionPLowest;
// Motor controllers
private ArrayList<TalonSRXEncoder> motorControllers = new ArrayList<TalonSRXEncoder>();
private TalonSRXEncoder motor1;
private TalonSRX motor2;
// PID controller and params
@@ -118,51 +105,67 @@ public class Arm extends Subsystem implements Loop
}
catch (Exception e) {
System.err.println("An error occurred in the DriveTrain constructor");
}
}
@Override
public void initDefaultCommand() {
}
public void resetZeroPosition(double position) {
mpController.resetZeroPosition(position);
}
private synchronized void setElevatorControlMode(ArmControlMode controlMode) {
this.elevatorControlMode = controlMode;
}
private synchronized ArmControlMode getElevatorControlMode() {
return this.elevatorControlMode;
}
public void setSpeed(double speed) {
motor1.set(ControlMode.PercentOutput, speed);
setElevatorControlMode(ArmControlMode.MANUAL);
}
public void setSpeedJoystick(double speed) {
motor1.set(ControlMode.PercentOutput, speed);
setElevatorControlMode(ArmControlMode.JOYSTICK_MANUAL);
}
public void setPositionPID(double targetPositionInches) {
mpController.setPIDSlot(PID_SLOT);
updatePositionPID(targetPositionInches);
setElevatorControlMode(ArmControlMode.JOYSTICK_PID);
setFinished(false);
}
public void updatePositionPID(double targetPositionInches) {
targetPositionInchesPID = limitPosition(targetPositionInches);
double startPositionInches = motor1.getPositionWorld();
mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN);
}
public void setPositionMP(double targetPositionInches) {
double startPositionInches = motor1.getPositionWorld();
mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2);
setFinished(false);
firstMpPoint = true;
setElevatorControlMode(ArmControlMode.MOTION_PROFILE);
}
//PID encoder position
public double getEncoderArmPosition()
{
return armMotor.getPositionWorld();
}
public double getArmHeightInchesAboveFloor()
{
return armMotor.getPositionWorld();
}
public synchronized void setControlMode(DriveControlMode controlMode)
{
this.controlMode = controlMode;
isFinished = false;
}
/*
public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError)
{
double ArmTargetPos = 0;
this.targetPPosition = ArmTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE??
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE);
}
public void setArmPIDLowScale(double ArmPosition, double maxError, double minError)
{
double ArmTargetPos = 0;
this.targetPPosition = ArmTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE);
}
public void setArmPIDSwitch(double ArmPosition, double maxError, double minError)
{
double ArmTargetPos = 0;
this.targetPPosition = ArmTargetPos;
pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError);
Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH);
private double limitPosition(double targetPosition) {
if (targetPosition < MIN_POSITION_INCHES) {
return MIN_POSITION_INCHES;
}
else if (targetPosition > MAX_POSITION_INCHES) {
return MAX_POSITION_INCHES;
}
return targetPosition;
}
@Override
@@ -174,9 +177,11 @@ public class Arm extends Subsystem implements Loop
mpController.setPID(pidPIDParamsHiGear, PID_SLOT);
mpController.setPIDSlot(PID_SLOT);
}
*/
public void rawSetOutput(double output){
armMotor.set(/*ControlMode.PercentOutput,*/ output);
@Override
public void onStop(double timestamp) {
// TODO Auto-generated method stub
}
@Override
@@ -204,24 +209,8 @@ public class Arm extends Subsystem implements Loop
default:
break;
}
pressed = true;
}
else
{
if (controlMode == DriveControlMode.STOP_MOTORS){
{
Robot.arm.setControlMode(DriveControlMode.JOYSTICK);
}
pressed = false;
}
}
}
//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
private void controlPidWithJoystick() {
double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
@@ -269,8 +258,11 @@ public class Arm extends Subsystem implements Loop
return isFinished;
}
public double getPeriodMs()
{
public synchronized void setFinished(boolean isFinished) {
this.isFinished = isFinished;
}
public double getPeriodMs() {
return periodMs;
}
@@ -286,10 +278,15 @@ public class Arm extends Subsystem implements Loop
// SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID));
SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID);
}
catch (Exception e)
{
System.err.println("Drivetrain update status error");
catch (Exception e) {
}
}
}
public static Arm getInstance() {
if(instance == null) {
instance = new Arm();
}
return instance;
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
-111
View File
@@ -930,114 +930,3 @@ public class Drive extends Subsystem implements Loop {
}
}
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
import frc.robot.commands.DriveCommand;
import com.revrobotics.CANSparkMax;
import com.revrobotics.ControlType;
import com.revrobotics.CANPIDController;
import com.revrobotics.CANEncoder;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.command.Subsystem;
/**
* Add your docs here.
*/
public class Drive extends Subsystem {
static double LOW_GEAR_RATIO = 4.821;//6.57992
static double HIGH_GEAR_RATIO = ;
static double CIRCUMFERENCE = 15.221;
double HIGH_INCHES_PER_REV = CIRCUMFERENCE/HIGH_GEAR_RATIO;
double LOW_INCHES_PER_REV = CIRCUMFERENCE/LOW_GEAR_RATIO;
int timeoutMs = 10;
// Front is the follow motor, and it is based on following the primary motor of its side.
public CANSparkMax leftDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless),
leftDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless),
rightDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, MotorType.kBrushless),
rightDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless);
public CANPIDController leftPID = new CANPIDController(leftDrivePrimary);
public CANPIDController rightPID = new CANPIDController(rightDrivePrimary);
public CANEncoder leftEncoder = new CANEncoder(leftDrivePrimary);
public CANEncoder rightEncoder = new CANEncoder(rightDrivePrimary);
@Override
public void initDefaultCommand() {
setDefaultCommand(new DriveCommand());
leftDriveFront.follow(leftDrivePrimary);
rightDriveFront.follow(rightDrivePrimary);
leftDrivePrimary.setCANTimeout(timeoutMs);
rightDrivePrimary.setCANTimeout(timeoutMs);
leftDrivePrimary.setMotorType(MotorType.kBrushless);
rightDrivePrimary.setMotorType(MotorType.kBrushless);
}
public void set(double left, double right) {
leftDrivePrimary.set(left);
rightDrivePrimary.set(right);
}
public double getLeftRevs() {
return leftEncoder.getPosition();
}
public double getRightRevs() {
return rightEncoder.getPosition();
}
public double lowinchesToRevs(double INCHES) {
return INCHES/LOW_INCHES_PER_REV;
}
public double highinchesToRevs(double INCHES) {
return INCHES/HIGH_INCHES_PER_REV;
}
// Takes an input of ticks
public void setPID(double left, double right) {
leftPID.setReference(left, ControlType.kPosition);
rightPID.setReference(right, ControlType.kPosition);
}
public void stop() {
leftDrivePrimary.set(0);
rightDrivePrimary.set(0);
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java
+11 -73
View File
@@ -26,11 +26,6 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.SensorCollection;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.wpilibj.command.WaitCommand;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.ControlMode;
@@ -41,7 +36,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode;
public class Wrist extends Subsystem
{
//Control Mode Array
public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL};
public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP_INTAKE, GRAB_BALL};
//Motor Controllers
private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();
@@ -77,19 +72,13 @@ public class Wrist extends Subsystem
public double armAngleDegrees = 90;
public static final double targetAngleDegreesBallIn = 180; //only have to flip intake and go down to get ball
///Change values
public static final double targetAngleDegreesBallIn = -45; ///Change values
public static final double targetAngleDegreesBallOut = 360;
public static final double targetAngleDegreesHatchIn = 130;
public static final double targetAngleDegreesHatchIn = 55;
public static final double targetAngleDegreesHatchOut = 0;
public final double jumpBarArmAngle = -50;
public static final double jumpBarAngle = 50; //hard limit switch?
public static final double armAngleForPIDSwitch = -45; ///Change values
public static final boolean ballIntakeOut = true;
//control mode for joystick control
private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
//Misc
private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL;
@@ -100,18 +89,12 @@ public class Wrist extends Subsystem
private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO;
private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO;
LimitSwitchSource limitSwitchSource;
public Wrist()
{
try
{
//PID wrist encoder and talon
wristRight = new CANTalonEncoder(RobotMap.WRIST_RIGHT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
//Limit Switch
wristRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
wristRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder);
}
catch(Exception e)
{
@@ -119,17 +102,12 @@ public class Wrist extends Subsystem
}
}
//Jump bar by putting power to the motors for a specific amount of time
//Jump bar output
public void jumpBar()
//Flipping the intake to the other side
public void flipIntake()
{
wristRight.set(0.8);
}
double currentWristAngle = wristRight.getPositionWorld();
//Stop wrist motor
public void stopMotor()
{
wristRight.set(0);
}
//Method for setting the control mode for the wrist
@@ -213,29 +191,8 @@ public class Wrist extends Subsystem
}
else if(armAngleDegrees <= armAngleForPIDSwitch)
{
if(ballIntakeOut)
{
if(armAngleDegrees > targetAngleDegreesBallIn)
{
controlPIDBallIn();
}
else
{
controlPIDBallOut();
}
}
else
{
if(armAngleDegrees > targetAngleDegreesHatchIn)
{
controlPIDBallIn();
}
else
{
controlPIDHatchOut();
}
}
}
controlPID();
break;
case JOYSTICK_MANUAL:
controlManualWithJoystick();
@@ -265,7 +222,7 @@ public class Wrist extends Subsystem
{
double joystickSpeed;
joystickSpeed = -Robot.oi.getOperatorController().getRightYAxis();
joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
setSpeedJoystick(joystickSpeed);
}
@@ -283,25 +240,6 @@ public class Wrist extends Subsystem
public void controlPIDBallIn()
{
updatePositionPID(targetAngleDegreesBallIn);
//Needs limit to lift up (so it can get over bar) in command group
}
public void controlPIDBallOut()
{
updatePositionPID(targetAngleDegreesBallOut);
//Needs to be tuned a lot
}
public void controlPIDHatchIn()
{
updatePositionPID(targetAngleDegreesHatchIn);
//Needs limit to lift up (so it can get over bar) in command group
}
public void controlPIDHatchOut()
{
updatePositionPID(targetAngleDegreesHatchOut);
//Needs to be tuned a lot
}
public synchronized boolean isFinished()
2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java
+1 -1
View File
@@ -96,7 +96,7 @@ public class MMTalonPIDController
// Update the set points
if (controlMode == MMControlMode.STRAIGHT) {
double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0;
double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, 1);
double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, Drive.TRACK_WIDTH_INCHES);
rightTarget = targetValue + deltaDistance;
leftTarget = targetValue - deltaDistance;
2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java
+1 -1
View File
@@ -35,7 +35,7 @@ public class PathGenerator {
Trajectory trajectory = Pathfinder.generate(points, config);
centerPoints = trajectory.segments;
TankModifier modifier = new TankModifier(trajectory).modify(1);
TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES);
leftPoints = modifier.getLeftTrajectory().segments;
rightPoints = modifier.getRightTrajectory().segments;