/**************************
* @function main
***************************/
int main( int argc, char* argv[] ) {
// Call sns
sns_init();
sns_start();
// 2. Open channels
sns_chan_open( &chan_state_right, "state-right", NULL );
{
ach_channel_t *chans[] = {&chan_state_right, NULL};
sns_sigcancel( chans, sns_sig_term_default );
}
// 3. Run
while( !sns_cx.shutdown ) {
update();
aa_mem_region_local_release();
usleep(0.5*1e6);
}
// 4. Leave
sns_end();
return 0;
}
/**
* @function openComm
*/
void openComm() {
// Start sns
sns_init();
sns_start();
// Open channels to read motor states
ach_channel_t* arm_state_chan[2];
ach_channel_t* hand_state_chan[2];
for( int i = 0; i < 2; ++i ) {
arm_state_chan[i] = new ach_channel_t();
hand_state_chan[i] = new ach_channel_t();
}
for( int i = 0; i < 2; ++i ) {
sns_chan_open( arm_state_chan[i], mS.limb[i].arm_state_chan.c_str(), NULL );
sns_chan_open( hand_state_chan[i], mS.limb[i].hand_state_chan.c_str(), NULL );
}
// BIMANUAL
sns_chan_open( &mBiTraj_chan, "bimanual_chan", NULL );
sns_chan_open( &mBiHand_chan, "bimanual_hand_chan", NULL );
mDli.set_numJoints(7,7);
mDli.set_hand_channels( hand_state_chan[0], hand_state_chan[1], &mBiHand_chan );
mDli.set_arm_channels( arm_state_chan[0], arm_state_chan[1], &mBiTraj_chan );
// Ready
printf("\t * [SET_COMM] We should be ready to go \n");
}
void main_init( void )
{
int sts;
sts = get_reset_reason();
main_status = MAIN_STS_NO_SNS_ERR;
main_is_enter_stby_mode = 0;
bio_init();
clk_init();
uart_init();
dbg_wait();
#ifdef FEATURE_WDOG_TRIG
IWDG_Enable();
bio_led_ctrl( BIO_LED_IX_POWER, 0, 0, 0 );
#else // FEATURE_WDOG_TRIG
dbg_out( "\r\n**********==> Watchdog Disabled!!!!!\r\n" );
bio_led_ctrl( BIO_LED_IX_POWER, 500, 500, BIO_LED_ALWAYS_WINK );
#endif // FEATURE_WDOG_TRIG
dbg_out_pool_const( UART_DBG_MSG_ENTER3 );
dbg_out( "================== 0x%02X =================\r\n", sts );
dbg_out( "==== Welcome to %s Ver %d.%02X ====\r\n",
MAIN_TIT_STR, MAIN_VER_MAJ, MAIN_VER_MIN );
dbg_out( "===== i=%d, si=%d, li=%d, f=%d, d=%d =====\r\n",
sizeof( int ), sizeof( short int ),
sizeof( long int ),sizeof( float ),sizeof( double ) );
dbg_out( "===== %dMHz %s %s =====\r\n",
SystemCoreClock / 1000000, __DATE__, __TIME__ );
dbg_out_pool_const( UART_DBG_MSG_EQ_LINE );
dbg_wait();
view_reset_reason( sts );
eep_param_init();
IWDG_ReloadCounter();
u3_ctrl.sw_baud = 115200;
uart3_init();
sns_init();
adc_init();
zb_init();
encb_init();
fnd_init();
indc_init();
bio_set_fan_pwm_base_freq( eep_hw_info.fan_base_f );
cmd_rx_ptr = u1_ctrl.rx_bptr;
// set monitor timer
clk_set_timer( &monit_timer, MAIN_MONITOR_MS, MAIN_MONITOR_MS,
CLK_SIGS_MONITOR );
} // end of main_init()
/* Menu function */
void gui_sns_file_new(void *pass, int call_type) {
/*
* create a new inner file
*
* */
gui_save_confirmation();
gui_clean_var();
_gui_clean_column();
sns_init(&SNS);
sns_changed = 0;
sns_filename = NULL;
}
void CrichtonView<PointT>::startComm( int state, void* userdata ) {
sns_init();
sns_start();
ach_status_t r;
r = ach_open( &mObj_param_chan, gObj_param_chan_name.c_str(), NULL );
if( r != ACH_OK ) { printf("[ERROR] COULD NOT OPEN OBJ PARAM CHAN"); return; }
r = ach_open( &mServer2Module_chan, gServer2Module_chan_name.c_str(), NULL );
if( r != ACH_OK ) { printf("[ERROR] COULD NOT OPEN SERVER-2-SEE CHAN"); return; }
r = ach_open( &mModule2Server_chan, gModule2Server_chan_name.c_str(), NULL );
if( r != ACH_OK ) { printf("[ERROR] COULD NOT SEE-2-SERVER CHAN"); return; }
printf("\t [OK] Communication stablished and ready to go \n");
mChanReady = true;
}
/**
* @function main
*/
int main( int argc, char* argv[] ) {
sns_init();
sns_start();
traj_name[0] = "test-left";
traj_name[1] = "test-right";
// Create channels
for( int i = 0; i < 2; ++i ) {
traj_chan[i] = new ach_channel_t();
}
// Open & set channels
for( int i = 0; i < 2; ++i ) {
sns_chan_open( traj_chan[i], traj_name[i].c_str(), NULL );
}
// Loop until you hear a trajectory coming
while(true) {
if( clock_gettime( ACH_DEFAULT_CLOCK, &now ) ) {
SNS_LOG( LOG_ERR, "clock_gettime failed: '%s' \n", strerror(errno) );
printf("Did not get time right \n");
return 0;
}
ts= sns_time_add_ns( now, 1000*1000*1 );
check_traj_chan( &ts );
usleep((1e6*mDt) );
}
return 0;
}
