Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add format script which invokes clang-format on the C++ source code (#41)

Browse Source 
On Windows machines, clang-format.exe must be in the PATH environment variable.
This commit is contained in:
Tyler Veness
2016-05-20 17:30:37 -07:00
committed by Peter Johnson
parent 68690643d2
commit e14e45da76
383 changed files with 13787 additions and 13198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
simulation/frc_gazebo_plugins/servo/src/servo.cpp
View File

@@ -6,9 +6,9 @@
/*----------------------------------------------------------------------------*/
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
#include "servo.h"
@@ -22,56 +22,57 @@ Servo::Servo() {}
Servo::~Servo() {}
void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf){
this->model = model;
signal = 0;
void Servo::Load(physics::ModelPtr model, sdf::ElementPtr sdf) {
this->model = model;
signal = 0;
//parse SDF Properries
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
}
else {
topic = "~/"+sdf->GetAttribute("name")->GetAsString();
}
if (sdf->HasElement("torque")) {
torque = sdf->Get<double>("torque");
}
else {
torque = 5;
}
gzmsg << "initializing awesome servo: " << topic
<< " joint=" << joint->GetName()
<< " torque=" << torque << std::endl;
//Connect to Gazebo transport for messaging
std::string scoped_name = model->GetWorld()->GetName()+"::"+model->GetScopedName();
boost::replace_all(scoped_name, "::","/");
node = transport::NodePtr(new transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &Servo::Callback, this);
//connect to the world update event
//this will call update every iteration
updateConn = event::Events::ConnectWorldUpdateBegin(boost::bind(&Servo::Update, this, _1));
}
void Servo::Update(const common::UpdateInfo &info){
//torque is in kg*cm
//joint->SetAngle(0,signal*180);
if (joint->GetAngle(0) < signal){
joint->SetForce(0,torque);
// parse SDF Properries
joint = model->GetJoint(sdf->Get<std::string>("joint"));
if (sdf->HasElement("topic")) {
topic = sdf->Get<std::string>("topic");
} else {
topic = "~/" + sdf->GetAttribute("name")->GetAsString();
}
else if (joint->GetAngle(0) > signal){
joint->SetForce(0,torque);
if (sdf->HasElement("torque")) {
torque = sdf->Get<double>("torque");
} else {
torque = 5;
}
joint->SetForce(0,0);
gzmsg << "initializing awesome servo: " << topic
<< " joint=" << joint->GetName() << " torque=" << torque << std::endl;
// Connect to Gazebo transport for messaging
std::string scoped_name =
model->GetWorld()->GetName() + "::" + model->GetScopedName();
boost::replace_all(scoped_name, "::", "/");
node = transport::NodePtr(new transport::Node());
node->Init(scoped_name);
sub = node->Subscribe(topic, &Servo::Callback, this);
// connect to the world update event
// this will call update every iteration
updateConn = event::Events::ConnectWorldUpdateBegin(
boost::bind(&Servo::Update, this, _1));
}
void Servo::Callback(const msgs::ConstFloat64Ptr &msg){
signal = msg->data();
if (signal < -1) { signal = -1; }
else if (signal > 1) { signal = 1; }
void Servo::Update(const common::UpdateInfo& info) {
// torque is in kg*cm
// joint->SetAngle(0,signal*180);
if (joint->GetAngle(0) < signal) {
joint->SetForce(0, torque);
} else if (joint->GetAngle(0) > signal) {
joint->SetForce(0, torque);
}
joint->SetForce(0, 0);
}
void Servo::Callback(const msgs::ConstFloat64Ptr& msg) {
signal = msg->data();
if (signal < -1) {
signal = -1;
} else if (signal > 1) {
signal = 1;
}
}