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

changes to arm, make it able to work and not time out

Browse Source 
times out becase of choosers in robot
This commit is contained in:
lukesta182
2019-01-31 19:37:31 -07:00
parent 6b51a76153
commit f6426c9324
4 changed files with 74 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java
+1 -20
View File
@@ -41,15 +41,6 @@ public class Robot extends IterativeRobot
public static OperationMode operationMode = OperationMode.TEST;
private SendableChooser<OperationMode> operationModeChooser;
private SendableChooser<Command> RRautonTaskChooser;
private SendableChooser<Command> RLautonTaskChooser;
private SendableChooser<Command> LRautonTaskChooser;
private SendableChooser<Command> LLautonTaskChooser;
private Command RRautonomousCommand;
private Command RLautonomousCommand;
private Command LRautonomousCommand;
private Command LLautonomousCommand;
public void robotInit()
{
@@ -71,8 +62,7 @@ public class Robot extends IterativeRobot
operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
SmartDashboard.putData("Operation Mode", operationModeChooser);
//Initializing the angle of the arm to be
arm.setInitAngle();
//ledLights.setAllLightsOn(false);
}
@@ -93,7 +83,6 @@ public class Robot extends IterativeRobot
public void autonomousInit() {
updateChoosers();
controlLoop.start();
drive.endGyroCalibration();
drive.resetEncoders();
@@ -110,10 +99,6 @@ public class Robot extends IterativeRobot
}
public void teleopInit() {
if (RRautonomousCommand != null) RRautonomousCommand.cancel();
if (RLautonomousCommand != null) RLautonomousCommand.cancel();
if (LRautonomousCommand != null) LRautonomousCommand.cancel();
if (LLautonomousCommand != null) LLautonomousCommand.cancel();
drive.setToBrakeOnNeutral(false);
updateChoosers();
controlLoop.start();
@@ -137,10 +122,6 @@ public class Robot extends IterativeRobot
private void updateChoosers() {
operationMode = (OperationMode)operationModeChooser.getSelected();
RRautonomousCommand = (Command)RRautonTaskChooser.getSelected();
RLautonomousCommand = (Command)RLautonTaskChooser.getSelected();
LRautonomousCommand = (Command)LRautonTaskChooser.getSelected();
LLautonomousCommand = (Command)LLautonTaskChooser.getSelected();
}
public Alliance getAlliance()
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java
+2 -1
View File
@@ -2,6 +2,7 @@ package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode;
import java.lang.Math;
import edu.wpi.first.wpilibj.command.Command;
@@ -20,7 +21,7 @@ public class ArmSetMode extends Command {
// Called just before this Command runs the first time
protected void initialize() {
if (controlMode == ArmControlMode.PID) {
Robot.wrist.setPositionPID(27);
Robot.wrist.setPositionPID(Robot.arm.getYPositionInches());
}
else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) {
Robot.wrist.setSpeedJoystick(0);
2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java
+42
View File
@@ -0,0 +1,42 @@
package org.usfirst.frc4388.robot.commands;
import org.usfirst.frc4388.robot.Robot;
import org.usfirst.frc4388.robot.subsystems.Arm;
import edu.wpi.first.wpilibj.command.Command;
/**
*
*/
public class ArmSetPositionPID extends Command {
private double targetPositionInches;
public ArmSetPositionPID(double targetPositionInches) {
this.targetPositionInches = targetPositionInches;
requires(Robot.arm);
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.arm.setPositionPID(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 Math.abs(Robot.arm.getYPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES;
}
// Called once after isFinished returns true
protected void end() {
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
}
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java
+29 -21
View File
@@ -1,6 +1,7 @@
package org.usfirst.frc4388.robot.subsystems;
import java.util.ArrayList;
import java.lang.Math;
import org.usfirst.frc4388.robot.Constants;
import org.usfirst.frc4388.robot.Robot;
@@ -14,6 +15,12 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource;
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.Subsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -23,10 +30,15 @@ public class Arm extends Subsystem implements Loop
private static Arm instance;
public static enum ArmControlMode {PID, JOYSTICK_MANUAL };
public static final double DEGREE_START_OFFSET = 70;
public static final double RADIUS_OF_ARM = 43.3;
public static final double OFF_SET_BELOW = 49.3;
// One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks
public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI);
public static final double ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3))-(DEGREE_START_OFFSET);
public static final double X_POSITION_MATH = ((RADIUS_OF_ARM)*(Math.cos(ENCODER_TICKS_TO_DEGREES)))+(OFF_SET_BELOW);
public static final double Y_POSITION_MATH = (RADIUS_OF_ARM)*(Math.sin(ENCODER_TICKS_TO_DEGREES));
public double ARM_ANGLE_DEGREES = 0;
@@ -34,17 +46,14 @@ public class Arm extends Subsystem implements Loop
public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75;
// Defined positions
public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0;
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 AFTER_INTAKE_POSITION_INCHES = 4.0;
LimitSwitchSource limitSwitchSource;
// Motion profile max velocities and accel times
public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60;
public static final double MP_T1 = 400; // Fast = 300
public static final double MP_T2 = 150; // Fast = 150
// Motor controllers
@@ -80,22 +89,13 @@ public class Arm extends Subsystem implements Loop
//motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID);
motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0);
}
catch (Exception e) {
System.err.println("An error occurred in the DriveTrain constructor");
}
}
//Set the degree to negative angle after initializing
public void setInitAngle()
{
double armAngleToHoriz = 70;
double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz;
ARM_ANGLE_DEGREES = initAngle;
}
@Override
public void initDefaultCommand() {
}
@@ -162,29 +162,37 @@ public class Arm extends Subsystem implements Loop
case PID:
controlPidWithJoystick();
break;
default: JOYSTICK_MANUAL:
case JOYSTICK_MANUAL:
controlManualWithJoystick();
break;
}
}
}
private void controlPidWithJoystick() {
//double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis();
//double deltaPosition = joystickPosition *.5;
targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition;
updatePositionPID(targetPositionInchesPID);
}
private void controlManualWithJoystick() {
double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis();
setSpeedJoystick(joyStickSpeed);
}
public double getPositionInches() {
return motor1.getPositionWorld();
public double getYPositionInches() {
return motor1.getPositionWorld()*Y_POSITION_MATH;
}
public double getXPositionInches() {
return motor1.getPositionWorld()*X_POSITION_MATH;
}
// public double getAverageMotorCurrent() {