// write the actuator state to the actuator buffer
void write_state( void ) {
//if( VERBOSE ) printf( "(dynamics::write_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
actuator_msg_c msg = actuator_msg_c( );
msg.header.type = ACTUATOR_MSG_REPLY;
msg.state.position = pivot->q[0];
msg.state.velocity = pivot->qd[0];
msg.state.time = sim->current_time;
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero as they were also originally added there.
// So, had to relocate the reset procedure here.
pendulum->reset_accumulators();
// Note: will block on acquiring mutex
if( dyn_amsgbuffer.write( msg ) != BUFFER_ERROR_NONE ) {
sprintf( strbuffer, "(dynamics.cpp) write_state() failed calling actuator_msg_buffer_c.write(msg)\n" );
dyn_error_log.write( strbuffer );
// TODO : return error condition
}
}
/**
publish the state of the actuators to the command buffers. In this case, one actuator
and one controller.
*/
void publish_state( void ) {
//printf( "(dynamics::publish_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
Command cmd = Command( );
cmd.state.position = pivot->q[0];
cmd.state.velocity = pivot->qd[0];
cmd.state.time = sim->current_time;
//cmd.print();
cmdbuffer.write( cmd );
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero. So, had to relocate the reset procedure
// here.
pendulum->reset_accumulators();
}
/**
publish the state of the actuators to the command buffers. In this case, one actuator
and one controller.
*/
void publish_state( void ) {
if( VERBOSE ) printf( "(dynamics::publish_state)\n" );
// get a reference to the actuator
JointPtr pivot = pendulum->find_joint( "pivot" );
// write the actuator state out to the command buffer
ActuatorMessage msg = ActuatorMessage( );
msg.header.type = MSG_TYPE_STATE_REPLY;
msg.state.position = pivot->q[0];
msg.state.velocity = pivot->qd[0];
msg.state.time = sim->current_time;
// this was a part of the Dynamic ode_both(.) procedure but if the accumulators are
// cleared there they can never change from zero as they were also originally added there.
// So, had to relocate the reset procedure here.
pendulum->reset_accumulators();
// Note: will block on acquiring mutex
amsgbuffer.write( msg );
}