Fixed robot drive for C++ Simulation

initial values for m_invertedMotors is now 1
previously it was done in some of the constructors, but not all of them.

Change-Id: I2c1ce8d8a67f82d02c4c51f1c4d1aaad143f3112

build.gradle

@@ -18,7 +18,6 @@ allprojects {
         maven {
             url publishUrl
         }
-        mavenLocal()
         maven {
             url repoBaseUrl
         }

hal/lib/Athena/ctre/PDP.cpp

@@ -11,10 +11,10 @@
 #define CONTROL_1		0x08041C00	/* PDP_Control_ClearStats */
 
 #define EXPECTED_RESPONSE_TIMEOUT_MS	(50)
-#define GET_STATUS1()		CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_STATUS2()		CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_STATUS3()		CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3,EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_STATUS_ENERGY()	CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY,EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS1()		CtreCanNode::recMsg<PdpStatus1_t> rx = GetRx<PdpStatus1_t>(STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS2()		CtreCanNode::recMsg<PdpStatus2_t> rx = GetRx<PdpStatus2_t>(STATUS_2|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS3()		CtreCanNode::recMsg<PdpStatus3_t> rx = GetRx<PdpStatus3_t>(STATUS_3|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
+#define GET_STATUS_ENERGY()	CtreCanNode::recMsg<PDP_Status_Energy_t> rx = GetRx<PDP_Status_Energy_t>(STATUS_ENERGY|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
 
 /* encoder/decoders */
 typedef struct _PdpStatus1_t{

hal/lib/Shared/HAL.cpp

@@ -34,11 +34,32 @@ int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons)
 {
 	return FRC_NetworkCommunication_getJoystickButtons(joystickNum, &buttons->buttons, &buttons->count);
 }
-
+/**
+ * Retrieve the Joystick Descriptor for particular slot
+ * @param desc [out] descriptor (data transfer object) to fill in.  desc is filled in regardless of success.
+ *                              In other words, if descriptor is not available, desc is filled in with default
+ *                              values matching the init-values in Java and C++ Driverstation for when caller
+ *                              requests a too-large joystick index.
+ *
+ * @return error code reported from Network Comm back-end.  Zero is good, nonzero is bad.
+ */
 int HALGetJoystickDescriptor(uint8_t joystickNum, HALJoystickDescriptor *desc)
 {
-	return FRC_NetworkCommunication_getJoystickDesc(joystickNum, &desc->isXbox, &desc->type, (char *)(&desc->name),
+	desc->isXbox = 0;
+	desc->type = -1;
+	desc->name[0] = '\0';
+	desc->axisCount = kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */
+	desc->buttonCount = 0;
+	desc->povCount = 0;
+	int retval = FRC_NetworkCommunication_getJoystickDesc(joystickNum, &desc->isXbox, &desc->type, (char *)(&desc->name),
 		&desc->axisCount, (uint8_t *)&desc->axisTypes, &desc->buttonCount, &desc->povCount);
+	/* check the return, if there is an error and the RIOimage predates FRC2017, then axisCount needs to be cleared */
+	if(retval != 0)
+	{
+		/* set count to zero so downstream code doesn't decode invalid axisTypes. */
+		desc->axisCount = 0;
+	}
+	return retval;
 }
 
 int HALGetJoystickIsXbox(uint8_t joystickNum)

simulation/install_resources/sim_ds

@@ -1 +1 @@
-java -Djava.library.path=~/wpilib/simulation/lib -jar ~/wpilib/simulation/jar/SimDS-all.jar
+java -Djava.library.path=${HOME}/wpilib/simulation/lib -jar ~/wpilib/simulation/jar/SimDS-all.jar

wpilibc/Athena/src/DriverStation.cpp

@@ -159,7 +159,7 @@ std::string DriverStation::GetJoystickName(uint32_t stick) const {
   if (stick >= kJoystickPorts) {
     wpi_setWPIError(BadJoystickIndex);
   }
-  std::string retVal(m_joystickDescriptor[0].name);
+  std::string retVal(m_joystickDescriptor[stick].name);
   return retVal;
 }
 

wpilibc/Athena/src/PIDController.cpp

@@ -469,6 +469,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
  * @param bufLength Number of previous cycles to average. Defaults to 1.
  */
 void PIDController::SetToleranceBuffer(unsigned bufLength) {
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   m_bufLength = bufLength;
 
   // Cut the buffer down to size if needed.
@@ -489,10 +490,9 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) {
  * This will return false until at least one input value has been computed.
  */
 bool PIDController::OnTarget() const {
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   if (m_buf.size() == 0) return false;
   double error = GetAvgError();
-
-  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
   switch (m_toleranceType) {
     case kPercentTolerance:
       return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);

wpilibc/Athena/src/RobotBase.cpp

@@ -27,6 +27,7 @@ void RobotBase::setInstance(RobotBase *robot) {
 RobotBase &RobotBase::getInstance() { return *m_instance; }
 
 void RobotBase::robotSetup(RobotBase *robot) {
+  printf("\n********** Robot program starting **********\n");
   robot->StartCompetition();
 }
 
@@ -55,7 +56,7 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
   file = fopen("/tmp/frc_versions/FRC_Lib_Version.ini", "w");
 
   if (file != nullptr) {
-    fputs("2016 C++ Beta5.0", file);
+    fputs("2016 C++ Release 3", file);
     fclose(file);
   }
 }

wpilibc/Athena/src/Ultrasonic.cpp

@@ -25,6 +25,7 @@ constexpr double Ultrasonic::kSpeedOfSoundInchesPerSec;
 Task Ultrasonic::m_task;
 // automatic round robin mode
 std::atomic<bool> Ultrasonic::m_automaticEnabled{false};
+std::set<Ultrasonic*> Ultrasonic::m_sensors;
 
 /**
  * Background task that goes through the list of ultrasonic sensors and pings

wpilibc/simulation/include/AnalogGyro.h

@@ -7,9 +7,7 @@
 
 #pragma once
 
-#include "SensorBase.h"
-#include "PIDSource.h"
-#include "LiveWindow/LiveWindowSendable.h"
+#include "GyroBase.h"
 #include "simulation/SimGyro.h"
 
 #include <memory>
@@ -19,7 +17,7 @@ class AnalogModule;
 
 /**
  * Use a rate gyro to return the robots heading relative to a starting position.
- * The Gyro class tracks the robots heading based on the starting position. As the robot
+ * The AnalogGyro class tracks the robots heading based on the starting position. As the robot
  * rotates the new heading is computed by integrating the rate of rotation returned
  * by the sensor. When the class is instantiated, it does a short calibration routine
  * where it samples the gyro while at rest to determine the default offset. This is
@@ -27,7 +25,7 @@ class AnalogModule;
  * with a channel that is assigned one of the Analog accumulators from the FPGA. See
  * AnalogInput for the current accumulator assignments.
  */
-class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable
+class AnalogGyro : public GyroBase
 {
 public:
 	static const uint32_t kOversampleBits;
@@ -36,25 +34,15 @@ public:
 	static const float kCalibrationSampleTime;
 	static const float kDefaultVoltsPerDegreePerSecond;
 
-	explicit Gyro(uint32_t channel);
-	virtual ~Gyro() = default;
-	virtual float GetAngle() const;
-	virtual double GetRate() const;
-	virtual void Reset();
-
-	// PIDSource interface
-	void SetPIDSourceType(PIDSourceType pidSource) override;
-	double PIDGet() override;
-
-	void UpdateTable() override;
-	void StartLiveWindowMode() override;
-	void StopLiveWindowMode() override;
-	std::string GetSmartDashboardType() const override;
-	void InitTable(std::shared_ptr<ITable> subTable) override;
-	std::shared_ptr<ITable> GetTable() const override;
+	explicit AnalogGyro(uint32_t channel);
+	virtual ~AnalogGyro() = default;
+	float GetAngle() const;
+	void Calibrate() override;
+	double GetRate() const;
+	void Reset();
 
 private:
-	void InitGyro(int channel);
+	void InitAnalogGyro(int channel);
 
 	SimGyro* impl;
 

wpilibc/simulation/include/RobotDrive.h

@@ -9,6 +9,7 @@
 
 #include "ErrorBase.h"
 #include <stdlib.h>
+#include <memory>
 #include "MotorSafety.h"
 #include "MotorSafetyHelper.h"
 
@@ -39,11 +40,17 @@ public:
 			uint32_t frontRightMotorChannel, uint32_t rearRightMotorChannel);
 	RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor);
 	RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor);
+	RobotDrive(std::shared_ptr<SpeedController> leftMotor,
+			   std::shared_ptr<SpeedController> rightMotor);
 	RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
 			SpeedController *frontRightMotor, SpeedController *rearRightMotor);
 	RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
 			SpeedController &frontRightMotor, SpeedController &rearRightMotor);
-	virtual ~RobotDrive();
+	RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
+			  std::shared_ptr<SpeedController> rearLeftMotor,
+			  std::shared_ptr<SpeedController> frontRightMotor,
+			  std::shared_ptr<SpeedController> rearRightMotor);
+	virtual ~RobotDrive() = default;
 
     RobotDrive(const RobotDrive&) = delete;
     RobotDrive& operator=(const RobotDrive&) = delete;
@@ -87,14 +94,14 @@ protected:
 
 	static const int32_t kMaxNumberOfMotors = 4;
 
-	int32_t m_invertedMotors[kMaxNumberOfMotors];
+	int32_t m_invertedMotors[kMaxNumberOfMotors] = {1,1,1,1};
 	float m_sensitivity = 0.5;
 	double m_maxOutput = 1.0;
 	bool m_deleteSpeedControllers;
-	SpeedController *m_frontLeftMotor = nullptr;
-	SpeedController *m_frontRightMotor = nullptr;
-	SpeedController *m_rearLeftMotor = nullptr;
-	SpeedController *m_rearRightMotor = nullptr;
+	std::shared_ptr<SpeedController> m_frontLeftMotor;
+	std::shared_ptr<SpeedController> m_frontRightMotor;
+	std::shared_ptr<SpeedController> m_rearLeftMotor;
+	std::shared_ptr<SpeedController> m_rearRightMotor;
 	// FIXME: MotorSafetyHelper *m_safetyHelper;
 
 private:

wpilibc/simulation/include/WPILib.h

@@ -47,7 +47,7 @@
 #include "Counter.h"
 #include "DigitalInput.h"
 #include "Encoder.h"
-#include "Gyro.h"
+#include "AnalogGyro.h"
 #include "GenericHID.h"
 #include "Joystick.h"
 #include "PIDController.h"

wpilibc/simulation/src/AnalogGyro.cpp

@@ -5,16 +5,16 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include "Gyro.h"
+#include "AnalogGyro.h"
 #include "Timer.h"
 #include "WPIErrors.h"
 #include "LiveWindow/LiveWindow.h"
 
-const uint32_t Gyro::kOversampleBits = 10;
-const uint32_t Gyro::kAverageBits = 0;
-const float Gyro::kSamplesPerSecond = 50.0;
-const float Gyro::kCalibrationSampleTime = 5.0;
-const float Gyro::kDefaultVoltsPerDegreePerSecond = 0.007;
+const uint32_t AnalogGyro::kOversampleBits = 10;
+const uint32_t AnalogGyro::kAverageBits = 0;
+const float AnalogGyro::kSamplesPerSecond = 50.0;
+const float AnalogGyro::kCalibrationSampleTime = 5.0;
+const float AnalogGyro::kDefaultVoltsPerDegreePerSecond = 0.007;
 
 /**
  * Initialize the gyro.
@@ -24,7 +24,7 @@ const float Gyro::kDefaultVoltsPerDegreePerSecond = 0.007;
  * in progress, this is typically done when the robot is first turned on while it's sitting at
  * rest before the competition starts.
  */
-void Gyro::InitGyro(int channel)
+void AnalogGyro::InitAnalogGyro(int channel)
 {
 	SetPIDSourceType(PIDSourceType::kDisplacement);
 
@@ -32,17 +32,17 @@ void Gyro::InitGyro(int channel)
 	int n = sprintf(buffer, "analog/%d", channel);
 	impl = new SimGyro(buffer);
 
-	LiveWindow::GetInstance()->AddSensor("Gyro", channel, this);
+	LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this);
 }
 
 /**
- * Gyro constructor with only a channel..
+ * AnalogGyro constructor with only a channel..
  *
  * @param channel The analog channel the gyro is connected to.
  */
-Gyro::Gyro(uint32_t channel)
+AnalogGyro::AnalogGyro(uint32_t channel)
 {
-    InitGyro(channel);
+    InitAnalogGyro(channel);
 }
 
 /**
@@ -50,11 +50,15 @@ Gyro::Gyro(uint32_t channel)
  * Resets the gyro to a heading of zero. This can be used if there is significant
  * drift in the gyro and it needs to be recalibrated after it has been running.
  */
-void Gyro::Reset()
+void AnalogGyro::Reset()
 {
     impl->Reset();
 }
 
+void AnalogGyro::Calibrate(){
+	Reset();
+}
+
 /**
  * Return the actual angle in degrees that the robot is currently facing.
  *
@@ -66,7 +70,7 @@ void Gyro::Reset()
  * @return the current heading of the robot in degrees. This heading is based on integration
  * of the returned rate from the gyro.
  */
-float Gyro::GetAngle() const
+float AnalogGyro::GetAngle() const
 {
     return impl->GetAngle();
 }
@@ -79,56 +83,7 @@ float Gyro::GetAngle() const
  *
  * @return the current rate in degrees per second
  */
-double Gyro::GetRate() const
+double AnalogGyro::GetRate() const
 {
     return impl->GetVelocity();
 }
-
-void Gyro::SetPIDSourceType(PIDSourceType pidSource)
-{
-    m_pidSource = pidSource;
-}
-
-/**
- * Get the angle in degrees for the PIDSource base object.
- *
- * @return The angle in degrees.
- */
-double Gyro::PIDGet()
-{
-	switch(GetPIDSourceType()){
-	case PIDSourceType::kRate:
-		return GetRate();
-	case PIDSourceType::kDisplacement:
-		return GetAngle();
-	default:
-		return 0;
-	}
-}
-
-void Gyro::UpdateTable() {
-	if (m_table != nullptr) {
-		m_table->PutNumber("Value", GetAngle());
-	}
-}
-
-void Gyro::StartLiveWindowMode() {
-
-}
-
-void Gyro::StopLiveWindowMode() {
-
-}
-
-std::string Gyro::GetSmartDashboardType() const {
-	return "Gyro";
-}
-
-void Gyro::InitTable(std::shared_ptr<ITable> subTable) {
-	m_table = subTable;
-	UpdateTable();
-}
-
-std::shared_ptr<ITable> Gyro::GetTable() const {
-	return m_table;
-}

wpilibc/simulation/src/PIDController.cpp

@@ -500,6 +500,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance)
  * @param bufLength Number of previous cycles to average. Defaults to 1.
  */
 void PIDController::SetToleranceBuffer(unsigned bufLength) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
   m_bufLength = bufLength;
 
   // Cut the buffer down to size if needed.
@@ -518,9 +519,9 @@ void PIDController::SetToleranceBuffer(unsigned bufLength) {
  */
 bool PIDController::OnTarget() const
 {
-	double error = GetError();
-
 	std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+	if (m_buf.size() == 0) return false;
+	double error = GetError();
 	switch (m_toleranceType) {
 	case kPercentTolerance:
 		return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);

wpilibc/simulation/src/RobotDrive.cpp

@@ -6,11 +6,10 @@
 /*----------------------------------------------------------------------------*/
 
 #include "RobotDrive.h"
-
 //#include "CANJaguar.h"
 #include "GenericHID.h"
 #include "Joystick.h"
-#include "Jaguar.h"
+#include "Talon.h"
 #include "Utility.h"
 #include "WPIErrors.h"
 #include <math.h>
@@ -20,6 +19,10 @@
 
 const int32_t RobotDrive::kMaxNumberOfMotors;
 
+static auto make_shared_nodelete(SpeedController *ptr) {
+	return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
+}
+
 /*
  * Driving functions
  * These functions provide an interface to multiple motors that is used for C programming
@@ -40,19 +43,15 @@ void RobotDrive::InitRobotDrive() {
 /** Constructor for RobotDrive with 2 motors specified with channel numbers.
  * Set up parameters for a two wheel drive system where the
  * left and right motor pwm channels are specified in the call.
- * This call assumes Jaguars for controlling the motors.
+ * This call assumes Talosn for controlling the motors.
  * @param leftMotorChannel The PWM channel number that drives the left motor.
  * @param rightMotorChannel The PWM channel number that drives the right motor.
  */
 RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
 {
 	InitRobotDrive();
-	m_rearLeftMotor = new Jaguar(leftMotorChannel);
-	m_rearRightMotor = new Jaguar(rightMotorChannel);
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
-	}
+	m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
+	m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
 	SetLeftRightMotorOutputs(0.0, 0.0);
 	m_deleteSpeedControllers = true;
 }
@@ -61,7 +60,7 @@ RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
  * Constructor for RobotDrive with 4 motors specified with channel numbers.
  * Set up parameters for a four wheel drive system where all four motor
  * pwm channels are specified in the call.
- * This call assumes Jaguars for controlling the motors.
+ * This call assumes Talons for controlling the motors.
  * @param frontLeftMotor Front left motor channel number
  * @param rearLeftMotor Rear Left motor channel number
  * @param frontRightMotor Front right motor channel number
@@ -71,14 +70,10 @@ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
 		uint32_t frontRightMotor, uint32_t rearRightMotor)
 {
 	InitRobotDrive();
-	m_rearLeftMotor = new Jaguar(rearLeftMotor);
-	m_rearRightMotor = new Jaguar(rearRightMotor);
-	m_frontLeftMotor = new Jaguar(frontLeftMotor);
-	m_frontRightMotor = new Jaguar(frontRightMotor);
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
-	}
+	m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
+	m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
+	m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
+	m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
 	SetLeftRightMotorOutputs(0.0, 0.0);
 	m_deleteSpeedControllers = true;
 }
@@ -91,34 +86,36 @@ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
  * @param leftMotor The left SpeedController object used to drive the robot.
  * @param rightMotor the right SpeedController object used to drive the robot.
  */
-RobotDrive::RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor)
-{
+RobotDrive::RobotDrive(SpeedController *leftMotor,
+                       SpeedController *rightMotor) {
 	InitRobotDrive();
-	if (leftMotor == nullptr || rightMotor == nullptr)
-	{
+	if (leftMotor == nullptr || rightMotor == nullptr) {
 		wpi_setWPIError(NullParameter);
 		m_rearLeftMotor = m_rearRightMotor = nullptr;
 		return;
 	}
-	m_rearLeftMotor = leftMotor;
-	m_rearRightMotor = rightMotor;
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
-	}
-	m_deleteSpeedControllers = false;
+	m_rearLeftMotor = make_shared_nodelete(leftMotor);
+	m_rearRightMotor = make_shared_nodelete(rightMotor);
 }
 
-RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor)
-{
+//TODO: Change to rvalue references & move syntax.
+RobotDrive::RobotDrive(SpeedController &leftMotor,
+                       SpeedController &rightMotor) {
 	InitRobotDrive();
-	m_rearLeftMotor = &leftMotor;
-	m_rearRightMotor = &rightMotor;
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
+	m_rearLeftMotor = make_shared_nodelete(&leftMotor);
+	m_rearRightMotor = make_shared_nodelete(&rightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
+                       std::shared_ptr<SpeedController> rightMotor) {
+	InitRobotDrive();
+	if (leftMotor == nullptr || rightMotor == nullptr) {
+		wpi_setWPIError(NullParameter);
+		m_rearLeftMotor = m_rearRightMotor = nullptr;
+	return;
 	}
-	m_deleteSpeedControllers = false;
+	m_rearLeftMotor = leftMotor;
+	m_rearRightMotor = rightMotor;
 }
 
 /**
@@ -129,12 +126,40 @@ RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor)
  * @param rearRightMotor The back right SpeedController object used to drive the robot.
  * @param frontRightMotor The front right SpeedController object used to drive the robot.
  */
-RobotDrive::RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
-						SpeedController *frontRightMotor, SpeedController *rearRightMotor)
-{
+RobotDrive::RobotDrive(SpeedController *frontLeftMotor,
+                       SpeedController *rearLeftMotor,
+                       SpeedController *frontRightMotor,
+                       SpeedController *rearRightMotor) {
 	InitRobotDrive();
-	if (frontLeftMotor == nullptr || rearLeftMotor == nullptr || frontRightMotor == nullptr || rearRightMotor == nullptr)
-	{
+	if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+	  frontRightMotor == nullptr || rearRightMotor == nullptr) {
+		wpi_setWPIError(NullParameter);
+		return;
+	}
+	m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
+	m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
+	m_frontRightMotor = make_shared_nodelete(frontRightMotor);
+	m_rearRightMotor = make_shared_nodelete(rearRightMotor);
+}
+
+RobotDrive::RobotDrive(SpeedController &frontLeftMotor,
+                       SpeedController &rearLeftMotor,
+                       SpeedController &frontRightMotor,
+                       SpeedController &rearRightMotor) {
+	InitRobotDrive();
+	m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
+	m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
+	m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
+	m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
+                       std::shared_ptr<SpeedController> rearLeftMotor,
+                       std::shared_ptr<SpeedController> frontRightMotor,
+                       std::shared_ptr<SpeedController> rearRightMotor) {
+	InitRobotDrive();
+	if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+	  frontRightMotor == nullptr || rearRightMotor == nullptr) {
 		wpi_setWPIError(NullParameter);
 		return;
 	}
@@ -142,42 +167,6 @@ RobotDrive::RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLef
 	m_rearLeftMotor = rearLeftMotor;
 	m_frontRightMotor = frontRightMotor;
 	m_rearRightMotor = rearRightMotor;
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
-	}
-	m_deleteSpeedControllers = false;
-}
-
-RobotDrive::RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
-						SpeedController &frontRightMotor, SpeedController &rearRightMotor)
-{
-	InitRobotDrive();
-	m_frontLeftMotor = &frontLeftMotor;
-	m_rearLeftMotor = &rearLeftMotor;
-	m_frontRightMotor = &frontRightMotor;
-	m_rearRightMotor = &rearRightMotor;
-	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
-	{
-		m_invertedMotors[i] = 1;
-	}
-	m_deleteSpeedControllers = false;
-}
-
-/**
- * RobotDrive destructor.
- * Deletes motor objects that were not passed in and created internally only.
- **/
-RobotDrive::~RobotDrive()
-{
-	if (m_deleteSpeedControllers)
-	{
-		delete m_frontLeftMotor;
-		delete m_rearLeftMotor;
-		delete m_frontRightMotor;
-		delete m_rearRightMotor;
-	}
-	// FIXME: delete m_safetyHelper;
 }
 
 /**

wpilibj/simulation.gradle

@@ -42,6 +42,7 @@ task wpilibjSimJavadoc(type: Javadoc) {
     description = 'Generates javadoc for the simulation components'
     group = 'WPILib'
     source sourceSets.sim.allJava, sourceSets.shared.allJava
+    destinationDir = file("$buildDir/docs/java-simulation")
     classpath = files([sourceSets.sim.compileClasspath, sourceSets.shared.compileClasspath])
 }
 
@@ -52,27 +53,8 @@ task wpilibjSimJavadocJar(type: Jar, dependsOn: wpilibjSimJavadoc) {
     from wpilibjSimJavadoc.destinationDir
 }
 
-// Maven publishing configuration
-publishing {
-    publications {
-        wpilibjSim(MavenPublication) {
-            artifact wpilibjSimJar
-            artifact(wpilibjSimSources) {
-                classifier = "sources"
-            }
-            artifact(wpilibjSimJavadocJar) {
-                classifier "javadoc"
-            }
-
-            groupId 'edu.wpi.first.wpilibj'
-            artifactId 'wpilibJavaSim'
-            version '0.1.0-SNAPSHOT'
-        }
-    }
-}
-
 //we need to move the simulation jars to the install directory
-task copyJars(type: Copy) {
+task copyJars(type: Copy, dependsOn: wpilibjSimJar) {
     description = 'copy wpilibj simulation jar to make simulation zip'
     group = 'WPILib Simulation'
     from wpilibjSimJar.archivePath

wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -223,7 +223,7 @@ public abstract class RobotBase {
 
       output = new FileOutputStream(file);
 
-      output.write("2016 Java Beta5.0".getBytes());
+      output.write("2016 Java Release 3".getBytes());
 
     } catch (IOException ex) {
       ex.printStackTrace();
@@ -238,6 +238,7 @@ public abstract class RobotBase {
 
     boolean errorOnExit = false;
     try {
+      System.out.println("********** Robot program starting **********");
       robot.startCompetition();
     } catch (Throwable t) {
       DriverStation.reportError(