int main( int argc, char **argv )
{
//ROS initialization and publisher/subscriber setup
ros::init( argc, argv, "GPIO_Driver" );
ros::NodeHandle nh("~");
std::string hw_id;
// Get parameters from .launch file or parameter server, or take defaults
nh.param<std::string>( "hardware_id", hw_id, "/dev/ttyACM0" );
NewArduinoInterface Arduino( hw_id );
GpioDriver* gpio_input = new GpioDriver( &Arduino, INPUT_PIN );
GpioDriver* gpio_output = new GpioDriver( &Arduino, OUTPUT_PIN );
if( gpio_input->initialize() == false || gpio_output->initialize() == false )
{
ROS_ERROR("Error initializing GPIO driver");
return -1;
}
ros::Rate loop_rate_Hz(10);
loop_rate_Hz.sleep();
bool value = 1;
bool value2 = 0;
while( nh.ok() )
{
if( !gpio_output->set( value ) )
{
return -1;
}
value = !value;
value2 = gpio_input->get( bosch_drivers_common::FLOATING );
ROS_INFO("Read value %i", value2);
ros::spinOnce();
loop_rate_Hz.sleep();
}
ROS_WARN( "Closing GPIO driver." );
return 0;
}
int main( int argc, char **argv )
{
//ROS initialization and publisher/subscriber setup
ros::init( argc, argv, "faulhaber_motor" );
ros::NodeHandle nh("~");
RaspiInterface* my_Raspi = new RaspiInterface();
MotionControlSystemsDriver* my_motor = new MotionControlSystemsDriver( my_Raspi, SERIAL_DEVICE, 9600 );
if( my_motor->initialize() == false )
{
ROS_ERROR("Error initializing Motor driver");
return -1;
}
ros::Rate loop_rate_Hz(10);
loop_rate_Hz.sleep();
my_motor->enable();
return 0;
}
int main( int argc, char **argv )
{
//ROS initialization and publisher/subscriber setup
ros::init( argc, argv, "Encoder_Driver" );
ros::NodeHandle nh("~");
std::string hw_id;
// Get parameters from .launch file or parameter server, or take defaults
nh.param<std::string>( "hardware_id", hw_id, "/dev/ttyACM0" );
ROS_INFO("Initializing Arduino");
NewArduinoInterface Arduino( hw_id );
Arduino.initialize();
ROS_INFO("Creating fast Encoder");
// create new encoder driver using pins 3 and 4
EncoderDriver* encoder_driver = new EncoderDriver( &Arduino , 4, 3 );
encoder_driver->invertOutput();
ROS_INFO("Initializing Encoder");
if( encoder_driver->initialize() == false)
{
ROS_ERROR("Error initializing encoder driver");
return -1;
}
ROS_INFO("Creating slow Encoder");
// create new encoder driver using pins 5 and 6
EncoderDriver* encoder_driver2 = new EncoderDriver( &Arduino , 5, 6 );
ROS_INFO("Initializing slow Encoder");
if( encoder_driver2->initialize() == false)
{
ROS_ERROR("Error initializing encoder driver");
return -1;
}
ros::Rate loop_rate_Hz(1);
int64_t position, position2;
ROS_INFO("Setting initial position");
encoder_driver->setPosition(0);
encoder_driver2->setPosition(0);
ROS_INFO("Start reading current position every second...");
while( nh.ok() )
{
position = encoder_driver->getPosition();
position2 = encoder_driver2->getPosition();
ROS_INFO("Position1: %li Position2: %li", position, position2);
ros::spinOnce();
loop_rate_Hz.sleep();
}
ROS_WARN( "Closing encoder driver." );
return 0;
}
/**
* This class is an example node that publishes the data from several
* BMA180s. In this case, the BMA180s are all connected on the same
* hardware interface, a sub20. However, multiple hardware interfaces
* can be instantiated. In this way, the user can create a node that
* publishes all BMA180 data regardless of the hardware interface that
* it is connected to.
**/
int main( int argc, char **argv )
{
//ROS initialization and publisher/subscriber setup
ros::init(argc, argv, "BMA180_node");
ros::NodeHandle nh;
// User configurable parameters
/**
* \brief The unique identifier for the hardware interface connecting the
* BMA180 sensors to the computer.
*/
std::string hw_id;
/**
*
* How many BMA180s are connected to the interface. In i2c mode, maximum
* is 2 on one device (using separate addresses). in SPI mode, maximum
* is # of chipselect lines on one device.
*/
int number_of_bma180_sensors;
int publish_rate_Hz;
// Get parameters from .launch file or parameter server, or take defaults
nh.param<int>( "/bma180_node/number_of_bma180_sensors", number_of_bma180_sensors, 1 );
nh.param<int>( "/bma180_node/publish_rate_Hz", publish_rate_Hz, 100 );
nh.param<std::string>( "/bma180_node/hardware_id", hw_id, "0208" );
Sub20Interface sub20( hw_id );
std::vector<BMA180*> bma180_chain;
// Initialize the BMA180s
for( int i = 0; i < number_of_bma180_sensors; i++ )
{
// instantiate a sensor and pass in a pointer to its hardware interface
BMA180* Accelerometer = new BMA180( &sub20 );
// Adjust sensor settings:
Accelerometer->setAccelRange( BMA180Parameters::RANGE_8 );
Accelerometer->setBandwidth( BMA180Parameters::BW_150 );
// SPI configuration
Accelerometer->setProtocol( SPI );
Accelerometer->setFrequency( 125000 );
Accelerometer->setPin( i );
// I2C configuration
//Accelerometer->setProtocol(I2C); // CSB connected to VDD
//Accelerometer->setFrequency( 400000 );
//Accelerometer->setSlaveAddressBit( 0 );
if( Accelerometer->initialize() == false )
{
ROS_ERROR( "BMA180 device: %d initialization failed.", i );
//return 0;
}
// Add the sensor to the sensor chain
bma180_chain.push_back( Accelerometer );
}
// Set up ROS message and publisher
bma180_driver::bma180_measurement msg;
ros::Publisher bma180_pub = nh.advertise<bma180_driver::bma180_measurement>( "BMA180_data", 20 );
//set loop rate for measurement polling
ros::Rate loop_rate_Hz( publish_rate_Hz );
double pitch, roll;
// Run sensor node
while( nh.ok() )
{
// Data goes in a standard vector
for( int j = 0; j < number_of_bma180_sensors; j++ )
{
// Read measurements from sensors
if( bma180_chain[j]->takeMeasurement() == false )
{
ROS_ERROR( "BMA180 device %d failed to acquire measurements.", j );
//return 0;
}
roll = bma180_chain[j]->getStaticRoll() * (180.0/M_PI);
pitch = bma180_chain[j]->getStaticPitch() * (180.0/M_PI);
msg.AccelerationX.push_back( bma180_chain[j]->AccelX_ );
msg.AccelerationY.push_back( bma180_chain[j]->AccelY_ );
msg.AccelerationZ.push_back( bma180_chain[j]->AccelZ_ );
msg.Temperature.push_back( bma180_chain[j]->Temperature_ );
msg.staticRoll.push_back( roll );
msg.staticPitch.push_back( pitch );
}
// publish message
msg.header.stamp = ros::Time::now();
bma180_pub.publish( msg );
// clear vectors so data doesn't accumulate
msg.AccelerationX.clear();
msg.AccelerationY.clear();
msg.AccelerationZ.clear();
msg.Temperature.clear();
msg.staticRoll.clear();
msg.staticPitch.clear();
ros::spinOnce();
loop_rate_Hz.sleep();
}
ROS_WARN( "Closing BMA180 driver." );
return 0;
}
//
// Main
//
int main(int argc, char **argv)
{
//
// initialize ROS
//
ros::init(argc, argv, "uplift_joint_manager");
ros::NodeHandle nh;
ros::NodeHandle nh_feedback("~");
//
// set up Arduino
//
std::string hw_id_arm;
std::string hw_id_spine;
nh.param<std::string>( "hardware_id_arm", hw_id_arm, "/dev/ttyACM0" );
nh.param<std::string>( "hardware_id_spine", hw_id_spine, "/dev/ttyACM1" );
//
// set up controlable joints and robot states
//
joint_names_.resize(2);
joint_names_[SPINE] = "spine";
joint_names_[ARM] = "arm";
// stores current position and velocity readings for all controlable joints
double current_position[joint_names_.size()];
double current_velocity[joint_names_.size()];
// set up robot joint state
sensor_msgs::JointState robot_state;
robot_state.name.resize(2);
robot_state.position.resize(2);
robot_state.velocity.resize(2);
robot_state.name[SPINE] = joint_names_[SPINE];
robot_state.name[ARM] = joint_names_[ARM];
// create lookup table with the same size as the number of joints
lookup.resize( joint_names_.size() );
//
// Create joint drivers for Spine
//
// create Arduino object and initialize
NewArduinoInterface Arduino_Spine( hw_id_spine );
if( Arduino_Spine.initialize() == false)
{
ROS_ERROR("Error initializing Arduino_Spine");
return -1;
}
// create joint driver for position control for the spine
spine_driver_position_.reset( new JointDriver(
&Arduino_Spine, // hardware interface
SPINE_PWM_PIN, SPINE_PWM_FREQUENCY, SPINE_DIRECTION_CONTROL1_PIN, SPINE_DIRECTION_CONTROL2_PIN, // Motor pins and settings
SPINE_ENCODER1_PIN, SPINE_ENCODER2_PIN, SPINE_ENCODER_TICKS_ON_STROKE / 4.0, CREATE_NEW_ENCODER, // Encoder pins and settings; ticks=4*marks
SPINE_LENGTH, JointDriver::POSITION)); // Joint settings
// initalize joint and connected hardware
spine_driver_position_->initialize();
// retrieve encoder id to use in next joint driver
spine_encoder_id_ = spine_driver_position_->getEncoderPtr()->getEncoderID();
// creat joint driver for velocity control for the spine
spine_driver_velocity_.reset( new JointDriver(
&Arduino_Spine, // hardware interface
SPINE_PWM_PIN, SPINE_PWM_FREQUENCY, SPINE_DIRECTION_CONTROL1_PIN, SPINE_DIRECTION_CONTROL2_PIN, // Motor pins and settings
SPINE_ENCODER1_PIN, SPINE_ENCODER2_PIN, SPINE_ENCODER_TICKS_ON_STROKE / 4.0, spine_encoder_id_, // Encoder pins and settings; ticks=4*marks
SPINE_LENGTH, JointDriver::VELOCITY)); // Joint setting
// initalize joint and connected hardware
spine_driver_velocity_->initialize();
//
// create joint drivers for arm
//
// create Arduino object and initialize
NewArduinoInterface Arduino_Arm( hw_id_arm );
if( Arduino_Arm.initialize() == false)
{
ROS_ERROR("Error initializing Arduino_Arm");
return -1;
}
// create joint driver for position control for the arm
arm_driver_position_.reset( new JointDriver(
&Arduino_Arm, // hardware interface
ARM_PWM_PIN, ARM_PWM_FREQUENCY, ARM_DIRECTION_CONTROL1_PIN, ARM_DIRECTION_CONTROL2_PIN, // Motor pins and settings
IMU_CHAIN_ID, // IMU settings
JointDriver::POSITION)); // Joint control mode
// initalize joint and connected hardware
arm_driver_position_->initialize();
// create joint driver for velocity control for the arm
arm_driver_velocity_.reset( new JointDriver(
&Arduino_Arm, // hardware interface
ARM_PWM_PIN, ARM_PWM_FREQUENCY, ARM_DIRECTION_CONTROL1_PIN, ARM_DIRECTION_CONTROL2_PIN, // Motor pins and settings
IMU_CHAIN_ID, // IMU settings
JointDriver::VELOCITY)); // Joint control mode
// initalize joint and connected hardware
arm_driver_velocity_->initialize();
//
// create joint driver for gripper
//
gripper_driver_position_.reset( new JointDriver(
&Arduino_Arm, // hardware interface
GRIPPER_PWM_PIN, GRIPPER_PWM_FREQUENCY, GRIPPER_DIRECTION_CONTROL1_PIN, GRIPPER_DIRECTION_CONTROL2_PIN, // Motor pins and settings
GRIPPER_ADC_PIN, ADC_REFERENCE_VOLTAGE, GRIPPER_LENGTH, // ADC settings
JointDriver::POSITION)); // Joint control mode
//initialize joint and connected hardware
gripper_driver_position_->initialize();
gripper_driver_position_->getPidPtr()->setParams(500,0,0,0);
ros::Subscriber sub = nh.subscribe( "/move_group/result", 1, trajectory_cb);
ros::Subscriber sub_height = nh.subscribe( "/height_detector/height", 1, heigth_cb );
ros::Publisher pub = nh.advertise<sensor_msgs::JointState> ("/joint_states", 1);
ros::Publisher pub_position_target_spine = nh_feedback.advertise<std_msgs::Float64> ("feedback/spine/position/target", 5);
ros::Publisher pub_velocity_target_spine = nh_feedback.advertise<std_msgs::Float64> ("feedback/spine/velocity/target", 5);
ros::Publisher pub_position_target_arm = nh_feedback.advertise<std_msgs::Float64> ("feedback/arm/position/target", 5);
ros::Publisher pub_velocity_target_arm = nh_feedback.advertise<std_msgs::Float64> ("feedback/arm/velocity/target", 5);
//
// dynamic reconfigure settings
//
dynamic_reconfigure::Server<uplift_joint_manager::JointConfig> server;
dynamic_reconfigure::Server<uplift_joint_manager::JointConfig>::CallbackType config;
config = boost::bind(&dynamic_cb, _1, _2);
server.setCallback(config);
ROS_INFO("Running control loop");
ros::Rate loop_rate_Hz(100);
ros::Duration duration_between_points;
while ( ros::ok() )
{
// record time of measurements
ros::Time now = ros::Time::now();
//
// measure current angles, positions and velocities of the robot and publish robot state
//
current_velocity[SPINE] = spine_driver_velocity_->getState();
current_position[SPINE] = spine_driver_position_->getState();
robot_state.position[SPINE] = current_position[SPINE];
robot_state.velocity[SPINE] = current_velocity[SPINE];
current_velocity[ARM] = arm_driver_velocity_->getState();
current_position[ARM] = arm_driver_position_->getState();
robot_state.position[ARM] = (-1) * current_position[ARM];
robot_state.velocity[ARM] = (-1) * current_velocity[ARM];
robot_state.header.stamp = now;
pub.publish( robot_state );
// check if any trajectory has been received yet (point_counter_ is initialized with -1)
if( point_counter_ < 0 )
{
ROS_DEBUG("Waiting for first trajectory to be published");
ros::spinOnce();
loop_rate_Hz.sleep();
continue;
}
// check if trajectory is not completed yet
if( (size_t)point_counter_ < trajectory_desired_->points.size() - 1)
{
// read time stamp of current trajectory point
ros::Time target_time = start_time_trajectory_ + trajectory_desired_->points[point_counter_].time_from_start;
// find next trajectory point which has a time stamp not in the past
// also make sure to not increase the counter above the trajectroy size
while( ( now > target_time ) && ( (size_t)point_counter_ < trajectory_desired_->points.size() - 1) )
{
++point_counter_;
// set target time to new trajectory point time stamp
target_time = start_time_trajectory_ + trajectory_desired_->points[point_counter_].time_from_start;
}
}
// check if gripper is supposed to open or close
if( now > gripper_action_time_ )
{
if( pickup )
{
gripper_target_ = GRIPPER_CLOSE;
ROS_INFO("Closing gripper");
}
else
{
gripper_target_ = GRIPPER_OPEN;
ROS_INFO("Opening gripper");
}
pickup = !pickup;
reached_gripper_limit_ = false;
successful_grab_ = false;
force_counter = 0;
// set action time to infinity
gripper_action_time_ = ros::Time((uint32_t)ULONG_MAX, 0);
}
//
// SPINE control
// extract trajectory target information and apply new control to spine motor
//
double target_velocity_spine = trajectory_desired_->points[point_counter_].velocities[lookup[SPINE]];
double output_velocity_control_spine = spine_driver_velocity_->compute( current_velocity[SPINE], target_velocity_spine );
ROS_DEBUG("spine velocity: current:%f target:%f output:%f", current_velocity[SPINE], target_velocity_spine, output_velocity_control_spine );
double target_position_spine = trajectory_desired_->points[point_counter_].positions[lookup[SPINE]];
double output_position_control_spine = spine_driver_position_->compute( current_position[SPINE], target_position_spine );
ROS_DEBUG("spine position: current:%f target:%f output:%f", current_position[SPINE], target_position_spine, output_position_control_spine );
// weigh position and velocity output and apply
spine_driver_velocity_->applyOutput( (output_velocity_control_spine * spine_velocity_influence_) + (output_position_control_spine * spine_position_influence_) );
// publish feedback information
std_msgs::Float64 temp;
temp.data = target_position_spine;
pub_position_target_spine.publish( temp );
temp.data = target_velocity_spine;
pub_velocity_target_spine.publish( temp );
//
// ARM control
// extract trajectory target information and apply new control to arm motor
//
double target_velocity_arm = (-1.0) * trajectory_desired_->points[point_counter_].velocities[lookup[ARM]]; // adjust rotation direction
double output_velocity_control_arm = arm_driver_velocity_->compute( current_velocity[ARM], target_velocity_arm );
ROS_DEBUG("arm velocity: current:%f target:%f output:%f", current_velocity[ARM], target_velocity_arm, output_velocity_control_arm );
double target_position_arm = (-1.0) * trajectory_desired_->points[point_counter_].positions[lookup[ARM]]; // adjust rotation direction
double output_position_control_arm = arm_driver_position_->compute( current_position[ARM], target_position_arm );
ROS_DEBUG("arm position: current:%f target:%f output:%f", current_position[ARM], target_position_arm, output_position_control_arm );
// weigh position and velocity output and apply
arm_driver_velocity_->applyOutput( (output_velocity_control_arm * arm_velocity_influence_) + (output_position_control_arm * arm_position_influence_) );
//
// Gripper control
//
double gripper_position = gripper_driver_position_->getState( );
double gripper_delta = gripper_last_position_ - gripper_position;
double gripper_control = gripper_driver_position_->compute( gripper_position, gripper_target_ );
gripper_last_position_ = gripper_position;
// check if endpoint has been reached
if( !pickup ) // gripper's target is CLOSE
{
if( gripper_position > GRIPPER_CLOSE - GRIPPER_DELTA )
{
ROS_DEBUG("gripper closed");
reached_gripper_limit_ = true;
successful_grab_ = false;
gripper_driver_position_->applyOutput(0.0);
}
}
else // gripper's target is open
{
if( gripper_position < GRIPPER_OPEN + GRIPPER_DELTA )
{
ROS_DEBUG("gripper open");
reached_gripper_limit_ = true;
successful_grab_ = false;
gripper_driver_position_->applyOutput(0.0);
}
}
// check if maximum motor torque is used but motor does not move -> object grabbed
if( gripper_control >= 1.0 && !pickup)
{
if( (-1) * gripper_delta < GRIPPER_DELTA )
{
ROS_DEBUG("gripper delta %f", gripper_delta);
if( ++force_counter > GRIPPER_FORCE_LIMIT )
{
ROS_DEBUG("gripper limit reached");
reached_gripper_limit_ = true;
gripper_driver_position_->applyOutput(0.0);
successful_grab_ = true;
}
}
else
{
force_counter = 0;
}
}
// apply new PWM duty cycle
if( !reached_gripper_limit_ )
{
gripper_driver_position_->applyOutput( gripper_control );
ROS_DEBUG("gripper target: %f real: %f", gripper_target_, gripper_position);
}
// publish feedback information
std_msgs::Float64 temp_arm;
temp_arm.data = target_position_arm;
pub_position_target_arm.publish( temp_arm );
temp_arm.data = target_velocity_arm;
pub_velocity_target_arm.publish( temp_arm );
ros::spinOnce();
loop_rate_Hz.sleep();
}
return 0;
}
