CollisionDetector::CollisionDetector(RTC::Manager* manager)
: RTC::DataFlowComponentBase(manager),
// <rtc-template block="initializer">
m_qRefIn("qRef", m_qRef),
m_qCurrentIn("qCurrent", m_qCurrent),
m_servoStateIn("servoStateIn", m_servoState),
m_qOut("q", m_q),
m_beepCommandOut("beepCommand", m_beepCommand),
m_CollisionDetectorServicePort("CollisionDetectorService"),
// </rtc-template>
#ifdef USE_HRPSYSUTIL
m_scene(&m_log),
m_window(&m_scene, &m_log),
m_glbody(NULL),
#endif // USE_HRPSYSUTIL
m_use_limb_collision(false),
m_use_viewer(false),
m_robot(hrp::BodyPtr()),
m_loop_for_check(0),
m_collision_loop(1),
m_debugLevel(0),
m_enable(true),
collision_beep_count(0),
is_beep_port_connected(false),
dummy(0)
{
m_service0.collision(this);
#ifdef USE_HRPSYSUTIL
m_log.enableRingBuffer(1);
#endif // USE_HRPSYSUTIL
init_beep();
start_beep(3136);
}
int main (int argc, char** argv)
{
int i;
wiringPiSetup () ;
init_LED(LED1_PIN);
init_LED(LED2_PIN);
init_LED(LED3_PIN);
init_LED(LED4_PIN);
init_LED(LED5_PIN);
init_LED(LED6_PIN);
init_button(BUTTON_PIN);
init_beep(BEEP_PIN);
if(strcmp(argv[1], "LED")==0)
{
int led = atoi(argv[2]);
int on = atoi(argv[3]);
int off= atoi(argv[4]);
switch(led)
{
case 1:
LED_blink(LED1_PIN, on, off);
break;
case 2:
LED_blink(LED2_PIN, on, off);
break;
case 3:
LED_blink(LED3_PIN, on, off);
break;
case 4:
LED_blink(LED4_PIN, on, off);
break;
case 5:
LED_blink(LED5_PIN, on, off);
break;
case 6:
LED_blink(LED6_PIN, on, off);
break;
}
}
else if(strcmp(argv[1], "BEEP")==0)
{
int freq = atoi(argv[2]);
Beep_Tone(BEEP_PIN, freq);
}
return 0 ;
}
int main()
{
init_led();
init_beep();
//init_key(); 轮循初始化
//init_interrupt_for_key(); //中断的方式
led_off_all();
if(flag){
led_on_all();
beep_on();
}
int i;
while(1){
}
return 0;
}
int main()
{
init_led();
init_beep();
int i;
while(1)
{
for(i=1;i<=4;i++)
{
beep_on();
led_on(i);
delay(0xffff);
beep_off();
led_off(i);
delay(0xffff);
}
}
return 0;
}
int main()
{
init_led();
init_beep();
init_key();
led_off_all();
int i;
while(1)
{
if(i= key_press()){
led_on(i);
beep_on();
}else{
led_off_all();
beep_off();
}
}
return 0;
}
