Added C++ support for solenoids.

Change-Id: If82c05196d2f4c09d148da64f6bdb0564fe5b4cc

wpilibc/wpilibC++Sim/include/DoubleSolenoid.h

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "simulation/SimContinuousOutput.h"
+#include "LiveWindow/LiveWindowSendable.h"
+#include "tables/ITableListener.h"
+
+/**
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output
+ * (9472 module).
+ * 
+ * The DoubleSolenoid class is typically used for pneumatics solenoids that
+ * have two positions controlled by two separate channels.
+ */
+class DoubleSolenoid : public LiveWindowSendable, public ITableListener
+{
+public:
+	enum Value
+	{
+		kOff,
+		kForward,
+		kReverse
+	};
+
+	explicit DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel);
+	DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel);
+	virtual ~DoubleSolenoid();
+	virtual void Set(Value value);
+	virtual Value Get();
+
+	void ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew);
+	void UpdateTable();
+	void StartLiveWindowMode();
+	void StopLiveWindowMode();
+	std::string GetSmartDashboardType();
+	void InitTable(ITable *subTable);
+	ITable * GetTable();
+
+private:
+    void InitSolenoid(int slot, int channel1, int channel2);
+    SimContinuousOutput* m_impl;
+    Value m_value;
+    bool m_reversed;
+
+	ITable *m_table;
+};

wpilibc/wpilibC++Sim/include/Jaguar.h

@@ -7,7 +7,7 @@
 #ifndef JAGUAR_H
 #define JAGUAR_H
 
-#include "simulation/SimSpeedController.h"
+#include "simulation/SimContinuousOutput.h"
 #include "SpeedController.h"
 #include "PIDOutput.h"
 
@@ -28,7 +28,7 @@ public:
 
 private:
 	void InitJaguar(int slot, int channel);
-    SimSpeedController* impl;
+    SimContinuousOutput* impl;
 };
 
 #endif

wpilibc/wpilibC++Sim/include/Solenoid.h

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "simulation/SimContinuousOutput.h"
+#include "LiveWindow/LiveWindowSendable.h"
+#include "tables/ITableListener.h"
+
+/**
+ * Solenoid class for running high voltage Digital Output (9472 module).
+ * 
+ * The Solenoid class is typically used for pneumatics solenoids, but could be used
+ * for any device within the current spec of the 9472 module.
+ */
+class Solenoid : public LiveWindowSendable, public ITableListener
+{
+public:
+	explicit Solenoid(uint32_t channel);
+	Solenoid(uint8_t moduleNumber, uint32_t channel);
+	virtual ~Solenoid();
+	virtual void Set(bool on);
+	virtual bool Get();
+
+	void ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew);
+	void UpdateTable();
+	void StartLiveWindowMode();
+	void StopLiveWindowMode();
+	std::string GetSmartDashboardType();
+	void InitTable(ITable *subTable);
+	ITable * GetTable();
+
+private:
+	void InitSolenoid(int slot, int channel);
+    SimContinuousOutput* m_impl;
+    bool m_on; 
+
+	ITable *m_table;
+};

wpilibc/wpilibC++Sim/include/Talon.h

@@ -7,7 +7,7 @@
 #ifndef Talon_H
 #define Talon_H
 
-#include "simulation/SimSpeedController.h"
+#include "simulation/SimContinuousOutput.h"
 #include "SpeedController.h"
 #include "PIDOutput.h"
 
@@ -28,7 +28,7 @@ public:
 
 private:
 	void InitTalon(int slot, int channel);
-    SimSpeedController* impl;
+    SimContinuousOutput* impl;
 };
 
 #endif

wpilibc/wpilibC++Sim/include/Victor.h

@@ -7,7 +7,7 @@
 #ifndef VICTOR_H
 #define VICTOR_H
 
-#include "simulation/SimSpeedController.h"
+#include "simulation/SimContinuousOutput.h"
 #include "SpeedController.h"
 #include "PIDOutput.h"
 
@@ -28,7 +28,7 @@ public:
 
 private:
 	void InitVictor(int slot, int channel);
-    SimSpeedController* impl;
+    SimContinuousOutput* impl;
 };
 
 #endif

wpilibc/wpilibC++Sim/include/WPILib.h

@@ -10,6 +10,27 @@
 
 #define SIMULATION "gazebo"
 
+#include "Buttons/Trigger.h"
+#include "Buttons/Button.h"
+#include "Buttons/InternalButton.h"
+#include "Buttons/JoystickButton.h"
+#include "Buttons/NetworkButton.h"
+
+#include "Commands/Command.h"
+#include "Commands/CommandGroup.h"
+#include "Commands/PIDCommand.h"
+#include "Commands/PIDSubsystem.h"
+#include "Commands/PrintCommand.h"
+#include "Commands/Scheduler.h"
+#include "Commands/StartCommand.h"
+#include "Commands/Subsystem.h"
+#include "Commands/WaitCommand.h"
+#include "Commands/WaitForChildren.h"
+#include "Commands/WaitUntilCommand.h"
+
+#include "SmartDashboard/SendableChooser.h"
+#include "SmartDashboard/SmartDashboard.h"
+
 #include "RobotBase.h"
 #include "SimpleRobot.h"
 #include "IterativeRobot.h"
@@ -17,6 +38,8 @@
 #include "Talon.h"
 #include "Victor.h"
 #include "Jaguar.h"
+#include "Solenoid.h"
+#include "DoubleSolenoid.h"
 #include "interfaces/Potentiometer.h"
 #include "AnalogChannel.h"
 #include "AnalogPotentiometer.h"
@@ -28,13 +51,6 @@
 #include "Joystick.h"
 #include "PIDController.h"
 #include "RobotDrive.h"
-#include "SmartDashboard/SmartDashboard.h"
 #include "LiveWindow/LiveWindow.h"
 
-#include "Buttons/Trigger.h"
-#include "Buttons/Button.h"
-#include "Buttons/InternalButton.h"
-#include "Buttons/JoystickButton.h"
-#include "Buttons/NetworkButton.h"
-
 #endif /* WPILIB_H_ */

wpilibc/wpilibC++Sim/include/simulation/SimContinuousOutput.h

@@ -0,0 +1,35 @@
+
+
+#ifndef _SIM_SPEED_CONTROLLER_H
+#define _SIM_SPEED_CONTROLLER_H
+
+#include <gazebo/transport/transport.hh>
+#include "SpeedController.h"
+
+using namespace gazebo;
+
+class SimContinuousOutput {
+private:
+	transport::PublisherPtr pub;
+	float speed;
+
+public:
+	SimContinuousOutput(std::string topic);
+
+	/**
+	 * Set the output value.
+	 *
+	 * The value is set using a range of -1.0 to 1.0, appropriately
+	 * scaling the value.
+	 *
+	 * @param value The value between -1.0 and 1.0 to set.
+	 */
+	void Set(float value);
+
+	/**
+	 * @return The most recently set value.
+	 */
+	float Get();
+};
+
+#endif

wpilibc/wpilibC++Sim/include/simulation/SimSpeedController.h

@@ -1,67 +0,0 @@
-
-
-#ifndef _SIM_SPEED_CONTROLLER_H
-#define _SIM_SPEED_CONTROLLER_H
-
-#include <gazebo/transport/transport.hh>
-#include "SpeedController.h"
-
-using namespace gazebo;
-
-class SimSpeedController : public SpeedController {
-private:
-	transport::PublisherPtr pub;
-	float speed;
-
-public:
-	/**
-	 * Constructor that assumes the default digital module.
-	 *
-	 * @param channel The PWM channel on the digital module that the Victor is attached to.
-	 */
-	SimSpeedController(std::string topic);
-
-	/**
-	 * Set the PWM value.
-	 *
-	 * @deprecated For compatibility with CANJaguar
-	 *
-	 * The PWM value is set using a range of -1.0 to 1.0, appropriately
-	 * scaling the value for the FPGA.
-	 *
-	 * @param speed The speed to set.  Value should be between -1.0 and 1.0.
-	 * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup().  If 0, update immediately.
-	 */
-	void Set(float speed, uint8_t syncGroup);
-
-	/**
-	 * Set the PWM value.
-	 *
-	 * The PWM value is set using a range of -1.0 to 1.0, appropriately
-	 * scaling the value for the FPGA.
-	 *
-	 * @param speed The speed value between -1.0 and 1.0 to set.
-	 */
-	void Set(float speed);
-
-	/**
-	 * Get the recently set value of the PWM.
-	 *
-	 * @return The most recently set value for the PWM between -1.0 and 1.0.
-	 */
-	float Get();
-
-	/**
-	 * Disable the speed controller
-	 */
-	void Disable();
-
-	/**
-	 * Write out the PID value as seen in the PIDOutput base object.
-	 *
-	 * @param output Write out the PWM value as was found in the PIDController
-	 */
-	void PIDWrite(float output);
-};
-
-#endif

wpilibc/wpilibC++Sim/src/DoubleSolenoid.cpp

@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "DoubleSolenoid.h"
+#include "WPIErrors.h"
+#include <string.h>
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Common function to implement constructor behavior.
+ */
+void DoubleSolenoid::InitSolenoid(int slot, int forwardChannel, int reverseChannel)
+{
+    m_reversed = false;
+    if (reverseChannel < forwardChannel) { // Swap ports to get the right address
+        int channel = reverseChannel;
+        reverseChannel = forwardChannel;
+        forwardChannel = channel;
+        m_reversed = true;
+    }
+    char buffer[50];
+    int n = sprintf(buffer, "pneumatic/%d/%d/%d/%d", slot, forwardChannel,
+                    slot, reverseChannel);
+    m_impl = new SimContinuousOutput(buffer);  
+  
+	LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", slot, forwardChannel, this);
+}
+
+/**
+ * Constructor.
+ *
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t reverseChannel)
+{
+    InitSolenoid(1, forwardChannel, reverseChannel);
+}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The solenoid module (1 or 2).
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel)
+{
+    InitSolenoid(moduleNumber, forwardChannel, reverseChannel);
+}
+
+/**
+ * Destructor.
+ */
+DoubleSolenoid::~DoubleSolenoid()
+{
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param value Move the solenoid to forward, reverse, or don't move it.
+ */
+void DoubleSolenoid::Set(Value value)
+{
+    m_value = value;
+	switch(value)
+	{
+	case kOff:
+        m_impl->Set(0);
+        break;
+	case kForward:
+        m_impl->Set(m_reversed ? -1 : 1);
+		break;
+	case kReverse:
+        m_impl->Set(m_reversed ? 1 : -1);
+		break;
+	}
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+DoubleSolenoid::Value DoubleSolenoid::Get()
+{
+	return m_value;
+}
+
+void DoubleSolenoid::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
+	Value lvalue = kOff;
+	std::string *val = (std::string *)value.ptr;
+	if (*val == "Forward")
+		lvalue = kForward;
+	else if (*val == "Reverse")
+		lvalue = kReverse;
+	Set(lvalue);
+}
+
+void DoubleSolenoid::UpdateTable() {
+	if (m_table != NULL) {
+		m_table->PutString("Value", (Get() == kForward ? "Forward" : (Get() == kReverse ? "Reverse" : "Off")));
+	}
+}
+
+void DoubleSolenoid::StartLiveWindowMode() {
+	Set(kOff);
+	if (m_table != NULL) {
+		m_table->AddTableListener("Value", this, true);
+	}
+}
+
+void DoubleSolenoid::StopLiveWindowMode() {
+	Set(kOff);
+	if (m_table != NULL) {
+		m_table->RemoveTableListener(this);
+	}
+}
+
+std::string DoubleSolenoid::GetSmartDashboardType() {
+	return "Double Solenoid";
+}
+
+void DoubleSolenoid::InitTable(ITable *subTable) {
+	m_table = subTable;
+	UpdateTable();
+}
+
+ITable * DoubleSolenoid::GetTable() {
+	return m_table;
+}

wpilibc/wpilibC++Sim/src/Jaguar.cpp

@@ -24,7 +24,7 @@ void Jaguar::InitJaguar(int slot, int channel)
 	 */
     char buffer[50];
     int n = sprintf(buffer, "pwm/%d/%d", slot, channel);
-    impl = new SimSpeedController(buffer);
+    impl = new SimContinuousOutput(buffer);
 
 	// TODO: LiveWindow::GetInstance()->AddActuator("Jaguar", GetModuleNumber(), GetChannel(), this);
 }
@@ -83,7 +83,7 @@ float Jaguar::Get()
  */
 void Jaguar::Disable()
 {
-    impl->Disable();
+    impl->Set(0);
 }
 
 /**

wpilibc/wpilibC++Sim/src/Solenoid.cpp

@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Solenoid.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Common function to implement constructor behavior.
+ */
+void Solenoid::InitSolenoid(int slot, int channel)
+{
+    char buffer[50];
+    int n = sprintf(buffer, "pneumatic/%d/%d", slot, channel);
+    m_impl = new SimContinuousOutput(buffer);
+  
+	LiveWindow::GetInstance()->AddActuator("Solenoid", slot, channel, this);
+}
+
+/**
+ * Constructor.
+ *
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(uint32_t channel)
+{
+    InitSolenoid(1, channel);
+}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The solenoid module (1 or 2).
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel)
+{
+    InitSolenoid(moduleNumber, channel);
+}
+
+/**
+ * Destructor.
+ */
+Solenoid::~Solenoid()
+{
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param on Turn the solenoid output off or on.
+ */
+void Solenoid::Set(bool on)
+{
+    m_on = on;
+    m_impl->Set(on ? 1 : -1);
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+bool Solenoid::Get()
+{
+    return m_on;
+}
+
+
+void Solenoid::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
+	Set(value.b);
+}
+
+void Solenoid::UpdateTable() {
+	if (m_table != NULL) {
+		m_table->PutBoolean("Value", Get());
+	}
+}
+
+void Solenoid::StartLiveWindowMode() {
+	Set(false);
+	if (m_table != NULL) {
+		m_table->AddTableListener("Value", this, true);
+	}
+}
+
+void Solenoid::StopLiveWindowMode() {
+	Set(false);
+	if (m_table != NULL) {
+		m_table->RemoveTableListener(this);
+	}
+}
+
+std::string Solenoid::GetSmartDashboardType() {
+	return "Solenoid";
+}
+
+void Solenoid::InitTable(ITable *subTable) {
+	m_table = subTable;
+	UpdateTable();
+}
+
+ITable * Solenoid::GetTable() {
+	return m_table;
+}

wpilibc/wpilibC++Sim/src/Talon.cpp

@@ -25,7 +25,7 @@
 void Talon::InitTalon(int slot, int channel) {
     char buffer[50];
     int n = sprintf(buffer, "pwm/%d/%d", slot, channel);
-    impl = new SimSpeedController(buffer);
+    impl = new SimContinuousOutput(buffer);
 
 	// TODO: LiveWindow::GetInstance()->AddActuator("Talon", slot, channel, this);
 }
@@ -84,7 +84,7 @@ float Talon::Get()
  */
 void Talon::Disable()
 {
-    impl->Disable();
+    impl->Set(0);
 }
 
 /**

wpilibc/wpilibC++Sim/src/Victor.cpp

@@ -26,7 +26,7 @@
 void Victor::InitVictor(int slot, int channel) {
     char buffer[50];
     int n = sprintf(buffer, "pwm/%d/%d", slot, channel);
-    impl = new SimSpeedController(buffer);
+    impl = new SimContinuousOutput(buffer);
     
 	// TODO: LiveWindow::GetInstance()->AddActuator("Victor", slot, channel, this);
 }
@@ -85,7 +85,7 @@ float Victor::Get()
  */
 void Victor::Disable()
 {
-    impl->Disable();
+    impl->Set(0);
 }
 
 /**

wpilibc/wpilibC++Sim/src/simulation/SimContinuousOutput.cpp

@@ -0,0 +1,24 @@
+/*
+ * SimContinuousOutput.cpp
+ *
+ *  Created on: May 28, 2014
+ *      Author: alex
+ */
+
+#include "simulation/SimContinuousOutput.h"
+#include "simulation/MainNode.h"
+
+SimContinuousOutput::SimContinuousOutput(std::string topic) {
+    pub = MainNode::Advertise<msgs::Float64>("~/simulator/"+topic);
+	std::cout << "Initialized ~/simulator/"+topic << std::endl;
+}
+
+void SimContinuousOutput::Set(float speed) {
+	msgs::Float64 msg;
+	msg.set_data(speed);
+	pub->Publish(msg);
+}
+
+float SimContinuousOutput::Get() {
+	return speed;
+}

wpilibc/wpilibC++Sim/src/simulation/SimFloatInput.cpp

@@ -1,5 +1,5 @@
 /*
- * SimSpeedController.cpp
+ * SimFloatInput.cpp
  *
  *  Created on: May 28, 2014
  *      Author: alex

wpilibc/wpilibC++Sim/src/simulation/SimSpeedController.cpp

@@ -1,39 +0,0 @@
-/*
- * SimSpeedController.cpp
- *
- *  Created on: May 28, 2014
- *      Author: alex
- */
-
-#include "simulation/SimSpeedController.h"
-#include "simulation/MainNode.h"
-
-SimSpeedController::SimSpeedController(std::string topic) {
-    pub = MainNode::Advertise<msgs::Float64>("~/simulator/"+topic);
-	std::cout << "Initialized ~/simulator/"+topic << std::endl;
-}
-
-void SimSpeedController::Set(float speed, uint8_t syncGroup) {
-	Set(speed);
-}
-
-void SimSpeedController::Set(float speed) {
-	msgs::Float64 msg;
-	msg.set_data(speed);
-	pub->Publish(msg);
-}
-
-float SimSpeedController::Get() {
-	return speed;
-}
-
-void SimSpeedController::Disable() {
-	Set(0);
-}
-
-void SimSpeedController::PIDWrite(float output) {
-	Set(output);
-}
-
-
-