Adds TriState JNI entry point (#938)

Also adds missing sim TriState DIO HAL call, and a ToDo for later

hal/src/main/native/athena/ctre/CtreCanNode.cpp

@@ -68,17 +68,13 @@ void CtreCanNode::UnregisterTx(uint32_t arbId)
 		_txJobs.erase(iter);
 	}
 }
-timespec diff(const timespec & start, const timespec & end)
+static int64_t GetTimeMs() {
-{
+	std::chrono::time_point < std::chrono::system_clock > now;
-	timespec temp;
+	now = std::chrono::system_clock::now();
-	if ((end.tv_nsec-start.tv_nsec)<0) {
+	auto duration = now.time_since_epoch();
-		temp.tv_sec = end.tv_sec-start.tv_sec-1;
+	auto millis = std::chrono::duration_cast < std::chrono::milliseconds
-		temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
+					> (duration).count();
-	} else {
+	return (int64_t) millis;
-		temp.tv_sec = end.tv_sec-start.tv_sec;
-		temp.tv_nsec = end.tv_nsec-start.tv_nsec;
-	}
-	return temp;
 }
 CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs)
 {
@@ -90,10 +86,11 @@ CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeout
 	if(timeoutMs > 999)
 		timeoutMs = 999;
 	FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status);
+	std::lock_guard<wpi::mutex> lock(_lck);
 	if(status == 0){
 		/* fresh update */
 		rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */
-		clock_gettime(2,&r.time); 			/* fill in time */
+		r.time = GetTimeMs();
 		memcpy(r.bytes,  dataBytes,  8);	/* fill in databytes */
 	}else{
 		/* did not get the message */
@@ -107,16 +104,13 @@ CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeout
 			/* we've gotten this message before but not recently */
 			memcpy(dataBytes,i->second.bytes,8);
 			/* get the time now */
-			struct timespec temp;
+			int64_t now = GetTimeMs(); /* get now */
-			clock_gettime(2,&temp); /* get now */
 			/* how long has it been? */
-			temp = diff(i->second.time,temp); /* temp = now - last */
+			int64_t temp = now - i->second.time; /* temp = now - last */
-			if(temp.tv_sec > 0){
+			if (temp > ((int64_t) timeoutMs)) {
-				retval = CTR_RxTimeout;
+					retval = CTR_RxTimeout;
-			}else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){
+			} else {
-				retval = CTR_RxTimeout;
+					/* our last update was recent enough */
-			}else {
-				/* our last update was recent enough */
 			}
 		}
 	}

hal/src/main/native/athena/ctre/CtreCanNode.h

@@ -4,6 +4,7 @@
 #include <map>
 #include <string.h> // memcpy
 #include <sys/time.h>
+#include <support/mutex.h>
 class CtreCanNode
 {
 public:
@@ -108,7 +109,7 @@ private:
 	class rxEvent_t{
 		public:
 			uint8_t bytes[8];
-			struct timespec time;
+			int64_t time;
 			rxEvent_t()
 			{
 				bytes[0] = 0;
@@ -127,5 +128,7 @@ private:
 
 	typedef std::map<uint32_t,rxEvent_t> rxRxEvents_t;
 	rxRxEvents_t _rxRxEvents;
+
+	wpi::mutex _lck;
 };
 #endif

hal/src/main/native/sim/DIO.cpp

@@ -198,6 +198,23 @@ void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
   SimDIOData[port->channel].SetValue(value);
 }
 
+/**
+ * Set direction of a DIO channel.
+ *
+ * @param channel The Digital I/O channel
+ * @param input true to set input, false for output
+ */
+void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
+                         int32_t* status) {
+  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  SimDIOData[port->channel].SetIsInput(input);
+}
+
 /**
  * Read a digital I/O bit from the FPGA.
  * Get a single value from a digital I/O channel.

wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp

@@ -7,6 +7,7 @@
 
 #include "Drive/DifferentialDrive.h"
 
+#include <algorithm>
 #include <cmath>
 
 #include <HAL/HAL.h>
@@ -171,6 +172,14 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
     }
   }
 
+  // Normalize the wheel speeds
+  double maxMagnitude =
+      std::max(std::abs(leftMotorOutput), std::abs(rightMotorOutput));
+  if (maxMagnitude > 1.0) {
+    leftMotorOutput /= maxMagnitude;
+    rightMotorOutput /= maxMagnitude;
+  }
+
   m_leftMotor.Set(leftMotorOutput * m_maxOutput);
   m_rightMotor.Set(-rightMotorOutput * m_maxOutput);
 

wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp

@@ -134,12 +134,12 @@ void MecanumDrive::InitSendable(SendableBuilder& builder) {
                             [=]() { return m_frontLeftMotor.Get(); },
                             [=](double value) { m_frontLeftMotor.Set(value); });
   builder.AddDoubleProperty(
-      "Front Right Motor Speed", [=]() { return m_frontRightMotor.Get(); },
+      "Front Right Motor Speed", [=]() { return -m_frontRightMotor.Get(); },
-      [=](double value) { m_frontRightMotor.Set(value); });
+      [=](double value) { m_frontRightMotor.Set(-value); });
   builder.AddDoubleProperty("Rear Left Motor Speed",
                             [=]() { return m_rearLeftMotor.Get(); },
                             [=](double value) { m_rearLeftMotor.Set(value); });
-  builder.AddDoubleProperty("Rear Right Motor Speed",
+  builder.AddDoubleProperty(
-                            [=]() { return m_rearRightMotor.Get(); },
+      "Rear Right Motor Speed", [=]() { return -m_rearRightMotor.Get(); },
-                            [=](double value) { m_rearRightMotor.Set(value); });
+      [=](double value) { m_rearRightMotor.Set(-value); });
 }

wpilibc/src/main/native/cpp/Joystick.cpp

@@ -131,7 +131,7 @@ int Joystick::GetThrottleChannel() const { return m_axes[Axis::kThrottle]; }
  *             here to complete the GenericHID interface.
  */
 double Joystick::GetX(JoystickHand hand) const {
-  return GetRawAxis(m_axes[kDefaultXAxis]);
+  return GetRawAxis(m_axes[Axis::kX]);
 }
 
 /**
@@ -143,7 +143,7 @@ double Joystick::GetX(JoystickHand hand) const {
  *             here to complete the GenericHID interface.
  */
 double Joystick::GetY(JoystickHand hand) const {
-  return GetRawAxis(m_axes[kDefaultYAxis]);
+  return GetRawAxis(m_axes[Axis::kY]);
 }
 
 /**
@@ -151,16 +151,14 @@ double Joystick::GetY(JoystickHand hand) const {
  *
  * This depends on the mapping of the joystick connected to the current port.
  */
-double Joystick::GetZ() const { return GetRawAxis(m_axes[kDefaultZAxis]); }
+double Joystick::GetZ() const { return GetRawAxis(m_axes[Axis::kZ]); }
 
 /**
  * Get the twist value of the current joystick.
  *
  * This depends on the mapping of the joystick connected to the current port.
  */
-double Joystick::GetTwist() const {
+double Joystick::GetTwist() const { return GetRawAxis(m_axes[Axis::kTwist]); }
-  return GetRawAxis(m_axes[kDefaultTwistAxis]);
-}
 
 /**
  * Get the throttle value of the current joystick.
@@ -168,7 +166,7 @@ double Joystick::GetTwist() const {
  * This depends on the mapping of the joystick connected to the current port.
  */
 double Joystick::GetThrottle() const {
-  return GetRawAxis(m_axes[kDefaultThrottleAxis]);
+  return GetRawAxis(m_axes[Axis::kThrottle]);
 }
 
 /**

wpilibc/src/main/native/cpp/TimedRobot.cpp

@@ -43,10 +43,15 @@ void TimedRobot::SetPeriod(double period) {
   m_period = period;
 
   if (m_startLoop) {
-    m_loop->StartPeriodic(m_period);
+    m_loop->StartPeriodic(period);
   }
 }
 
+/**
+ * Get time period between calls to Periodic() functions.
+ */
+double TimedRobot::GetPeriod() const { return m_period; }
+
 TimedRobot::TimedRobot() {
   m_loop = std::make_unique<Notifier>(&TimedRobot::LoopFunc, this);
 

wpilibc/src/main/native/include/TimedRobot.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <atomic>
 #include <memory>
 
 #include "IterativeRobotBase.h"
@@ -30,13 +31,14 @@ class TimedRobot : public IterativeRobotBase {
   void StartCompetition() override;
 
   void SetPeriod(double seconds);
+  double GetPeriod() const;
 
  protected:
   TimedRobot();
   virtual ~TimedRobot();
 
  private:
-  double m_period = kDefaultPeriod;
+  std::atomic<double> m_period{kDefaultPeriod};
 
   // Prevents loop from starting if user calls SetPeriod() in RobotInit()
   bool m_startLoop = false;

wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java

@@ -99,6 +99,18 @@ public class Notifier {
       }
     });
     m_thread.setDaemon(true);
+    m_thread.setUncaughtExceptionHandler((thread, error) -> {
+      Throwable cause = error.getCause();
+      if (cause != null) {
+        error = cause;
+      }
+      DriverStation.reportError("Unhandled exception: " + error.toString(), error.getStackTrace());
+      DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+      DriverStation.reportError(
+          "The loopFunc() method (or methods called by it) should have handled "
+              + "the exception above.", false);
+      System.exit(1);
+    });
     m_thread.start();
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java

@@ -21,7 +21,7 @@ import edu.wpi.first.wpilibj.hal.HAL;
 public class TimedRobot extends IterativeRobotBase {
   public static final double DEFAULT_PERIOD = 0.02;
 
-  private double m_period = DEFAULT_PERIOD;
+  private volatile double m_period = DEFAULT_PERIOD;
 
   // Prevents loop from starting if user calls setPeriod() in robotInit()
   private boolean m_startLoop = false;
@@ -68,4 +68,11 @@ public class TimedRobot extends IterativeRobotBase {
       m_loop.startPeriodic(m_period);
     }
   }
+
+  /**
+   * Get time period between calls to Periodic() functions.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java

@@ -261,6 +261,13 @@ public class DifferentialDrive extends RobotDriveBase {
       }
     }
 
+    // Normalize the wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput));
+    if (maxMagnitude > 1.0) {
+      leftMotorOutput /= maxMagnitude;
+      rightMotorOutput /= maxMagnitude;
+    }
+
     m_leftMotor.set(leftMotorOutput * m_maxOutput);
     m_rightMotor.set(-rightMotorOutput * m_maxOutput);
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java

@@ -189,11 +189,11 @@ public class MecanumDrive extends RobotDriveBase {
     builder.setSmartDashboardType("MecanumDrive");
     builder.addDoubleProperty("Front Left Motor Speed", m_frontLeftMotor::get,
         m_frontLeftMotor::set);
-    builder.addDoubleProperty("Front Right Motor Speed", m_frontRightMotor::get,
+    builder.addDoubleProperty("Front Right Motor Speed", () -> -m_frontRightMotor.get(),
-        m_frontRightMotor::set);
+        value -> m_frontRightMotor.set(-value));
     builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get,
         m_rearLeftMotor::set);
-    builder.addDoubleProperty("Rear Right Motor Speed", m_rearRightMotor::get,
+    builder.addDoubleProperty("Rear Right Motor Speed", () -> -m_rearRightMotor.get(),
-        m_rearRightMotor::set);
+        value -> m_rearRightMotor.set(-value));
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/DIOJNI.java

@@ -15,8 +15,11 @@ public class DIOJNI extends JNIWrapper {
 
   public static native void freeDIOPort(int dioPortHandle);
 
+  // TODO(Thad): Switch this to use boolean
   public static native void setDIO(int dioPortHandle, short value);
 
+  public static native void setDIODirection(int dioPortHandle, boolean input);
+
   public static native boolean getDIO(int dioPortHandle);
 
   public static native boolean getDIODirection(int dioPortHandle);

wpilibj/src/main/native/cpp/DIOJNI.cpp

@@ -90,6 +90,22 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_setDIO(
   CheckStatus(env, status);
 }
 
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_DIOJNI
+ * Method:    setDIODirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_setDIODirection(
+  JNIEnv *env, jclass, jint id, jboolean input) {
+  // DIOJNI_LOG(logDEBUG) << "Calling DIOJNI setDIO";
+  // DIOJNI_LOG(logDEBUG) << "Port Handle = " << (HAL_DigitalHandle)id;
+  // DIOJNI_LOG(logDEBUG) << "IsInput = " << input;
+  int32_t status = 0;
+  HAL_SetDIODirection((HAL_DigitalHandle)id, input, &status);
+  // DIOJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
 /*
  * Class:     edu_wpi_first_wpilibj_hal_DIOJNI
  * Method:    getDIO

wpilibj/src/main/native/cpp/EncoderJNI.cpp

@@ -36,7 +36,7 @@ extern "C" {
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_wpilibj_hal_EncoderJNI_initializeEncoder(
     JNIEnv* env, jclass, jint digitalSourceHandleA, jint analogTriggerTypeA,
-    jint digitalSourceHandleB, jint analogTriggerTypeB, jboolean reverseDirection, 
+    jint digitalSourceHandleB, jint analogTriggerTypeB, jboolean reverseDirection,
     jint encodingType) {
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI initializeEncoder";
   ENCODERJNI_LOG(logDEBUG) << "Source Handle A = " << digitalSourceHandleA;
@@ -49,10 +49,10 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_initializeEncoder(
   ENCODERJNI_LOG(logDEBUG) << "EncodingType = " << encodingType;
   int32_t status = 0;
   auto encoder = HAL_InitializeEncoder(
-      (HAL_Handle)digitalSourceHandleA, (HAL_AnalogTriggerType)analogTriggerTypeA, 
+      (HAL_Handle)digitalSourceHandleA, (HAL_AnalogTriggerType)analogTriggerTypeA,
       (HAL_Handle)digitalSourceHandleB, (HAL_AnalogTriggerType)analogTriggerTypeB,
       reverseDirection, (HAL_EncoderEncodingType)encodingType, &status);
-      
+
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   ENCODERJNI_LOG(logDEBUG) << "ENCODER Handle = " << encoder;
   CheckStatusForceThrow(env, status);
@@ -340,7 +340,7 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderSamplesToAverage(
  */
 JNIEXPORT void JNICALL
 Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderIndexSource(
-    JNIEnv* env, jclass, jint id, jint digitalSourceHandle, 
+    JNIEnv* env, jclass, jint id, jint digitalSourceHandle,
     jint analogTriggerType, jint type) {
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderIndexSource";
   ENCODERJNI_LOG(logDEBUG) << "Encoder Handle = " << (HAL_EncoderHandle)id;
@@ -349,8 +349,8 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderIndexSource(
                            << analogTriggerType;
   ENCODERJNI_LOG(logDEBUG) << "IndexingType = " << type;
   int32_t status = 0;
-  HAL_SetEncoderIndexSource((HAL_EncoderHandle)id, (HAL_Handle)digitalSourceHandle, 
+  HAL_SetEncoderIndexSource((HAL_EncoderHandle)id, (HAL_Handle)digitalSourceHandle,
-                            (HAL_AnalogTriggerType)analogTriggerType, 
+                            (HAL_AnalogTriggerType)analogTriggerType,
                             (HAL_EncoderIndexingType)type, &status);
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   CheckStatus(env, status);
@@ -405,7 +405,7 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderDecodingScaleFactor(
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderSamplesToAverage";
   ENCODERJNI_LOG(logDEBUG) << "Encoder Handle = " << (HAL_EncoderHandle)id;
   int32_t status = 0;
-  jint returnValue = HAL_GetEncoderDecodingScaleFactor((HAL_EncoderHandle)id, &status);
+  jdouble returnValue = HAL_GetEncoderDecodingScaleFactor((HAL_EncoderHandle)id, &status);
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   ENCODERJNI_LOG(logDEBUG) << "getEncoderSamplesToAverageResult = "
                            << returnValue;
@@ -424,7 +424,7 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderDistancePerPulse(
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderSamplesToAverage";
   ENCODERJNI_LOG(logDEBUG) << "Encoder Handle = " << (HAL_EncoderHandle)id;
   int32_t status = 0;
-  jint returnValue = HAL_GetEncoderDistancePerPulse((HAL_EncoderHandle)id, &status);
+  jdouble returnValue = HAL_GetEncoderDistancePerPulse((HAL_EncoderHandle)id, &status);
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   ENCODERJNI_LOG(logDEBUG) << "getEncoderSamplesToAverageResult = "
                            << returnValue;