diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..38d3bba --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ + +proj/FRC7146_Java/bin/ + +proj/FRC7146_Java/dist/ + +doc/2017RobotQuickBuild/ diff --git a/proj/FRC7146_Java/.classpath b/proj/FRC7146_Java/.classpath index 6134915..92eec40 100644 --- a/proj/FRC7146_Java/.classpath +++ b/proj/FRC7146_Java/.classpath @@ -7,5 +7,9 @@ + + + + diff --git a/proj/FRC7146_Java/RobotBuilder.yaml b/proj/FRC7146_Java/RobotBuilder.yaml new file mode 100644 index 0000000..787edb0 --- /dev/null +++ b/proj/FRC7146_Java/RobotBuilder.yaml @@ -0,0 +1,64 @@ +Version 2.0 + +--- +Children: +- Children: [] + Properties: {} + Base: Subsystems + Name: Subsystems +- Children: [] + Properties: {} + Base: OI + Name: Operator Interface +- Children: + - Children: [] + Properties: + Autonomous Selection: !!robotbuilder.data.properties.BooleanProperty {default: false, + name: Autonomous Selection, validators: null, value: false} + Parameters: !!robotbuilder.data.properties.ParametersProperty + default: [] + name: Parameters + validators: null + value: [] + Run When Disabled: !!robotbuilder.data.properties.BooleanProperty {default: false, + name: Run When Disabled, validators: null, value: false} + Requires: !!robotbuilder.data.properties.TypeSelectionProperty {default: None, + name: Requires, type: Subsystem, validators: null, value: None} + Parameter presets: !!robotbuilder.data.properties.ParameterSetProperty + default: [] + name: Parameter presets + validators: null + value: [] + Button on SmartDashboard: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Button on SmartDashboard, validators: null, value: true} + Base: Command + Name: Autonomous Command + Properties: {} + Base: Commands + Name: Commands +Properties: + Eclipse Workspace: !!robotbuilder.data.properties.FileProperty {default: /home/d0048/workspace, + extension: null, folder: true, name: Eclipse Workspace, validators: null, value: /home/d0048/Desktop/FRC2018/FRC/proj} + Export Commands: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Export Commands, validators: null, value: true} + Autonomous command parameters: !!robotbuilder.data.properties.ParametersProperty + default: [] + name: Autonomous command parameters + validators: null + value: [] + Java Package: !!robotbuilder.data.properties.StringProperty {default: org.usfirst.frc0000, + name: Java Package, validators: null, value: org.usfirst.frc7146} + Use Default Java Package: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Use Default Java Package, validators: null, value: true} + Team Number: !!robotbuilder.data.properties.IntegerProperty {default: 0, name: Team Number, + validators: null, value: 7146} + Export Subsystems: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Export Subsystems, validators: null, value: true} + Simulation World File: !!robotbuilder.data.properties.StringProperty {default: /usr/share/frcsim/worlds/GearsBotDemo.world, + name: Simulation World File, validators: null, value: /usr/share/frcsim/worlds/GearsBotDemo.world} + Wiring file location: !!robotbuilder.data.properties.FileProperty {default: /home/d0048/, + extension: null, folder: true, name: Wiring file location, validators: null, value: ./} + Autonomous Command: !!robotbuilder.data.properties.TypeSelectionProperty {default: None, + name: Autonomous Command, type: Command, validators: null, value: Autonomous Command} +Base: Robot +Name: FRC7146_Java diff --git a/proj/FRC7146_Java/build.properties b/proj/FRC7146_Java/build.properties index 4624743..0cfa308 100644 --- a/proj/FRC7146_Java/build.properties +++ b/proj/FRC7146_Java/build.properties @@ -1,4 +1,5 @@ # Project specific information package=org.usfirst.frc.team7146.robot robot.class=${package}.Robot -simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world \ No newline at end of file +simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world +# lib.path=/home/d0048/Program-code/netbean-java/FRC2018/FRC/proj/FRC7146_Java/deps \ No newline at end of file diff --git a/proj/FRC7146_Java/build.xml b/proj/FRC7146_Java/build.xml index 76fd29a..4e3366a 100644 --- a/proj/FRC7146_Java/build.xml +++ b/proj/FRC7146_Java/build.xml @@ -2,7 +2,7 @@ - - - + + - - + + - - + + - + - - - - - + + + + + diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/OI.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/OI.class index d678792..5d95fd8 100644 Binary files a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/OI.class and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/OI.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/Robot.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/Robot.class index 5c014b9..84ce46d 100644 Binary files a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/Robot.class and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/Robot.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$JOYSTICK.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$JOYSTICK.class new file mode 100644 index 0000000..ca34dc0 Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$JOYSTICK.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$MOTOR.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$MOTOR.class new file mode 100644 index 0000000..c9c6b9b Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$MOTOR.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$STATUS.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$STATUS.class new file mode 100644 index 0000000..718cf37 Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap$STATUS.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap.class index cd582c2..533ab74 100644 Binary files a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap.class and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/RobotMap.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commandGroups/AutonomousCommandGroup.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commandGroups/AutonomousCommandGroup.class new file mode 100644 index 0000000..f056562 Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commandGroups/AutonomousCommandGroup.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/EmergencyRecoverCommand.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/EmergencyRecoverCommand.class new file mode 100644 index 0000000..0fb6dfb Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/EmergencyRecoverCommand.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/EmergencyStopCommand.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/EmergencyStopCommand.class new file mode 100644 index 0000000..c999bb9 Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/EmergencyStopCommand.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/TeleopArcadeDriveCommand.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/TeleopArcadeDriveCommand.class new file mode 100644 index 0000000..7024d9f Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/TeleopArcadeDriveCommand.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.class index affdef9..00d3657 100644 Binary files a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.class and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/ChasisDriveSubsystem.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/ChasisDriveSubsystem.class new file mode 100644 index 0000000..c1366ec Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/ChasisDriveSubsystem.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/GyroSubsystem.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/GyroSubsystem.class new file mode 100644 index 0000000..fd6bb30 Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/GyroSubsystem.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/LiftSubsystem.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/LiftSubsystem.class new file mode 100644 index 0000000..b5daf13 Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/LiftSubsystem.class differ diff --git a/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/VisionSubsystem.class b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/VisionSubsystem.class new file mode 100644 index 0000000..e0916ff Binary files /dev/null and b/proj/FRC7146_Java/build/org/usfirst/frc/team7146/robot/subsystems/VisionSubsystem.class differ diff --git a/proj/FRC7146_Java/deps/README.md b/proj/FRC7146_Java/deps/README.md new file mode 100644 index 0000000..289a275 --- /dev/null +++ b/proj/FRC7146_Java/deps/README.md @@ -0,0 +1 @@ +### Copy all the files here to `~/wpilib/user/java/lib/` for the to be recognized!! diff --git a/proj/FRC7146_Java/dist/FRCUserProgram.jar b/proj/FRC7146_Java/dist/FRCUserProgram.jar index 1c088da..324177e 100644 Binary files a/proj/FRC7146_Java/dist/FRCUserProgram.jar and b/proj/FRC7146_Java/dist/FRCUserProgram.jar differ diff --git a/proj/FRC7146_Java/src/io/github/d0048/NumPID.java b/proj/FRC7146_Java/src/io/github/d0048/NumPID.java new file mode 100644 index 0000000..60dbfc7 --- /dev/null +++ b/proj/FRC7146_Java/src/io/github/d0048/NumPID.java @@ -0,0 +1,13 @@ +package io.github.d0048; + +public class NumPID { + public double P = 0, I = 0, D = 0; + + public NumPID(double p, double i, double d) { + super(); + P = p; + I = i; + D = d; + } + +} diff --git a/proj/FRC7146_Java/src/io/github/d0048/Utils.java b/proj/FRC7146_Java/src/io/github/d0048/Utils.java new file mode 100644 index 0000000..d43dfbd --- /dev/null +++ b/proj/FRC7146_Java/src/io/github/d0048/Utils.java @@ -0,0 +1,58 @@ +package io.github.d0048; + +import java.util.List; +import java.util.logging.Logger; +import org.opencv.core.Mat; +import org.opencv.core.Point; + +public class Utils { + private static final Logger logger = Logger.getLogger(Utils.class.getName()); + + public static double AngleOffsetCal(double src, double dst) { + return dst - src; + } + + public static double Ang2tanh(double ofs, double tol) { + double angle = 0; + if (Math.abs(ofs) > tol) {// TODO: Change to tanh + angle = ofs / 15; + } + return angle; + + } + + public static Point mid(Point p1, Point p2) { + return new Point((p1.x + p2.x) / 2, (p1.y + p2.y) / 2); + } + + public static void release(Mat o) { + if (o != null) { + try { + ((Mat) o).release(); + } finally { + + } + } + } + + public static void release(Object[] os) { + for (Object o : os) { + try { + release((Mat) o); + } finally { + + } + } + } + + public static void release(List os) { + for (Object o : os) { + try { + ((Mat) o).release(); + } finally { + + } + } + } + +} diff --git a/proj/FRC7146_Java/src/io/github/d0048/vision/VisualTarget.java b/proj/FRC7146_Java/src/io/github/d0048/vision/VisualTarget.java new file mode 100644 index 0000000..5db36e7 --- /dev/null +++ b/proj/FRC7146_Java/src/io/github/d0048/vision/VisualTarget.java @@ -0,0 +1,18 @@ +package io.github.d0048.vision; + +import org.opencv.core.Point; +import org.opencv.core.Size; + + +public class VisualTarget { + // in view,deviation from center + + public Point center = new Point(0, 0); + public Size size = new Size(); + + public VisualTarget(Point center, Size size) { + this.center = center; + this.size = size; + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/MatchInfo.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/MatchInfo.java new file mode 100644 index 0000000..c43a937 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/MatchInfo.java @@ -0,0 +1,45 @@ +package org.usfirst.frc.team7146.robot; + +import java.util.logging.Logger; + +import edu.wpi.first.wpilibj.DriverStation; + +public class MatchInfo { + private static final Logger logger = Logger.getLogger(MatchInfo.class.getName()); + public static boolean matchInfoDebug = false; + + public static final String NAH = ""; + public static char scale1 = 'L'; + public static char balance = 'L'; + public static char scale2 = 'L'; + public static boolean success = false; + + public MatchInfo() { + } + + public static boolean infoInit() { + String msg = DriverStation.getInstance().getGameSpecificMessage(); + if (matchInfoDebug) { + logger.info("Recieve: " + msg); + } + try { + if (!msg.equals(NAH)) { + MatchInfo.scale1 = msg.charAt(0); + MatchInfo.balance = msg.charAt(1); + MatchInfo.scale2 = msg.charAt(2); + logger.info("match info reterived: " + scale1 + "|" + balance + "|" + scale2); + return success = true; + } else { + if (matchInfoDebug) { + logger.info("useless message:" + msg); + } + return false; + } + } catch (Exception e) { + logger.throwing("InfoInit", "None Strandard message recieved", e); + logger.warning("error resolving msg: " + e.getMessage()); + return false; + } + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/OI.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/OI.java index 24c9592..9ee927f 100644 --- a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/OI.java +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/OI.java @@ -7,9 +7,35 @@ package org.usfirst.frc.team7146.robot; +import java.util.HashMap; +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.commands.AngleTurnCommand; +import org.usfirst.frc.team7146.robot.commands.AutoGrabCubeCommand; +import org.usfirst.frc.team7146.robot.commands.CmdBase; +import org.usfirst.frc.team7146.robot.commands.EmergencyRecoverCommand; +import org.usfirst.frc.team7146.robot.commands.EmergencyStopCommand; +import org.usfirst.frc.team7146.robot.commands.Lift2MidCommand; +import org.usfirst.frc.team7146.robot.commands.StraightDriveCommand; +import org.usfirst.frc.team7146.robot.subsystems.ChasisDriveSubsystem; +import org.usfirst.frc.team7146.robot.subsystems.LiftSubsystem.POSITION; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.BuiltInAccelerometer; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.interfaces.Accelerometer; /** * This class is the glue that binds the controls on the physical operator @@ -17,16 +43,53 @@ */ public class OI { + private static final Logger logger = Logger.getLogger(OI.class.getName()); + + public SpeedController mLeftMotor = new Spark(RobotMap.MOTOR.LEFT_MOTOR_GROUP); + public SpeedController mRightMotor = new Spark(RobotMap.MOTOR.RIGHT_MOTOR_GROUP); + public DifferentialDrive mDifferentialDrive = new DifferentialDrive(mLeftMotor, mRightMotor); + + public SpeedController collectorWheelMotors = new Spark(RobotMap.MOTOR.COLLECTOR_WHEEL_PORT); + public SpeedController liftMotor = new Spark(RobotMap.MOTOR.LIFT_PORT); + public Joystick mJoystick1 = new Joystick(0); - public Button mBtn1 = new JoystickButton(mJoystick1, 1), mBtn2 = new JoystickButton(mJoystick1, 2), - mBtn3 = new JoystickButton(mJoystick1, 3), mBtn4 = new JoystickButton(mJoystick1, 4), - mBtn5 = new JoystickButton(mJoystick1, 5), mBtn6 = new JoystickButton(mJoystick1, 6), - mBtn7 = new JoystickButton(mJoystick1, 7), mBtn8 = new JoystickButton(mJoystick1, 8); + public Button mXboxBtnA = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_A), + mXboxBtnB = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_B), + mXboxBtnX = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_X), + mXboxBtnY = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_Y), + mXboxBtnLb = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_LB), + mXboxBtnRb = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_RB), + mXboxBtnLt = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_LT), + mXboxBtnRt = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_RT), + mXboxBtnLftStk = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_LEFT_STICK_BTN), + mXboxBtnRghtStk = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_RIGHT_STICK_BTN), + mXboxBtnBack = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_BACK), + mXboxBtnStart = new JoystickButton(mJoystick1, RobotMap.JOYSTICK.NUM_XBOX_START); + + public ADXRS450_Gyro mGyro = new ADXRS450_Gyro(SPI.Port.kOnboardCS0); + // public Accelerometer mAccelerometer = new ADXL345_I2C(I2C.Port.kOnboard, + // Accelerometer.Range.k4G, 0x3A); + public Accelerometer mAccelerometer = new BuiltInAccelerometer(Accelerometer.Range.k4G);// TODO: TEST, ACC + public TalonSRX mTalon1 = new TalonSRX(0); + public DigitalInput mLimitSwitchUp = new DigitalInput(RobotMap.SENSOR.NUM_LSW_UP); + public DigitalInput mLimitSwitchMID = new DigitalInput(RobotMap.SENSOR.NUM_LSW_MID); + public DigitalInput mLimitSwitchDw = new DigitalInput(RobotMap.SENSOR.NUM_LSW_DW); + + public HashMap mCommands = new HashMap(); public OI() { - /* Possible btn binding */ - // mBtn.whenPressed(new ExampleCommand()); - // mBtn.whileHeld(new ExampleCommand()); - // mBtn.whenReleased(new ExampleCommand()); + // mTalon1.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, + // 0, 10); + // mTalon1.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10); + mTalon1.set(ControlMode.PercentOutput, 0.0); // Disable output coz not needed + } + + public void mapOI() { + /* Btn binding */ + logger.info("OI map"); + EmergencyStopCommand mEmergencyStopCommand = new EmergencyStopCommand(); + mXboxBtnB.whileHeld(mEmergencyStopCommand); + mXboxBtnStart.whileHeld(new AutoGrabCubeCommand(0.6)); + } } \ No newline at end of file diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/Robot.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/Robot.java index 8616fa2..5b42fc1 100644 --- a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/Robot.java +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/Robot.java @@ -12,8 +12,22 @@ import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import testCommands.AccelTestCMD; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.RobotMap.STATUS; +import org.usfirst.frc.team7146.robot.commands.AutonomousCommandGroup; +import org.usfirst.frc.team7146.robot.commands.ChasisStateUpdateCommand; +import org.usfirst.frc.team7146.robot.commands.CmdBase; +import org.usfirst.frc.team7146.robot.commands.TeleopArcadeDriveCommand; import org.usfirst.frc.team7146.robot.commands.TeleopTankDriveCommand; import org.usfirst.frc.team7146.robot.subsystems.ChasisDriveSubsystem; +import org.usfirst.frc.team7146.robot.subsystems.ChasisDriveSubsystem.CHASIS_MODE; +import org.usfirst.frc.team7146.robot.subsystems.GyroSubsystem; +import org.usfirst.frc.team7146.robot.subsystems.LiftSubsystem; +import org.usfirst.frc.team7146.robot.subsystems.LiftSubsystem.POSITION; +import org.usfirst.frc.team7146.robot.subsystems.VisionSubsystem; /** * The VM is configured to automatically run this class, and to call the @@ -23,8 +37,13 @@ * project. */ public class Robot extends TimedRobot { - public static ChasisDriveSubsystem m_ChasisTrainSubsystem; + private static final java.util.logging.Logger logger = Logger.getLogger(Robot.class.getName()); + public static ChasisDriveSubsystem m_ChasisDriveSubsystem; + public static GyroSubsystem m_GyroSubsystem; + public static VisionSubsystem m_VisionSubsystem; + public static LiftSubsystem m_LiftSubsystem; public static OI m_oi; + public static boolean EMERGENCY_HALT = false; Command m_autonomousCommand; SendableChooser m_chooser = new SendableChooser<>(); @@ -36,11 +55,15 @@ public class Robot extends TimedRobot { @Override public void robotInit() { m_oi = new OI(); - m_ChasisTrainSubsystem = new ChasisDriveSubsystem(RobotMap.MOTOR_PID[0], RobotMap.MOTOR_PID[1], - RobotMap.MOTOR_PID[2]); - m_chooser.addDefault("Default Auto", new TeleopTankDriveCommand()); - // chooser.addObject("My Auto", new MyAutoCommand()); + m_GyroSubsystem = new GyroSubsystem(); + m_ChasisDriveSubsystem = new ChasisDriveSubsystem(ChasisDriveSubsystem.CHASIS_MODE.ARCADE, + RobotMap.MOTOR.ARCADE_SPD_NUM_PID, RobotMap.MOTOR.ARCADE_ANG_NUM_PID); + m_VisionSubsystem = new VisionSubsystem(); + m_LiftSubsystem = new LiftSubsystem(); SmartDashboard.putData("Auto mode", m_chooser); + m_oi.mapOI();// execute at the end to make sure all other subsystems initialized + // chooser.addObject("My Auto", new MyAutoCommand()); + } /** @@ -50,14 +73,21 @@ public void robotInit() { */ @Override public void disabledInit() { - + RobotMap.mStatus = STATUS.STAT_HALT; } @Override public void disabledPeriodic() { + if (!MatchInfo.success) { + MatchInfo.infoInit(); + } Scheduler.getInstance().run(); } + public void mPeriodic() {// execute everywhere except disable + m_LiftSubsystem.writeStatus(); + } + /** * This autonomous (along with the chooser code above) shows how to select * between different autonomous modes using the dashboard. The sendable chooser @@ -72,52 +102,108 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - m_autonomousCommand = m_chooser.getSelected(); + ChasisDriveSubsystem.LOCK_OVERRIDE = false; + //m_autonomousCommand = m_chooser.getSelected(); + m_autonomousCommand = new AutonomousCommandGroup(99); + + RobotMap.mStatus = STATUS.STAT_AUTO; + if (m_autonomousCommand != null) { + m_autonomousCommand.start(); + } /* * String autoSelected = SmartDashboard.getString("Auto Selector", "Default"); * switch(autoSelected) { case "My Auto": autonomousCommand = new * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new * ExampleCommand(); break; } */ - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.start(); - } } - /** - * This function is called periodically during autonomous. - */ @Override public void autonomousPeriodic() { - Scheduler.getInstance().run(); + if (!RobotMap.mStatus.equals(STATUS.STAT_ERR)) { + this.mPeriodic(); + Scheduler.getInstance().run(); + } else { + logger.warning("Err in Periodic"); + if (!RobotMap.DEBUG) {// Stop running for debug + logger.warning("Ignoring"); + Scheduler.getInstance().run(); + } + } } @Override public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. + ChasisDriveSubsystem.LOCK_OVERRIDE = true; + RobotMap.mStatus = STATUS.STAT_TELEOP; if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_GyroSubsystem.mGyro.reset(); + if (m_ChasisDriveSubsystem.mode == CHASIS_MODE.ARCADE) { + new TeleopArcadeDriveCommand().start(); + } else if (m_ChasisDriveSubsystem.mode == CHASIS_MODE.ARCADE) { + new TeleopTankDriveCommand().start(); + } } - /** - * This function is called periodically during operator control. - */ @Override public void teleopPeriodic() { - Scheduler.getInstance().run(); + if (!RobotMap.mStatus.equals(STATUS.STAT_ERR)) { + this.mPeriodic(); + Scheduler.getInstance().run(); + } else { + logger.warning("Err in Teleop"); + if (!RobotMap.DEBUG) {// Stop running for debug + logger.warning("Ignoring"); + Scheduler.getInstance().run(); + } + } } - /** - * This function is called periodically during test mode. - */ + @Override + public void testInit() { + ChasisDriveSubsystem.LOCK_OVERRIDE = true; + accelTest = new AccelTestCMD(); + } + + AccelTestCMD accelTest; + @Override public void testPeriodic() { + if (!RobotMap.mStatus.equals(STATUS.STAT_ERR)) { + this.mPeriodic(); + + if (true) {// Accel Test + accelTest.disp(); + } + if (true) {// CV Test + try { + m_VisionSubsystem.findCube(); + } catch (Exception e) { + e.printStackTrace(); + } + } + + // m_oi.liftMotor.set(20); + // Scheduler.getInstance().run(); + } else { + logger.warning("Err in Test"); + if (!RobotMap.DEBUG) {// Stop running for debug + logger.warning("Ignoring"); + Scheduler.getInstance().run(); + } + } + } + + public static boolean cmdCanRun(CmdBase cmd) { + for (String k : m_oi.mCommands.keySet()) { + if (m_oi.mCommands.get(k).priority < cmd.priority && m_oi.mCommands.get(k).isRunning()) { + return false; + } + } + return true; } } diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/RobotMap.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/RobotMap.java index d015b9f..a4be98d 100644 --- a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/RobotMap.java +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/RobotMap.java @@ -7,6 +7,10 @@ package org.usfirst.frc.team7146.robot; +import java.util.logging.Logger; + +import io.github.d0048.NumPID; + /** * The RobotMap is a mapping from the ports sensors and actuators are wired into * to a variable name. This provides flexibility changing wiring, makes checking @@ -15,22 +19,61 @@ */ public class RobotMap { - /* - * DifferentialDrive Motor Group Port Number, now using two set of spark - * - * TODO: Change to spark mixed spx - */ - public static final int LEFT_MOTOR_GROUP = 0; - public static final int RIGHT_MOTOR_GROUP = 1; - public static final double[] MOTOR_PID = { 2., 0., 0. }; - - /* - * Teleop TankDrive Speed Factor - * - * Do not raise more than 1 - */ - public static double TELE_LEFT_SPEED_FACTOR = 0.7; - public static double TELE_RIGHT_SPEED_FACTOR = 0.7; + // private static final java.util.logging.Logger logger = + // Logger.getLogger(RobotMap.class.getName()); + public static boolean DEBUG = true; + + public static class MOTOR { + /* + * DifferentialDrive Motor Group Port Number, now using two set of spark + * + * TODO: Change to spark mixed spx + */ + public static final int LEFT_MOTOR_GROUP = 0; + public static final int RIGHT_MOTOR_GROUP = 1; + public static final NumPID TANK_NUM_PID = new NumPID(2, 0, 0), ARCADE_SPD_NUM_PID = new NumPID(0.1, 0, 0), + ARCADE_ANG_NUM_PID = new NumPID(0.1, 0., 0.), LIFT_NUM_PID = new NumPID(2, 0, 0); + + public final static int COLLECTOR_WHEEL_PORT = 3; + public final static int LIFT_PORT = 2; + /* + * Motor Speed Factor + * + * Do not raise more than 1 + */ + public static double TELE_LEFT_SPEED_FACTOR = 0.7; + public static double TELE_RIGHT_SPEED_FACTOR = 0.7; + public static double TELE_SPD_FACTOR = 0.6; + public static double TELE_ANG_FACTOR = 0.6; + } + + public static class JOYSTICK { + // TODO: Reference only + public static final int NUM_XBOX_X = 1; + public static final int NUM_XBOX_A = 2; + public static final int NUM_XBOX_B = 3; + public static final int NUM_XBOX_Y = 4; + public static final int NUM_XBOX_LB = 5; + public static final int NUM_XBOX_RB = 6; + public static final int NUM_XBOX_LT = 7; + public static final int NUM_XBOX_RT = 8; + public static final int NUM_XBOX_LEFT_STICK_BTN = 11; + public static final int NUM_XBOX_RIGHT_STICK_BTN = 12; + public static final int NUM_XBOX_BACK = 9; + public static final int NUM_XBOX_START = 10; + } + + public static class SENSOR { + public static final int NUM_LSW_UP = 1; + public static final int NUM_LSW_MID = 2; + public static final int NUM_LSW_DW = 0; + } + + public enum STATUS { + STAT_AUTO, STAT_TELEOP, STAT_HALT, STAT_ERR + }; + + public static STATUS mStatus; // If you are using multiple modules, make sure to define both the port // number and the module. For example you with a rangefinder: diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AbsoluteAngleTurnCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AbsoluteAngleTurnCommand.java new file mode 100644 index 0000000..5458cd7 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AbsoluteAngleTurnCommand.java @@ -0,0 +1,68 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.subsystems.ChasisDriveSubsystem; + + +public class AbsoluteAngleTurnCommand extends CmdBase { + + private static final java.util.logging.Logger logger = Logger.getLogger(AbsoluteAngleTurnCommand.class.getName()); + public static CmdBase instance; + public static boolean AbsAngleTurnCommandDebug = true; + + private double absAng = 0; + private double absDisp = 0; + private ChasisDriveSubsystem mChassis = Robot.m_ChasisDriveSubsystem; + + public AbsoluteAngleTurnCommand(double absDisp, double absAng) { + super("AbsoluteAngleTurnCommand", 99); + requires(Robot.m_ChasisDriveSubsystem); + requires(Robot.m_GyroSubsystem); + + logger.info("Instance created"); + AbsoluteAngleTurnCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + + this.absDisp = absDisp; + this.absAng = absAng; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (!Robot.cmdCanRun(this)) + return; + mChassis.mArcadeRequestAbsolute(absDisp, absAng); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return Robot.m_ChasisDriveSubsystem.isOnPosition(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + super.end(); + Robot.m_ChasisDriveSubsystem.resetAngleState(); + logger.info("Instance destroyed"); + AbsoluteAngleTurnCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AngleTurnCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AngleTurnCommand.java new file mode 100644 index 0000000..cece3c0 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AngleTurnCommand.java @@ -0,0 +1,64 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; + +public class AngleTurnCommand extends CmdBase { + + private static final java.util.logging.Logger logger = Logger.getLogger(AngleTurnCommand.class.getName()); + public static CmdBase instance; + public static boolean AngleTurnCommandDebug = true; + + private double ang = 0; + private double disp = 0; + + public AngleTurnCommand(double disp, double ang) { + super("AngleTurnCommand", 99); + requires(Robot.m_ChasisDriveSubsystem); + requires(Robot.m_GyroSubsystem); + + logger.info("Instance created"); + AngleTurnCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + + this.disp = disp; + this.ang = ang; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.m_ChasisDriveSubsystem.mArcadeRequest(disp, ang); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (!Robot.cmdCanRun(this)) + return; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return Robot.m_ChasisDriveSubsystem.isOnPosition(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + super.end(); + Robot.m_ChasisDriveSubsystem.resetAngleState(); + logger.info("Instance destroyed"); + AngleTurnCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AutoGrabCubeCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AutoGrabCubeCommand.java new file mode 100644 index 0000000..0432191 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AutoGrabCubeCommand.java @@ -0,0 +1,98 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.opencv.core.Point; +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.subsystems.ChasisDriveSubsystem; +import org.usfirst.frc.team7146.robot.subsystems.VisionSubsystem; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import io.github.d0048.vision.VisualTarget; + +public class AutoGrabCubeCommand extends CmdBase { + private static final java.util.logging.Logger logger = Logger.getLogger(AutoGrabCubeCommand.class.getName()); + public static CmdBase instance; + public static boolean AutoGrabCubeDebug = true; + + public VisionSubsystem mVision = Robot.m_VisionSubsystem; + ChasisDriveSubsystem mChasis = Robot.m_ChasisDriveSubsystem; + + double spd = 0; + + public AutoGrabCubeCommand(double spd) { + super("AutoGrabCubeCommand", 99); + requires(Robot.m_ChasisDriveSubsystem); + requires(Robot.m_GyroSubsystem); + requires(Robot.m_VisionSubsystem); + + logger.info("Instance created"); + AutoGrabCubeCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + this.spd = spd; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + VisualTarget lastTarget = null; + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (!Robot.cmdCanRun(this)) + return; + try { + VisualTarget target = mVision.findCube(); + if (target != null) { + lastTarget = target; + move2Target(target); + } else { + move2Target(lastTarget); + } + } catch (Exception e) { + logger.throwing("Err calling find cube", "auto grab cube cmd", e); + e.printStackTrace(); + } + + } + + public void move2Target(VisualTarget v) { + if (v == null) + return; + Point c = v.center; + if (c.y > 160 / 2) { + mChasis.mArcadeRequest(spd, 0.2); + SmartDashboard.putString("Vision", "Right"); + } else { + mChasis.mArcadeRequest(spd, -0.2); + SmartDashboard.putString("Vision", "Left"); + } + } + + // 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() { + super.end(); + mChasis.mArcadeRequest(0, 0); + logger.info("Instance destroyed"); + AutoGrabCubeCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AutonomousCommandGroup.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AutonomousCommandGroup.java new file mode 100644 index 0000000..68d5592 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/AutonomousCommandGroup.java @@ -0,0 +1,179 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import javax.swing.plaf.basic.BasicBorders.RolloverButtonBorder; + +import org.usfirst.frc.team7146.robot.MatchInfo; +import org.usfirst.frc.team7146.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class AutonomousCommandGroup extends CmdGroupBase { + private static final java.util.logging.Logger logger = Logger.getLogger(AutonomousCommandGroup.class.getName()); + + public static char mStartPoint = 'M'; + public static int DIST_LF_MID = 10, DIST_RT_MID = 10;// TODO: Measure + public static int DIST_BK_SW = 10; + + public AutonomousCommandGroup(int priority) {// TODO: Timeout + this("AutonomouseCrommandGroup", priority); + + } + + public AutonomousCommandGroup(String name, int priority) { + super(name, priority); + addParallel(liftUp, 2); + leave(); + if (MatchInfo.scale1 == 'L') { + addSequential(new AbsoluteAngleTurnCommand(0, 90)); + } else { + addSequential(new AbsoluteAngleTurnCommand(0, -90)); + } + addParallel(collectorOut, 0.5); + // TODO: Put cube + addParallel(collectorIn, 0.5); + addSequential(new AutoGrabCubeCommand(0.1), 5); + } + + public void leave() { + if (mStartPoint == 'M') { + switch (MatchInfo.scale1) { + case 'L': + addSequential(new AbsoluteAngleTurnCommand(0, -90)); + addSequential(new AbsoluteAngleTurnCommand(DIST_LF_MID, -90)); + addSequential(new AbsoluteAngleTurnCommand(0, -0)); + break; + case 'R': + addSequential(new AbsoluteAngleTurnCommand(0, 90)); + addSequential(new AbsoluteAngleTurnCommand(DIST_RT_MID, -90)); + addSequential(new AbsoluteAngleTurnCommand(0, -0)); + break; + + default: + logger.warning("This code should not be reached"); + break; + } + } + + else if (MatchInfo.scale1 == mStartPoint) {// Nothing to do + } else { + switch (MatchInfo.scale1) { + case 'L': + addSequential(new AbsoluteAngleTurnCommand(0, -90)); + addSequential(new AbsoluteAngleTurnCommand(DIST_LF_MID, -90)); + addSequential(new AbsoluteAngleTurnCommand(DIST_RT_MID, -90)); + addSequential(new AbsoluteAngleTurnCommand(0, -0)); + break; + case 'R': + addSequential(new AbsoluteAngleTurnCommand(0, 90)); + addSequential(new AbsoluteAngleTurnCommand(DIST_LF_MID, 90)); + addSequential(new AbsoluteAngleTurnCommand(DIST_RT_MID, 90)); + addSequential(new AbsoluteAngleTurnCommand(0, -0)); + break; + + default: + logger.warning("This code should not be reached"); + break; + } + } + + new AbsoluteAngleTurnCommand(DIST_BK_SW, 0); + addSequential(new AbsoluteAngleTurnCommand(0, 90)); + + } + + Command liftUp = new Command() { + @Override + protected void initialize() { + if (!Robot.m_LiftSubsystem.isUp()) { + Robot.m_LiftSubsystem.up(); + } else { + Robot.m_LiftSubsystem.stop(); + } + } + + @Override + protected void end() { + Robot.m_LiftSubsystem.stop(); + } + + @Override + protected void interrupted() { + end(); + } + + @Override + protected boolean isFinished() { + return false; + } + }; + Command liftDown = new Command() { + @Override + protected void initialize() { + // if (!Robot.m_LiftSubsystem.isUp()) { + Robot.m_LiftSubsystem.down(); + // } else { + // Robot.m_LiftSubsystem.stop(); + // } + } + + @Override + protected void end() { + Robot.m_LiftSubsystem.stop(); + } + + @Override + protected void interrupted() { + end(); + } + + @Override + protected boolean isFinished() { + return Robot.m_LiftSubsystem.isDown(); + } + }; + Command collectorIn = new Command() { + @Override + protected void initialize() { + Robot.m_oi.collectorWheelMotors.set(0.9); + } + + @Override + protected void end() { + Robot.m_oi.collectorWheelMotors.stopMotor(); + } + + @Override + protected void interrupted() { + end(); + } + + @Override + protected boolean isFinished() { + return false; + } + }; + Command collectorOut = new Command() { + + @Override + protected void initialize() { + Robot.m_oi.collectorWheelMotors.set(-0.9); + } + + @Override + protected void end() { + Robot.m_oi.collectorWheelMotors.stopMotor(); + } + + @Override + protected void interrupted() { + end(); + } + + @Override + protected boolean isFinished() { + return false; + } + }; +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/ChasisStateUpdateCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/ChasisStateUpdateCommand.java new file mode 100644 index 0000000..99d0ec8 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/ChasisStateUpdateCommand.java @@ -0,0 +1,78 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.subsystems.ChasisDriveSubsystem; + +import edu.wpi.first.wpilibj.interfaces.Gyro; + +public class ChasisStateUpdateCommand extends CmdBase { + /** + * Abandoned + */ + + private static final java.util.logging.Logger logger = Logger.getLogger(ChasisStateUpdateCommand.class.getName()); + public static CmdBase instance; + public static boolean ChasisUpdateDebug = false; + + public ChasisDriveSubsystem mChasis = Robot.m_ChasisDriveSubsystem; + public Gyro mGyro = Robot.m_GyroSubsystem.mGyro; + + public ChasisStateUpdateCommand() { + super("ChasisStateUpdateCommand", 1000); + requires(Robot.m_ChasisDriveSubsystem); + requires(Robot.m_GyroSubsystem); + + logger.info("Instance created"); + ChasisStateUpdateCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + @Override + protected void execute() { + /* + * if (!Robot.cmdCanRun(this)) return; + */ + this.update_actual(); + + } + + // public static boolean OVERRIDE = false; + + public void updateAndDispatch() { + /* + * if (!OVERRIDE) { // update_actual(); // mChasis.mArcadeDispatch(); if + * (ChasisUpdateDebug) { logger.info("Chasis state update"); } } else { // + * mChasis.resetAngleState(); // logger.info("Update Override"); } + * SmartDashboard.putBoolean("Dispatch Update:", !OVERRIDE); + */ + } + + protected void update_actual() { + // nothing to do really.. + } + + // 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() { + super.end(); + Robot.m_ChasisDriveSubsystem.stopDrive(); + StraightDriveCommand.instance = null; + logger.info("Instance destroyed"); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/CmdBase.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/CmdBase.java new file mode 100644 index 0000000..9814deb --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/CmdBase.java @@ -0,0 +1,41 @@ +package org.usfirst.frc.team7146.robot.commands; + +import org.usfirst.frc.team7146.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class CmdBase extends Command { + public int priority = 100; + + public CmdBase(int priority) { + this.priority = priority; + } + + public CmdBase(String name, int priority) { + super(name); + this.priority = priority; + } + + public CmdBase(double timeout, int priority) { + super(timeout); + this.priority = priority; + } + + public CmdBase(String name, double timeout, int priority) { + super(name, timeout); + this.priority = priority; + } + + protected void end() { + Robot.m_oi.mCommands.remove(this.getName()); + } + + @Override + protected boolean isFinished() { + return false; + } + @Override + protected void execute() { + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/CmdGroupBase.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/CmdGroupBase.java new file mode 100644 index 0000000..2c0a1e6 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/CmdGroupBase.java @@ -0,0 +1,32 @@ +package org.usfirst.frc.team7146.robot.commands; + +import org.usfirst.frc.team7146.robot.Robot; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class CmdGroupBase extends CommandGroup { + public int priority = 100; + + public CmdGroupBase(int priority) { + this.priority = priority; + } + + public CmdGroupBase(String name, int priority) { + super(name); + this.priority = priority; + } + + protected void end() { + Robot.m_oi.mCommands.remove(this.getName()); + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void execute() { + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/EmergencyRecoverCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/EmergencyRecoverCommand.java new file mode 100644 index 0000000..17df9f7 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/EmergencyRecoverCommand.java @@ -0,0 +1,61 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.RobotMap; + + +public class EmergencyRecoverCommand extends CmdBase { + + private static final java.util.logging.Logger logger = Logger.getLogger(EmergencyRecoverCommand.class.getName()); + public static CmdBase instance; + + public EmergencyRecoverCommand() { + super("EmergencyRecoverCommand", -1000); + requires(Robot.m_ChasisDriveSubsystem); + + logger.info("Instance created"); + EmergencyRecoverCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + @Override + protected void execute() { + if (EmergencyStopCommand.instance != null) { + logger.info("Recover"); + RobotMap.MOTOR.TELE_LEFT_SPEED_FACTOR = ((EmergencyStopCommand) EmergencyStopCommand.instance).LEFT_FACTOR_BAK; + RobotMap.MOTOR.TELE_RIGHT_SPEED_FACTOR = ((EmergencyStopCommand) EmergencyStopCommand.instance).RIGHT_FACTOR_BAK; + EmergencyRecoverCommand.instance.cancel(); + Robot.EMERGENCY_HALT = false; + this.cancel(); + } else { + logger.info("Failed to Recover: Can not find stop instance"); + this.cancel(); + } + } + + // 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() { + super.end(); + Robot.m_ChasisDriveSubsystem.stopDrive(); + logger.info("Instance destroyed"); + // EmergencyStopCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/EmergencyStopCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/EmergencyStopCommand.java new file mode 100644 index 0000000..bea76ef --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/EmergencyStopCommand.java @@ -0,0 +1,70 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.RobotMap; + +public class EmergencyStopCommand extends CmdBase { + + private static final java.util.logging.Logger logger = Logger.getLogger(EmergencyStopCommand.class.getName()); + public static CmdBase instance; + + public EmergencyStopCommand() { + super("EmergencyStopCommand", -1000); + requires(Robot.m_ChasisDriveSubsystem); + + logger.info("Instance created"); + EmergencyStopCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + public double LEFT_FACTOR_BAK = RobotMap.MOTOR.TELE_LEFT_SPEED_FACTOR, + RIGHT_FACTOR_BAK = RobotMap.MOTOR.TELE_RIGHT_SPEED_FACTOR, SPD_FACTOR_BAK = RobotMap.MOTOR.TELE_SPD_FACTOR, + ANG_FACTOR_BAK = RobotMap.MOTOR.TELE_ANG_FACTOR; + + @Override + protected void execute() { + RobotMap.MOTOR.TELE_LEFT_SPEED_FACTOR = 0; + RobotMap.MOTOR.TELE_RIGHT_SPEED_FACTOR = 0; + RobotMap.MOTOR.TELE_SPD_FACTOR = 0; + RobotMap.MOTOR.TELE_ANG_FACTOR = 0; + Robot.EMERGENCY_HALT = true; + Robot.m_ChasisDriveSubsystem.stopDrive(); + System.exit(-1); + logger.info("Emergency stop!"); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + super.end(); + RobotMap.MOTOR.TELE_LEFT_SPEED_FACTOR = this.LEFT_FACTOR_BAK; + RobotMap.MOTOR.TELE_LEFT_SPEED_FACTOR = this.RIGHT_FACTOR_BAK; + RobotMap.MOTOR.TELE_SPD_FACTOR = this.SPD_FACTOR_BAK; + RobotMap.MOTOR.TELE_ANG_FACTOR = this.ANG_FACTOR_BAK; + Robot.EMERGENCY_HALT = false; + logger.info("Instance destroyed"); + EmergencyStopCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + // end(); + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/Lift2MidCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/Lift2MidCommand.java new file mode 100644 index 0000000..a4110d3 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/Lift2MidCommand.java @@ -0,0 +1,99 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.subsystems.LiftSubsystem; +import org.usfirst.frc.team7146.robot.subsystems.LiftSubsystem.POSITION; + +public class Lift2MidCommand extends CmdBase { + private static final java.util.logging.Logger logger = Logger.getLogger(Lift2MidCommand.class.getName()); + public static CmdBase instance; + public static boolean AngleTurnCommandDebug = true; + + public POSITION dst = POSITION.DOWN; + LiftSubsystem mLift = Robot.m_LiftSubsystem; + + public Lift2MidCommand() { + this(POSITION.MID); + } + + public Lift2MidCommand(POSITION p) { + super("AngleTurnCommand", 99); + //requires(Robot.m_LiftSubsystem); + + logger.info("Instance created"); + Lift2MidCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + + dst = p; + } + + @Override + protected void initialize() { + } + + boolean uped = false, downed = false; + + @Override + protected void execute() { + if (!Robot.cmdCanRun(this)) + return; + switch (dst) { + case UP: + if (!mLift.isUp()) + mLift.up(); + break; + case MID: + if (mLift.isMiddle()) { + mLift.down(); + uped = true; + } else { + mLift.up(); + downed = true; + } + break; + case DOWN: + if (!mLift.isDown()) + mLift.down(); + break; + + default: + logger.warning("This code should not be reached"); + break; + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + switch (dst) { + case UP: + return mLift.isUp(); + case MID: + return uped && downed; + case DOWN: + return mLift.isDown(); + default: + logger.warning("This code should not be reached"); + break; + } + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + super.end(); + mLift.stop(); + logger.info("Instance destroyed"); + AngleTurnCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/StraightDriveCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/StraightDriveCommand.java new file mode 100644 index 0000000..d19fb09 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/StraightDriveCommand.java @@ -0,0 +1,70 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; + +public class StraightDriveCommand extends CmdBase { + private static final java.util.logging.Logger logger = Logger.getLogger(StraightDriveCommand.class.getName()); + public static CmdBase instance; + public static boolean StraightDriveDebug = true; + + private double spd = 0; + + public StraightDriveCommand(double spd) { + super("StraightDriveCommand", 99); + requires(Robot.m_ChasisDriveSubsystem); + requires(Robot.m_GyroSubsystem); + + logger.info("Instance created"); + StraightDriveCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + + this.spd = spd; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + //this.ang = Robot.m_GyroSubsystem.getAngle(); + if (StraightDriveDebug) + logger.info("Angle marked"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (!Robot.cmdCanRun(this)) + return; + /* + * double angle = Utils.AngleOffsetCal(Robot.m_GyroSubsystem.mGyro.getAngle(), + * this.ang); angle = Utils.Ang2tanh(angle, this.tol); + * Robot.m_ChasisDriveSubsystem.mDriveArcade(this.spd, angle); if + * (StraightDriveDebug) logger.info("Straight Drive: \nangle: " + angle); + */ + Robot.m_ChasisDriveSubsystem.mArcadeRequest(this.spd, 0); + } + + // 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() { + super.end(); + Robot.m_ChasisDriveSubsystem.mArcadeRequest(0, 0); + logger.info("Instance destroyed"); + StraightDriveCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/TeleopArcadeDriveCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/TeleopArcadeDriveCommand.java new file mode 100644 index 0000000..62387d3 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/TeleopArcadeDriveCommand.java @@ -0,0 +1,155 @@ +package org.usfirst.frc.team7146.robot.commands; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.RobotMap; +import org.usfirst.frc.team7146.robot.subsystems.ChasisDriveSubsystem; +import org.usfirst.frc.team7146.robot.subsystems.LiftSubsystem; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class TeleopArcadeDriveCommand extends CmdBase { + + private static final java.util.logging.Logger logger = Logger.getLogger(TeleopArcadeDriveCommand.class.getName()); + public static CmdBase instance; + public static boolean TeleArcadeDriveDebug = false; + + private Joystick mJoystick = Robot.m_oi.mJoystick1; + private ChasisDriveSubsystem mChasis = Robot.m_ChasisDriveSubsystem; + private LiftSubsystem mLift = Robot.m_LiftSubsystem; + + public TeleopArcadeDriveCommand() { + super("TeleopArcadeDriveCommand", 100); + // DO NOT INTERUPT + requires(Robot.m_ChasisDriveSubsystem); + + logger.info("Instance created"); + TeleopArcadeDriveCommand.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + } + + @Override + protected void initialize() { + } + + boolean isReturning = false; + + @Override + protected void execute() { + if (!Robot.cmdCanRun(this)) { + logger.info("drive override"); + return; + } + double speed = Robot.m_oi.mJoystick1.getRawAxis(3);// Right x + double ang = Robot.m_oi.mJoystick1.getTwist(); + double pov = mJoystick.getPOV(); + SmartDashboard.putNumber("POV", pov); + + // if (ChasisDriveSubsystem.LOCK_OVERRIDE) {// not operating + + // if (!DriverStation.getInstance().isOperatorControl()) {// otherwise will be + // handle by operator + // ChasisDriveSubsystem.LOCK_OVERRIDE = false; + // } + + // } else {// operating + + // if (!DriverStation.getInstance().isOperatorControl()) {// otherwise will be + // handle by operator + // ChasisDriveSubsystem.LOCK_OVERRIDE = true; + // } + + if (ChasisDriveSubsystem.LOCK_OVERRIDE) { + mChasis.mArcadeForceDrive(speed, ang); + isReturning = true; + } + // } + + if (isReturning) { + mChasis.resetAngleState(); + isReturning = false; + } + + if (pov != -1) { + mChasis.mArcadeRequestAbsolute(0, pov); + } + + if (Robot.m_oi.mXboxBtnY.get()) {// up + if (mLift.isUp()) { + mLift.stop(); + } else { + mLift.up(); + } + } else if (Robot.m_oi.mXboxBtnX.get()) {// stop + mLift.stop(); + } else if (Robot.m_oi.mXboxBtnA.get()) {// down + if (mLift.isDown()) { + mLift.stop(); + } else { + mLift.down(); + } + } else { + if (mLift.isDown() || mLift.isMiddle() || mLift.isUp()) { + mLift.stop(); + } + } + + if (Robot.m_oi.mXboxBtnRb.get()) { + Robot.m_oi.collectorWheelMotors.set(-0.9); + } else if (Robot.m_oi.mXboxBtnRt.get()) { + if (Robot.m_oi.collectorWheelMotors.get() != 0) { + Robot.m_oi.collectorWheelMotors.stopMotor(); + } else { + Robot.m_oi.collectorWheelMotors.set(0.9); + } + } + + if (DriverStation.getInstance().isOperatorControl()) { + if (Robot.m_oi.mXboxBtnLt.get()) { + ChasisDriveSubsystem.LOCK_OVERRIDE = false; + } else { + ChasisDriveSubsystem.LOCK_OVERRIDE = true; + mChasis.resetAngleState(); + } + } else { + if (Robot.m_oi.mXboxBtnLt.get()) { + ChasisDriveSubsystem.LOCK_OVERRIDE = true; + mChasis.resetAngleState(); + } else { + ChasisDriveSubsystem.LOCK_OVERRIDE = false; + } + } + + if (Robot.m_oi.mXboxBtnLb.get()) { + mChasis.mArcadeRequest(10, 0); + } + + if (TeleArcadeDriveDebug) { + logger.info("Requested: " + speed + ", " + ang + ", " + pov); + } + } + + @Override + protected boolean isFinished() { + return false; + } + + @Override + protected void end() { + super.end(); + mChasis.stopDrive(); + logger.info("Instance destroyed"); + TeleopArcadeDriveCommand.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.java index 2fa6249..db5d7bd 100644 --- a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.java +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/commands/TeleopTankDriveCommand.java @@ -7,17 +7,23 @@ package org.usfirst.frc.team7146.robot.commands; -import edu.wpi.first.wpilibj.command.Command; +import java.util.logging.Logger; + import org.usfirst.frc.team7146.robot.Robot; /** * An example command. You can replace me with your own command. */ -public class TeleopTankDriveCommand extends Command { +public class TeleopTankDriveCommand extends CmdBase { + private static final java.util.logging.Logger logger = Logger.getLogger(TeleopTankDriveCommand.class.getName()); + public static CmdBase instance; + public TeleopTankDriveCommand() { - // Use requires() here to declare subsystem dependencies - super("TeleopTankDriveCommand"); - requires(Robot.m_ChasisTrainSubsystem); + super("TeleopTankDriveCommand", 100); + requires(Robot.m_ChasisDriveSubsystem); + + logger.info("Instance created"); + TeleopTankDriveCommand.instance = this; } // Called just before this Command runs the first time @@ -28,7 +34,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.m_ChasisTrainSubsystem.mDriveTank(Robot.m_oi.mJoystick1); + Robot.m_ChasisDriveSubsystem.mDriveTank(Robot.m_oi.mJoystick1.getY(), Robot.m_oi.mJoystick1.getThrottle()); } // Make this return true when this Command no longer needs to run execute() @@ -40,7 +46,10 @@ protected boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { - Robot.m_ChasisTrainSubsystem.stopDrive(); + super.end(); + Robot.m_ChasisDriveSubsystem.stopDrive(); + logger.info("Instance destroyed"); + TeleopTankDriveCommand.instance = null; } // Called when another command which requires one or more of the same diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/ChasisDriveSubsystem.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/ChasisDriveSubsystem.java index aef21ad..67eaec0 100644 --- a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/ChasisDriveSubsystem.java +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/ChasisDriveSubsystem.java @@ -7,52 +7,282 @@ package org.usfirst.frc.team7146.robot.subsystems; +import java.util.logging.Logger; + +import javax.xml.parsers.DocumentBuilder; + +import org.omg.PortableServer.THREAD_POLICY_ID; +import org.usfirst.frc.team7146.robot.Robot; import org.usfirst.frc.team7146.robot.RobotMap; -import org.usfirst.frc.team7146.robot.commands.TeleopTankDriveCommand; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.Spark; -import edu.wpi.first.wpilibj.command.PIDSubsystem; +import org.usfirst.frc.team7146.robot.commands.ChasisStateUpdateCommand; +import org.usfirst.frc.team7146.robot.commands.TeleopArcadeDriveCommand; + +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.interfaces.Accelerometer; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import io.github.d0048.NumPID; + +public class ChasisDriveSubsystem extends Subsystem { + private static final Logger logger = Logger.getLogger(ChasisDriveSubsystem.class.getName()); + public static boolean CHASIS_DEBUG = false; + public static boolean LOCK_OVERRIDE = false; + + public static enum CHASIS_MODE { + TANK, ARCADE + }; + + public CHASIS_MODE mode; + + SpeedController mLeftMotor = Robot.m_oi.mLeftMotor; + SpeedController mRightMotor = Robot.m_oi.mRightMotor; + DifferentialDrive mDifferentialDrive = Robot.m_oi.mDifferentialDrive; + + Gyro mGyro = Robot.m_oi.mGyro; + Accelerometer mAccelerometer = Robot.m_oi.mAccelerometer; + + // PID Speed Control + /** + * requested: src|requested: dst|actual: current + */ + public PIDController mPIDArcadeAngCtler;// Arcade drive mode + public PIDController mPIDArcadeDispCtler;// Arcade drive mode + public double requestedDisp = Robot.m_GyroSubsystem.getPosition(), requestedSpd = 0, + requestedAng = Robot.m_GyroSubsystem.getAngle(); + + /** + * Either (arcade, spd_pid, ang_pid) Or (Tank, left_pid, right_pid) !!!Ang pid + * not enabled + * + * @param mode + * @param numPID1 + * @param numPID2 + */ + public ChasisDriveSubsystem(CHASIS_MODE mode, NumPID numPID1, NumPID numPID2) { + super(); + this.mode = mode; + this.mDifferentialDrive.setSafetyEnabled(false); + logger.info("CHasis mode: " + (mode.equals(CHASIS_MODE.ARCADE) ? "Arcade" : "Tank")); + if (mode == CHASIS_MODE.ARCADE) {// Arcade mode pid + // Arcade angle + // pid-------------------------------------------------------------- + this.mPIDArcadeAngCtler = new PIDController(numPID2.P, numPID2.I, numPID2.D, new PIDSource() { + + PIDSourceType pidSourceType = PIDSourceType.kDisplacement; + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + this.pidSourceType = pidSource; + } + + @Override + public double pidGet() { + SmartDashboard.putBoolean("Dispatch Update:", !LOCK_OVERRIDE); + return Robot.m_GyroSubsystem.getAngle(); + } + + @Override + public PIDSourceType getPIDSourceType() { + return this.pidSourceType; + } + }, new PIDOutput() { + + @Override + public void pidWrite(double output) { + // execAng = output; + if (!LOCK_OVERRIDE) { + mArcadeDispatch(); + } else { + logger.info("Chassis pid Override"); + resetAngleState(); + } + + } + }); + this.mPIDArcadeAngCtler.setAbsoluteTolerance(8); + this.mPIDArcadeAngCtler.setInputRange(Integer.MIN_VALUE, Integer.MAX_VALUE); + this.mPIDArcadeAngCtler.setOutputRange(-0.9, 0.9); + this.mPIDArcadeAngCtler.setContinuous(true); + this.mPIDArcadeAngCtler.enable();// TODO: Enable + // Arcade angle + // pid-------------------------------------------------------------- + + this.mPIDArcadeDispCtler = new PIDController(numPID1.P, numPID1.I, numPID1.D, new PIDSource() { + + PIDSourceType pidSourceType = PIDSourceType.kDisplacement; + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + + this.pidSourceType = pidSource; + } -public class ChasisDriveSubsystem extends PIDSubsystem { + @Override + public double pidGet() { + return Robot.m_GyroSubsystem.getPosition(); + } - Spark mLeftMotor = new Spark(RobotMap.LEFT_MOTOR_GROUP); - Spark mRightMotor = new Spark(RobotMap.RIGHT_MOTOR_GROUP); - DifferentialDrive mDifferentialDrive = new DifferentialDrive(mLeftMotor, mRightMotor); + @Override + public PIDSourceType getPIDSourceType() { + return this.pidSourceType; + } + }, new PIDOutput() { - // Put methods for controlling this subsystem - // here. Call these from Commands. - public ChasisDriveSubsystem(double p, double i, double d) { - super(p, i, d); - setAbsoluteTolerance(0.05); - getPIDController().setContinuous(false); + @Override + public void pidWrite(double output) { + // requestedSpd = output; + if (!LOCK_OVERRIDE) { + mArcadeDispatch(); + } else { + logger.info("Chassis pid Override"); + resetAngleState(); + } + } + }); + } + this.mPIDArcadeDispCtler.setAbsoluteTolerance(8); + this.mPIDArcadeDispCtler.setInputRange(Integer.MIN_VALUE, Integer.MAX_VALUE); + this.mPIDArcadeDispCtler.setOutputRange(-0.9, 0.9); + this.mPIDArcadeDispCtler.setContinuous(false); + this.mPIDArcadeDispCtler.enable();// TODO: Enable } public void initDefaultCommand() { - // Set the default command for a subsystem here. - setDefaultCommand(new TeleopTankDriveCommand()); + this.setDefaultCommand(new TeleopArcadeDriveCommand()); } - public void mDriveTank(Joystick j) { - // x: [0,1];y: [0,1] - mDifferentialDrive.tankDrive(j.getY() * RobotMap.TELE_LEFT_SPEED_FACTOR, - j.getThrottle() * RobotMap.TELE_RIGHT_SPEED_FACTOR); + public void mDriveTank(double l, double r) { + // x: [-1,1];y: [-1,1] + if (mode != CHASIS_MODE.TANK) { + logger.warning("This chasis is not init to tank mode!"); + if (RobotMap.DEBUG) { + logger.warning("Ignoring tank drive cmd"); + return; + } + } + mDifferentialDrive.tankDrive(l * RobotMap.MOTOR.TELE_LEFT_SPEED_FACTOR, + r * RobotMap.MOTOR.TELE_RIGHT_SPEED_FACTOR); + } + + public void mDriveArcade(double xSpeed, double zRot) {// TODO: TEST + // x: [-1,1] + if (mode != CHASIS_MODE.ARCADE) { + logger.warning("This chasis is not init to arcade mode!"); + if (RobotMap.DEBUG) { + logger.warning("Ignoring arcade drive cmd"); + return; + } + } + if (CHASIS_DEBUG) { + logger.info("ArcadeDrive:" + xSpeed + ", " + zRot); + } + if (Robot.EMERGENCY_HALT) { + logger.warning("Emergency stop detected thus cancel"); + return; + } + if (Math.abs(zRot) * RobotMap.MOTOR.TELE_ANG_FACTOR > 0.8 + || Math.abs(xSpeed) * RobotMap.MOTOR.TELE_SPD_FACTOR > 0.8) { + logger.warning("Threadhold reached, cancelled"); + return; + } + mDifferentialDrive.arcadeDrive(xSpeed * RobotMap.MOTOR.TELE_SPD_FACTOR, zRot * RobotMap.MOTOR.TELE_ANG_FACTOR); + writeStatus(); + } + + public void mArcadeRequest(double spd, double rot) { + this.requestedSpd = spd; + /* + * if (this.requestedAng + rot > 180) { this.requestedAng = (this.requestedAng + + * rot - 360); } else if (this.requestedAng + rot < -180) { this.requestedAng = + * (this.requestedAng + rot + 360); } else { + * + * requestedAng += rot; } + */ + this.requestedAng += rot; + mPIDArcadeAngCtler.setSetpoint(this.requestedAng); + SmartDashboard.putNumber("Requested Angle: ", requestedAng); + SmartDashboard.putNumber("Requested speed", requestedSpd); + if (CHASIS_DEBUG) { + logger.info("Got an request of" + spd + ", " + rot); + } + writeStatus(); + } + + public void mArcadeRequestAbsolute(double disp, double rot) { + this.requestedDisp += disp; + rot -= 1; + this.requestedAng = (requestedAng - requestedAng % 360) + rot; + mPIDArcadeAngCtler.setSetpoint(this.requestedAng); + mPIDArcadeDispCtler.setSetpoint(this.requestedDisp); + SmartDashboard.putNumber("Requested Angle: ", requestedAng); + SmartDashboard.putNumber("Requested Disp", requestedSpd); + if (CHASIS_DEBUG) { + logger.info("Got an absolute request of" + disp + ", " + rot); + } + writeStatus(); + } + + public void mArcadeForceDrive(double spd, double rot) { + writeStatus(); + resetAngleState(); + writeStatus(); + this.mDriveArcade(spd, rot); + writeStatus(); + resetAngleState(); + writeStatus(); + } + + public void mArcadeDispatch() { + if (this.requestedSpd >= 0.05) { + this.mDriveArcade(this.requestedSpd, this.mPIDArcadeAngCtler.get()); + SmartDashboard.putBoolean("Use speed", false); + } else { + this.mDriveArcade(this.mPIDArcadeDispCtler.get(), mPIDArcadeAngCtler.get()); + } + writeStatus(); + } + + public void writeStatus() { + SmartDashboard.putNumber("Requested Angle: ", requestedAng); + SmartDashboard.putNumber("Requested speed", requestedSpd != 0 ? requestedSpd : this.mPIDArcadeDispCtler.get()); + SmartDashboard.putNumber("Exec Angle: ", this.mPIDArcadeAngCtler.get()); + SmartDashboard.putNumber("Measured angle", Robot.m_GyroSubsystem.getAngle()); + SmartDashboard.putNumber("Gyro ang", this.mGyro.getAngle()); + SmartDashboard.putNumber("Gyro Absolute ang", Robot.m_GyroSubsystem.getAbsoluteAngle()); + SmartDashboard.putNumber("Encoder displacement", Robot.m_GyroSubsystem.getPosition()); + SmartDashboard.putNumber("Requested displacement", Robot.m_GyroSubsystem.getPosition()); + } + + public void resetAngleState() { + this.requestedAng = Robot.m_GyroSubsystem.getAngle(); + this.requestedDisp = Robot.m_GyroSubsystem.getPosition(); + this.mPIDArcadeAngCtler.setSetpoint(Robot.m_GyroSubsystem.getAngle()); + this.mPIDArcadeDispCtler.setSetpoint(Robot.m_GyroSubsystem.getPosition()); + writeStatus(); + if (CHASIS_DEBUG) { + logger.info("Chasis angle reset"); + } + } + + public boolean isOnPosition() { + return (Math.abs(mPIDArcadeAngCtler.get()) < 0.02 && Math.abs(mPIDArcadeDispCtler.get()) < 0.02); } public void stopDrive() { + logger.info("Chassis Drive stops"); mLeftMotor.stopMotor(); mRightMotor.stopMotor(); } @Override - protected double returnPIDInput() { - // TODO Return PID offset - return 0; + public void finalize() { + mGyro.free(); } - @Override - protected void usePIDOutput(double output) { - // TODO Use PID output - - } } diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/GyroSubsystem.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/GyroSubsystem.java new file mode 100644 index 0000000..511edbb --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/GyroSubsystem.java @@ -0,0 +1,68 @@ +package org.usfirst.frc.team7146.robot.subsystems; + +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.interfaces.Accelerometer; +import edu.wpi.first.wpilibj.interfaces.Gyro; + +public class GyroSubsystem extends Subsystem { + private static final Logger logger = Logger.getLogger(GyroSubsystem.class.getName()); + public static final double DISP_2_METER = 8885; + + public Gyro mGyro = Robot.m_oi.mGyro; + public Accelerometer mAccel = Robot.m_oi.mAccelerometer; + public TalonSRX mTalon1 = Robot.m_oi.mTalon1; + + public GyroSubsystem() { + super("GyroSubsystem"); + this.calibrate(); + } + + public void calibrate() { + logger.info("Calibrating gyro/acc"); + Robot.m_oi.mGyro.reset(); + // Robot.m_oi.mGyro.calibrate(); + logger.info("Calibration done"); + } + + public double getPosition() { + return mTalon1.getSensorCollection().getQuadraturePosition(); + } + + public ErrorCode setPosition(int newPosition) { + return mTalon1.getSensorCollection().setQuadraturePosition(newPosition, 19); + } + + public double getAngle() { + double angle = this.mGyro.getAngle();/* + * while (!(angle > -180 && angle < 180)) { if (angle > 180) { angle -= + * 360; } else if (angle < -180) { angle += 180; } } + */ + return angle; + } + + public double getAbsoluteAngle() { + double angle = this.mGyro.getAngle(); + while (!(angle > -180 && angle < 180)) { + if (angle > 180) { + angle -= 360; + } else if (angle < -180) { + angle += 180; + } + } + return angle + 180; + + } + + @Override + protected void initDefaultCommand() { + + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/LiftSubsystem.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/LiftSubsystem.java new file mode 100644 index 0000000..c2c1937 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/LiftSubsystem.java @@ -0,0 +1,65 @@ +package org.usfirst.frc.team7146.robot.subsystems; + +import org.usfirst.frc.team7146.robot.Robot; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.command.PIDSubsystem; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import io.github.d0048.NumPID; + +public class LiftSubsystem extends Subsystem { + public static enum POSITION { + UP, MID, DOWN + }; + + DigitalInput mUpSw = Robot.m_oi.mLimitSwitchUp; + DigitalInput mMidSw = Robot.m_oi.mLimitSwitchDw; + DigitalInput mDwSw = Robot.m_oi.mLimitSwitchDw; + SpeedController mMotor = Robot.m_oi.liftMotor; + + public LiftSubsystem() { + super("LiftSubsystem"); + // TODO Auto-generated constructor stub + } + + public void up() { + mMotor.set(0.98); + } + + public void down() { + mMotor.set(-0.98); + } + + public void stop() { + mMotor.stopMotor(); + } + + public boolean isUp() { + return !mUpSw.get(); + } + + public boolean isMiddle() { + return !mMidSw.get(); + + } + + public boolean isDown() { + return !mDwSw.get(); + } + + public void writeStatus() { + SmartDashboard.putBoolean("Switch Up", mUpSw.get()); + SmartDashboard.putBoolean("Switch Middle", mMidSw.get()); + SmartDashboard.putBoolean("Switch Down", mDwSw.get()); + SmartDashboard.putNumber("Switch Speed", mMotor.get()); + } + + @Override + protected void initDefaultCommand() { + // TODO Auto-generated method stub + + } + +} diff --git a/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/VisionSubsystem.java b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/VisionSubsystem.java new file mode 100644 index 0000000..57ae399 --- /dev/null +++ b/proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/subsystems/VisionSubsystem.java @@ -0,0 +1,188 @@ +package org.usfirst.frc.team7146.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; +import java.util.logging.Logger; + +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.RotatedRect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +import edu.wpi.cscore.CvSink; +import edu.wpi.cscore.CvSource; +import edu.wpi.cscore.UsbCamera; +import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import io.github.d0048.Utils; +import io.github.d0048.vision.VisualTarget; + +public class VisionSubsystem extends Subsystem { + + private static final java.util.logging.Logger logger = Logger.getLogger(VisionSubsystem.class.getName()); + public CameraServer mCameraServer = CameraServer.getInstance(); + public UsbCamera mUsbCamera = mCameraServer.startAutomaticCapture(); + CvSink cvSink = mCameraServer.getVideo(); + CvSource cvSrcOut, cvSrcMask; + // MjpegServer mjpegServer = mCameraServer.addServer("Cubejpeg"); + public boolean VisionDebug = false; + + public VisionSubsystem() { + this("VisionSubSystem"); + this.mUsbCamera.setFPS(10); + this.mUsbCamera.setResolution(160, 120); + cvSrcOut = mCameraServer.putVideo("src out", 160, 120); + cvSrcMask = mCameraServer.putVideo("src mask", 160, 120); + this.cvSrcOut.setFPS(1); + // this.mjpegServer.setSource(this.cvSrcOut); + // this.mUsbCamera.setExposureAuto(); + } + + public VisionSubsystem(String name) { + super(name); + } + + @Override + protected void initDefaultCommand() { + // TODO + } + + MatOfPoint LargestCnt(List cnts) { + int max_area = -1; + MatOfPoint max = null; + for (MatOfPoint cnt : cnts) { + if (Imgproc.contourArea(cnt) >= max_area) { + try { + (max).release(); + } catch (Exception e) { + } + max = cnt; + } + } + return max; + } + + Scalar lowerY = new Scalar(20, 90, 90); + Scalar upperY = new Scalar(35, 250, 250); + + /** + * + * @return null if nothing found + */ + double minSize = 10; + + public VisualTarget findCube() throws Exception { + // TODO + List toRelsease = new ArrayList(); + + Mat cubeMat = new Mat(); + toRelsease.add(cubeMat); + if (0 == cvSink.grabFrame(cubeMat)) { + logger.warning("Error grabbing fram from camera"); + } + + Mat cubeMatHSV = new Mat(); + toRelsease.add(cubeMatHSV); + Imgproc.cvtColor(cubeMat, cubeMatHSV, Imgproc.COLOR_BGR2HSV); + + Mat cubeMatBlur = new Mat(); + toRelsease.add(cubeMatBlur); + Imgproc.GaussianBlur(cubeMatHSV, cubeMatBlur, new Size(3, 3), 0); + + Mat cubeMatFilter = new Mat(); + toRelsease.add(cubeMatFilter); + Imgproc.bilateralFilter(cubeMatBlur, cubeMatFilter, 9, 9, 75); + + Mat cubeMask = cubeMatHSV.clone(); + toRelsease.add(cubeMask); + Core.inRange(cubeMask, lowerY, upperY, cubeMask); + + List contours = new ArrayList(); + Imgproc.findContours(cubeMask.clone(), contours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); + MatOfPoint max = this.LargestCnt(contours); + if (max == null) { + logger.info("Nothing found"); + return null; + } + toRelsease.add(max); + + MatOfPoint2f max2f = new MatOfPoint2f(); + max.convertTo(max2f, CvType.CV_32F); + toRelsease.add(max2f); + + if (Imgproc.contourArea(max2f) > minSize) { + Imgproc.drawContours(cubeMat, contours, -1, new Scalar(100, 100, 256), 4); + } + RotatedRect cube = Imgproc.minAreaRect(max2f); + VisualTarget ret = new VisualTarget(cube.center, cube.size); + if (VisionDebug) { + logger.info("Detected:" + cube.toString()); + } + + if (cube.size.area() > minSize) { + SmartDashboard.putNumber("Cube X:", cube.center.x); + SmartDashboard.putNumber("Cube Y:", cube.center.y); + SmartDashboard.putNumber("Cube Size", cube.size.area()); + } + this.cvSrcMask.putFrame(cubeMask); + this.cvSrcOut.putFrame(cubeMat); + + for (Mat m : toRelsease) { + try { + m.release(); + } catch (Exception e) { + e.printStackTrace(); + } + } + + return ret.size.area() > minSize ? ret : null; + } + + public void dispFram(Mat m) { + // this.cvSrcOut.notifyError(cvSink.getError()); + this.cvSrcOut.putFrame(m); + + } + + /** + * + * @return null if nothing found + */ + public VisualTarget findBusket() { + // TODO + return null; + } + + /** + * + * @return null if nothing found + */ + public VisualTarget findPWStation() { + // TODO + return null; + } +} + +class VisualTarget { + // in view,deviation from center + public double x = 0, y = 0, area = 0; + public double lu = 0, ru = 0, ld = 0, rd = 0; + + public VisualTarget(double x, double y, double area, double lu, double ru, double ld, double rd) { + super(); + this.x = x; + this.y = y; + this.area = area; + this.lu = lu; + this.ru = ru; + this.ld = ld; + this.rd = rd; + } + +} \ No newline at end of file diff --git a/proj/FRC7146_Java/src/testCommands/AccelTestCMD.java b/proj/FRC7146_Java/src/testCommands/AccelTestCMD.java new file mode 100644 index 0000000..4487e24 --- /dev/null +++ b/proj/FRC7146_Java/src/testCommands/AccelTestCMD.java @@ -0,0 +1,85 @@ +package testCommands; + +import java.math.BigDecimal; +import java.util.logging.Logger; + +import org.usfirst.frc.team7146.robot.Robot; +import org.usfirst.frc.team7146.robot.commands.CmdBase; + +import edu.wpi.first.wpilibj.interfaces.Accelerometer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class AccelTestCMD extends CmdBase { + private static final java.util.logging.Logger logger = Logger.getLogger(AccelTestCMD.class.getName()); + public static CmdBase instance; + public Accelerometer mAccel = Robot.m_GyroSubsystem.mAccel; + double ofs = 0; + + public AccelTestCMD() { + super("AccelTestCMD", 99); + requires(Robot.m_ChasisDriveSubsystem); + requires(Robot.m_GyroSubsystem); + + logger.info("Instance created"); + AccelTestCMD.instance = this; + Robot.m_oi.mCommands.put(this.getName(), this); + int itr = 100; + for (int i = itr; i > 0; i--) { + this.ofs += mAccel.getX(); + } + this.ofs /= itr; + + mAccel.getX(); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + public BigDecimal v = new BigDecimal(0), x = new BigDecimal(0), dt = new BigDecimal(1 / 3); + int itr = 5, i = itr; + + public void disp() { + // if (!Robot.cmdCanRun(this)) + // return; + /* + * v = v.add(dt.multiply(new BigDecimal((mAccel.getX())))); x = + * x.add(v.multiply(dt)); logger.info("Accel:\n" + "\na: " + (mAccel.getX() - + * ofs) + "\nv: " + v + "\nx: " + x); SmartDashboard.putNumber("AccX", + * (mAccel.getX() - ofs)); SmartDashboard.putNumber("v", this.v.doubleValue()); + * SmartDashboard.putNumber("x", this.x.doubleValue()); + */ + if (i-- < 0) { + i = itr; + SmartDashboard.putNumber("Encoder position", Robot.m_GyroSubsystem.getPosition()); + logger.info("Encode velocity: " + Robot.m_oi.mTalon1.getSelectedSensorVelocity(0)); + logger.info("Encoder position: " + Robot.m_oi.mTalon1.getSelectedSensorPosition(0)); + } + } + + // 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() { + super.end(); + Robot.m_ChasisDriveSubsystem.stopDrive(); + logger.info("Instance destroyed"); + AccelTestCMD.instance = null; + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + end(); + } + +} diff --git a/proj/cube_detection/example.py b/proj/cube_detection/example.py index 8c125f8..77cb3d3 100644 --- a/proj/cube_detection/example.py +++ b/proj/cube_detection/example.py @@ -8,15 +8,16 @@ colorUpper = np.array([255, 200, 200], dtype="uint8") # Hue from http://color.yafla.com/ -lower_yellow = np.array([30, 60, 60]) -upper_yellow = np.array([40, 255, 255]) +lower_yellow = np.array([25, 90, 90]) +upper_yellow = np.array([30, 250, 250]) camera = cv2.VideoCapture(1) camera.set(cv2.CAP_PROP_AUTO_EXPOSURE, True) +camera.set(cv2.CAP_PROP_AUTO_EXPOSURE, False) print(camera.isOpened()) while True: - (grabbed, frame) = camera.read(cv2.COLOR_BGR2HSV) + (grabbed, frame) = camera.read(cv2.COLOR_RGB2HSV) cv2.imshow('original', frame) # Remove noise @@ -37,7 +38,7 @@ approx = cv2.approxPolyDP(cnt, 0.01 * cv2.arcLength(cnt, True), True) rect = np.int32(cv2.boxPoints(cv2.minAreaRect(cnt))) # Draw a rectangular frame around the detected object - frame = cv2.drawContours(frame, [rect], -1, (100, 100, 256), 4) + frame = cv2.drawContours(frame, [rect, approx], -1, (100, 100, 256), 4) cv2.imshow('classified', frame) time.sleep(0.025)