Daily code sync #2

.gitignore

@@ -0,0 +1,6 @@
+
+proj/FRC7146_Java/bin/
+
+proj/FRC7146_Java/dist/
+
+doc/2017RobotQuickBuild/

proj/FRC7146_Java/.classpath

@@ -7,5 +7,9 @@
 	<classpathentry kind="var" path="cscore" sourcepath="cscore.sources"/>
 	<classpathentry kind="var" path="wpiutil" sourcepath="wpiutil.sources"/>
 	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.8"/>
+	<classpathentry kind="lib" path="/home/d0048/wpilib/user/java/lib/CTRE_Phoenix.jar"/>
+	<classpathentry kind="lib" path="/home/d0048/wpilib/user/java/lib/CTRE_Phoenix-sources.jar"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix-sources.jar"/>
+	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

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

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
+simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world
+# lib.path=/home/d0048/Program-code/netbean-java/FRC2018/FRC/proj/FRC7146_Java/deps

proj/FRC7146_Java/build.xml

@@ -2,29 +2,29 @@
 
 <project name="FRC Deployment" default="deploy">
 
-  <!--
+	<!--
   The following properties can be defined to override system level
   settings. These should not be touched unless you know what you're
   doing. The primary use is to override the wpilib version when
   working with older robots that can't compile with the latest
   libraries.
   -->
 
-  <!-- By default the system version of WPI is used -->
-  <!-- <property name="version" value=""/> -->
+	<!-- By default the system version of WPI is used -->
+	<!-- <property name="version" value=""/> -->
 
-  <!-- By default the system team number is used -->
-  <!-- <property name="team-number" value=""/> -->
+	<!-- By default the system team number is used -->
+	<!-- <property name="team-number" value=""/> -->
 
-  <!-- By default the target is set to 10.TE.AM.2 -->
-  <!-- <property name="target" value=""/> -->
+	<!-- By default the target is set to 10.TE.AM.2 -->
+	<!-- <property name="target" value=""/> -->
 
-  <!-- Any other property in build.properties can also be overridden. -->
+	<!-- Any other property in build.properties can also be overridden. -->
 
-  <property file="${user.home}/wpilib/wpilib.properties"/>
-  <property file="build.properties"/>
-  <property file="${user.home}/wpilib/java/${version}/ant/build.properties"/>
-
-  <import file="${wpilib.ant.dir}/build.xml"/>
+	<property file="${user.home}/wpilib/wpilib.properties" />
+	<property file="build.properties" />
+	<property file="${user.home}/wpilib/java/${version}/ant/build.properties" />
+	<!--<property name="classpath" value="${wpilib.jar}:${networktables.jar}:${lib.path}" />-->
 
+	<import file="${wpilib.ant.dir}/build.xml" />
 </project>

proj/FRC7146_Java/deps/README.md

@@ -0,0 +1 @@
+### Copy all the files here to `~/wpilib/user/java/lib/` for the to be recognized!!

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;
+	}
+
+}

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<Mat> os) {
+		for (Object o : os) {
+			try {
+				((Mat) o).release();
+			} finally {
+
+			}
+		}
+	}
+
+}

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;
+	}
+
+}

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;
+		}
+	}
+
+}

proj/FRC7146_Java/src/org/usfirst/frc/team7146/robot/OI.java

@@ -7,26 +7,89 @@
 
 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
  * interface to the commands and command groups that allow control of the robot.
  */
 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<String, CmdBase> mCommands = new HashMap<String, CmdBase>();
 
 	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));
+
 	}
 }