Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed FRCSim artf2619, and misc reformatting.

Browse Source 
Change-Id: I7133f46f88f7e2cb2451c2a6714daa8f3f368b40
This commit is contained in:
Colby Skeggs
2014-07-22 13:10:23 -07:00
parent 461e359484
commit 2dd45c3ea6
5 changed files with 86 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
wpilibc/wpilibC++Sim/src/simulation/SimEncoder.cpp
View File

@@ -3,50 +3,52 @@
#include "simulation/MainNode.h"
SimEncoder::SimEncoder(std::string topic) {
commandPub = MainNode::Advertise<msgs::GzString>("~/simulator/"+topic+"/control");
posSub = MainNode::Subscribe("~/simulator/"+topic+"/position",
&SimEncoder::positionCallback, this);
velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity",
&SimEncoder::velocityCallback, this);
commandPub = MainNode::Advertise<msgs::GzString>("~/simulator/"+topic+"/control");
commandPub->WaitForConnection();
std::cout << "Initialized ~/simulator/"+topic << std::endl;
posSub = MainNode::Subscribe("~/simulator/"+topic+"/position",
&SimEncoder::positionCallback, this);
velSub = MainNode::Subscribe("~/simulator/"+topic+"/velocity",
&SimEncoder::velocityCallback, this);
if (commandPub->WaitForConnection(gazebo::common::Time(5.0))) { // Wait up to five seconds.
std::cout << "Initialized ~/simulator/" + topic << std::endl;
} else {
std::cerr << "Failed to initialize ~/simulator/" + topic + ": does the encoder exist?" << std::endl;
}
}
void SimEncoder::Reset() {
sendCommand("reset");
sendCommand("reset");
}
void SimEncoder::Start() {
sendCommand("start");
sendCommand("start");
}
void SimEncoder::Stop() {
sendCommand("stop");
sendCommand("stop");
}
double SimEncoder::GetPosition() {
return position;
return position;
}
double SimEncoder::GetVelocity() {
return velocity;
return velocity;
}
void SimEncoder::sendCommand(std::string cmd) {
msgs::GzString msg;
msg.set_data(cmd);
commandPub->Publish(msg);
msgs::GzString msg;
msg.set_data(cmd);
commandPub->Publish(msg);
}
void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr &msg) {
position = msg->data();
position = msg->data();
}
void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr &msg) {
velocity = msg->data();
velocity = msg->data();
}