CANTalon throttle works.

Fleshed out CANTalon interface; it currently works just to write a
throttle. The firmware doesn't fully support everything yet, so we are
still significantly limited.

Change-Id: I2868c4c168a8cb42cda754589777beef31ffd354

test-scripts/config.sh

@@ -21,7 +21,7 @@ DEFAULT_DESTINATION_TEST_RESULTS_DIR=${DEFAULT_DESTINATION_DIR}/testResults
 # C++ test variables
 DEFAULT_CPP_TEST_NAME=FRCUserProgram
 DEFAULT_CPP_TEST_ARGS="--gtest_color=yes"
-DEFAULT_LOCAL_CPP_TEST_FILE=../cmake/target/cmake/wpilibc/wpilibC++IntegrationTests/FRCUserProgram
+DEFAULT_LOCAL_CPP_TEST_FILE=../build/wpilibc/wpilibC++IntegrationTests/FRCUserProgram
 CPP_REPORT=cppreport.xml
 DEFAULT_LOCAL_CPP_TEST_RESULT=${DEFAULT_LOCAL_TEST_RESULTS_DIR}/${CPP_REPORT}
 DEFAULT_DESTINATION_CPP_TEST_RESULTS=${DEFAULT_DESTINATION_TEST_RESULTS_DIR}/${CPP_REPORT}

wpilibc/wpilibC++Devices/include/CANSpeedController.h

@@ -16,11 +16,14 @@ class CANSpeedController : public SpeedController
 {
 public:
 	enum ControlMode {
-		kPercentVbus,
-		kCurrent,
-		kSpeed,
-		kPosition,
-		kVoltage
+		kPercentVbus=0,
+    // TODO(james): Handle kFollower in CANJaguar.
+    kFollower=1, // Only supported on Talon SRX.
+		kVoltage=2,
+		kPosition=3,
+		kSpeed=4,
+		kCurrent=5,
+    kDisabled=15
 	};
 
 	enum Faults {

wpilibc/wpilibC++Devices/include/CANTalon.h

@@ -39,6 +39,7 @@ public:
 	virtual float Get() override;
 	virtual void Set(float value, uint8_t syncGroup=0) override;
 	virtual void Disable() override;
+  virtual void EnableControl();
 	virtual void SetP(double p) override;
 	virtual void SetI(double i) override;
 	virtual void SetD(double d) override;
@@ -74,4 +75,7 @@ private:
 	uint8_t m_deviceNumber;
 	CanTalonSRX *m_impl;
 	MotorSafetyHelper *m_safetyHelper;
+
+  bool m_controlEnabled;
+  ControlMode m_controlMode;
 };

wpilibc/wpilibC++Devices/src/CANJaguar.cpp

@@ -343,6 +343,7 @@ void CANJaguar::Set(float outputValue, uint8_t syncGroup)
 			}
 			break;
 		default:
+		  wpi_setWPIErrorWithContext(IncompatibleMode, "The Jaguar only supports Current, Voltage, Position, Speed, and Percent (throttle) modes.");
 			return;
 		}
 		if (syncGroup != 0)
@@ -1158,6 +1159,9 @@ void CANJaguar::SetP(double p)
 		dataSize = packFXP16_16(dataBuffer, p);
 		sendMessage(LM_API_ICTRL_PC, dataBuffer, dataSize);
 		break;
+  default:
+		wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode");
+    break;
 	}
 
 	m_p = p;
@@ -1192,6 +1196,9 @@ void CANJaguar::SetI(double i)
 		dataSize = packFXP16_16(dataBuffer, i);
 		sendMessage(LM_API_ICTRL_IC, dataBuffer, dataSize);
 		break;
+  default:
+		wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode");
+    break;
 	}
 
 	m_i = i;
@@ -1226,6 +1233,9 @@ void CANJaguar::SetD(double d)
 		dataSize = packFXP16_16(dataBuffer, d);
 		sendMessage(LM_API_ICTRL_DC, dataBuffer, dataSize);
 		break;
+  default:
+		wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode");
+    return;
 	}
 
 	m_d = d;
@@ -1313,6 +1323,9 @@ void CANJaguar::EnableControl(double encoderInitialPosition)
 	case kVoltage:
 		sendMessage(LM_API_VCOMP_T_EN, dataBuffer, dataSize);
 		break;
+  default:
+		wpi_setWPIErrorWithContext(IncompatibleMode, "The Jaguar only supports Current, Voltage, Position, Speed, and Percent (throttle) modes.");
+    break;
 	}
 
 	m_controlEnabled = true;

wpilibc/wpilibC++Devices/src/CANTalon.cpp

@@ -8,6 +8,12 @@
 #include "WPIErrors.h"
 #include "ctre/CanTalonSRX.h"
 
+/**
+ * The CANTalon object is currently incomplete. As of Nov 14 2014, we only know
+ * for sure that sending a throttle and checking basic values (eg current,
+ * temperature) work.
+ */
+
 /**
  * Constructor for the CANTalon device.
  * @param deviceNumber The CAN ID of the Talon SRX
@@ -16,6 +22,8 @@ CANTalon::CANTalon(uint8_t deviceNumber)
 	: m_deviceNumber(deviceNumber)
 	, m_impl(new CanTalonSRX(deviceNumber))
 	, m_safetyHelper(new MotorSafetyHelper(this))
+  , m_controlEnabled(false)
+  , m_controlMode(kPercentVbus)
 {
 }
 
@@ -48,16 +56,43 @@ void CANTalon::PIDWrite(float output)
  */
 float CANTalon::Get()
 {
-	// TODO
-	return 0.0f;
+  return 0.0f;
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * Sets the output set-point value.
  */
 void CANTalon::Set(float value, uint8_t syncGroup)
 {
-	// TODO
+  if(m_controlEnabled) {
+    CTR_Code status;
+    switch(GetControlMode()) {
+      case kPercentVbus:
+        {
+          m_impl->Set(value);
+          status = CTR_OKAY;
+        }
+        break;
+      case kFollower:
+        {
+          status = m_impl->SetDemand24((int)value);
+        }
+        break;
+      case kVoltage:
+        {
+          // Voltage is an 8.8 fixed point number.
+          int volts = int(value * 256);
+          status = m_impl->SetDemand24(volts);
+        }
+      default:
+        // TODO: Add support for other modes. Need to figure out what format
+        // SetDemand24 needs.
+        break;
+    }
+    if (status != CTR_OKAY) {
+      wpi_setErrorWithContext(status, getHALErrorMessage(status));
+    }
+  }
 }
 
 /**
@@ -65,7 +100,18 @@ void CANTalon::Set(float value, uint8_t syncGroup)
  */
 void CANTalon::Disable()
 {
-	// TODO
+  // Until Modes other than throttle work, just disable by setting throttle to 0.0.
+  m_impl->Set(0.0); // TODO when firmware is updated, remove this.
+  //m_impl->SetModeSelect(kDisabled); // TODO when firmware is updated, uncomment this.
+  m_controlEnabled = false;
+}
+
+/**
+ * TODO documentation (see CANJaguar.cpp)
+ */
+void CANTalon::EnableControl() {
+  SetControlMode(m_controlMode);
+  m_controlEnabled = true;
 }
 
 /**
@@ -130,12 +176,18 @@ double CANTalon::GetD()
 }
 
 /**
- * TODO documentation (see CANJaguar.cpp)
+ * Returns the voltage coming in from the battery.
+ *
+ * @return The input voltage in vols.
  */
 float CANTalon::GetBusVoltage()
 {
-	// TODO
-	return 0.0f;
+  double voltage;
+  CTR_Code status = m_impl->GetBatteryV(voltage);
+	if(status != CTR_OKAY) {
+		wpi_setErrorWithContext(status, getHALErrorMessage(status));
+	}
+  return voltage;
 }
 
 /**
@@ -143,17 +195,29 @@ float CANTalon::GetBusVoltage()
  */
 float CANTalon::GetOutputVoltage()
 {
-	// TODO
-	return 0.0f;
+  int throttle11;
+  CTR_Code status = m_impl->GetAppliedThrottle11(throttle11);
+  float voltage = GetBusVoltage() * float(throttle11) / 1023.0;
+	if(status != CTR_OKAY) {
+		wpi_setErrorWithContext(status, getHALErrorMessage(status));
+	}
+	return voltage;
 }
 
+
 /**
  * TODO documentation (see CANJaguar.cpp)
  */
 float CANTalon::GetOutputCurrent()
 {
-	// TODO
-	return 0.0f;
+  double current;
+
+  CTR_Code status = m_impl->GetCurrent(current);
+	if(status != CTR_OKAY) {
+		wpi_setErrorWithContext(status, getHALErrorMessage(status));
+	}
+
+	return current;
 }
 
 /**
@@ -161,26 +225,47 @@ float CANTalon::GetOutputCurrent()
  */
 float CANTalon::GetTemperature()
 {
-	// TODO
-	return 0.0f;
+  double temp;
+
+  CTR_Code status = m_impl->GetTemp(temp);
+	if(temp != CTR_OKAY) {
+		wpi_setErrorWithContext(status, getHALErrorMessage(status));
+	}
+	return temp;
 }
 
 /**
  * TODO documentation (see CANJaguar.cpp)
+ *
+ * @return The position of the sensor currently providing feedback.
  */
 double CANTalon::GetPosition()
 {
-	// TODO
-	return 0.0;
+  int postition;
+  // TODO convert from int to appropriate units (or at least document it).
+
+  CTR_Code status = m_impl->GetSensorPosition(postition);
+	if(status != CTR_OKAY) {
+		wpi_setErrorWithContext(status, getHALErrorMessage(status));
+	}
+	return (double)postition;
 }
 
 /**
  * TODO documentation (see CANJaguar.cpp)
+ *
+ * @returns The speed of the sensor currently providing feedback.
  */
 double CANTalon::GetSpeed()
 {
-	// TODO
-	return 0.0;
+  int speed;
+  // TODO convert from int to appropriate units (or at least document it).
+
+  CTR_Code status = m_impl->GetSensorVelocity(speed);
+	if(status != CTR_OKAY) {
+		wpi_setErrorWithContext(status, getHALErrorMessage(status));
+	}
+	return (double)speed;
 }
 
 /**
@@ -299,6 +384,7 @@ void CANTalon::ConfigReverseLimit(double reverseLimitPosition)
 
 /**
  * TODO documentation (see CANJaguar.cpp)
+ * Does this exist on the Talon?
  */
 void CANTalon::ConfigMaxOutputVoltage(double voltage)
 {
@@ -307,6 +393,7 @@ void CANTalon::ConfigMaxOutputVoltage(double voltage)
 
 /**
  * TODO documentation (see CANJaguar.cpp)
+ * Does this exist on the Talon?
  */
 void CANTalon::ConfigFaultTime(float faultTime)
 {
@@ -318,7 +405,11 @@ void CANTalon::ConfigFaultTime(float faultTime)
  */
 void CANTalon::SetControlMode(CANSpeedController::ControlMode mode)
 {
-	// TODO
+  m_controlMode = mode;
+  CTR_Code status = m_impl->SetModeSelect((int)mode);
+	if(status != CTR_OKAY) {
+		wpi_setErrorWithContext(status, getHALErrorMessage(status));
+	}
 }
 
 /**
@@ -326,8 +417,7 @@ void CANTalon::SetControlMode(CANSpeedController::ControlMode mode)
  */
 CANSpeedController::ControlMode CANTalon::GetControlMode()
 {
-	// TODO
-	return CANSpeedController::ControlMode::kPercentVbus;
+  return m_controlMode;
 }
 
 void CANTalon::SetExpiration(float timeout)
@@ -361,10 +451,12 @@ void CANTalon::GetDescription(char *desc)
 }
 
 /**
-* Common interface for stopping the motor
-* Part of the MotorSafety interface
+ * Common interface for stopping the motor
+ * Part of the MotorSafety interface
+ *
+ * @deprecated Call Disable instead.
 */
 void CANTalon::StopMotor()
 {
-	// TODO
+  Disable();
 }

wpilibc/wpilibC++IntegrationTests/src/CANTalonTest.cpp

@@ -9,10 +9,13 @@
 #include "TestBench.h"
 
 TEST(CANTalonTest, QuickTest) {
-	CANTalon talon(10);
+	CANTalon talon(0);
+  talon.SetControlMode(CANTalon::kPercentVbus);
+  talon.EnableControl();
+  talon.Set(1.0);
+  Wait(0.25);
+  EXPECT_GT(talon.GetOutputCurrent(), 4.0);
 
-	for(;;) {
-		std::cout << "Firmware version: " << talon.GetFirmwareVersion() << std::endl;
-		Wait(0.25);
-	}
+  talon.Set(0.0);
+  talon.Disable();
 }