void KSGTaskCollDeviceState::CollDeviceState::Visit(KSGDeviceNode& node)
{
char devphy_no[9] = "";
if(dev_start_ >= sizeof(dev_state_buf_))
{
// 发送心跳
if(send_state())
{
ACE_DEBUG((LM_ERROR,"发送心跳失败!"));
return;
}
else
dev_start_ = 0;
}
if(!node.Accept(*this))
return;
int state = 0;
ACE_Time_Value now_time = ACE_OS::gettimeofday();
now_time -= node.get_update_time();
if(now_time.sec() > 60 * 8)
state = 1;
if(node.get_vendor() == KSG_HD_DEV)
{
encode_hd_sn(node.GetPhyId().c_str(),devphy_no);
}
else
ACE_OS::strncpy(devphy_no,node.GetPhyId().c_str(),8);
ACE_OS::sprintf(&dev_state_buf_[dev_start_],"%s%02X",devphy_no,state);
dev_start_ += s_dev_state_len_;
}
void fsm_left_button(){
switch (g_state) {
case STATE_MENU:
if(menu_home()){
play_melody(MENU_SOUND);
}
g_state = STATE_MENU;
break;
case STATE_GAME:
menu_home();
g_state = STATE_MENU;
play_melody(MENU_SOUND);
_delay_ms(50);
send_state(g_state);
break;
case STATE_GAME_OVER:
break;
case STATE_CALIBRATE:
menu_home();
g_state = STATE_MENU;
break;
case STATE_DEBUG:
menu_home();
g_state = STATE_MENU;
break;
case STATE_PID:
break;
default:
break;
}
}
void *threadSttRover(void *sArg){
int time0 = 0, time1;
sInfos inf2send;
sArgThrdSttCom* arg = (sArgThrdSttCom*)sArg;
memcpy(arg, sArg, sizeof(sArgThrdSttCom));
while(arg->clt_sock > 0){
time1 = millisec();
if((time1 - time0) > T_INFOS){
time0 = time1;
/*printf("arg->sinf.num = %d\n"
"arg->sinf.bat = %d\n"
"arg->sinf.son = %.2f\n"
"arg->sinf.pos = (%.2f; %.2f)\n"
"arg->sinf.ang = %.2f\n", arg->sinf.num, arg->sinf.bat, arg->sinf.son, arg->sinf.pos.x, arg->sinf.pos.y, arg->sinf.ang);
*/
inf2send.num = arg->sinf.num;
inf2send.bat = arg->sinf.bat;
inf2send.son = arg->sinf.son;
inf2send.pos = arg->sinf.pos;
inf2send.ang = arg->sinf.ang;
send_state(inf2send, arg->clt_sock);
}
}
pthread_exit(NULL);
}
static void new_game(uint8_t controller){
g_state = STATE_GAME;
g_game_controller = controller;
send_controller(g_game_controller);
play_melody(g_game_melody);
reset_score();
print_score();
send_state(g_state);
}
void fsm_right_button(){
switch (g_state) {
case STATE_MENU:
if(menu_next()){
play_melody(MENU_SOUND);
}
_delay_ms(50);
if(menu_get_index() == 0){
if(menu_get_arrow_position == 2){
oled_print_pong();
}
else if(menu_get_arrow_position() == 3){
oled_print_game_over();
}
}
else if(menu_get_index() == 4){
g_state = STATE_CALIBRATE;
}
else if(menu_get_index() == 6){
g_state = STATE_DEBUG;
}
else if(menu_get_index() == 7){
g_game_melody = menu_get_arrow_position();
play_melody(g_game_melody);
}
else if(menu_get_index() == 2){
new_game(JOYSTICK);
}
else if(menu_get_index() == 3){
new_game(PLAYSTATION);
}
break;
case STATE_GAME: // Send pulse to solenoid
set_bit(DDRD, PD4);
_delay_ms(200);
clear_bit(DDRD,PD4);
break;
case STATE_GAME_OVER:
break;
case STATE_CALIBRATE:
joy_calibrate();
break;
case STATE_DEBUG:
if(menu_get_arrow_position() == 1){
send_state(STATE_DEBUG);
printf("Debug Mode\n");
}
break;
case STATE_PID:
break;
default:
break;
}
}
int main(int argc, char *argv[])
{
struct display *display;
char buf[256], *p;
int ret, fd;
display = malloc(sizeof *display);
assert(display);
display->display = wl_display_connect(NULL);
assert(display->display);
display->registry = wl_display_get_registry(display->display);
wl_registry_add_listener(display->registry,
®istry_listener, display);
wl_display_dispatch(display->display);
wl_display_dispatch(display->display);
fd = 0;
p = getenv("TEST_SOCKET");
if (p)
fd = strtol(p, NULL, 0);
while (1) {
ret = read(fd, buf, sizeof buf);
if (ret == -1) {
fprintf(stderr, "test-client: read error: fd %d, %m\n",
fd);
return -1;
}
fprintf(stderr, "test-client: got %.*s\n", ret - 1, buf);
if (strncmp(buf, "bye\n", ret) == 0) {
return 0;
} else if (strncmp(buf, "create-surface\n", ret) == 0) {
create_surface(fd, display);
} else if (strncmp(buf, "send-state\n", ret) == 0) {
send_state(fd, display);
} else if (strncmp(buf, "send-button-state\n", ret) == 0) {
send_button_state(fd, display);
} else if (strncmp(buf, "send-keyboard-state\n", ret) == 0) {
send_keyboard_state(fd, display);
} else {
fprintf(stderr, "test-client: unknown command %.*s\n",
ret, buf);
return -1;
}
}
assert(0);
}
void fsm_joy_right(){
switch (g_state) {
case STATE_MENU:
if(menu_next()){
play_melody(MENU_SOUND);
}
if(menu_get_index() == 0){
if(menu_get_arrow_position == 2){
oled_print_pong();
}
else if(menu_get_arrow_position() == 3){
oled_print_game_over();
}
}
else if(menu_get_index() == 4){
g_state = STATE_CALIBRATE;
}
else if(menu_get_index() == 6){
g_state = STATE_DEBUG;
}
else if(menu_get_index() == 7){
g_game_melody = menu_get_arrow_position();
play_melody(g_game_melody);
}
else if(menu_get_index() == 2){
new_game(JOYSTICK);
}
else if(menu_get_index() == 3){
new_game(PLAYSTATION);
}
break;
case STATE_GAME:
break;
case STATE_GAME_OVER:
break;
case STATE_CALIBRATE:
break;
case STATE_DEBUG:
if(menu_get_arrow_position() == 1){
send_state(STATE_DEBUG);
printf("Debug Mode\n");
}
break;
case STATE_PID:
break;
default:
break;
}
}
void motion(bool enable, int velocity_left, int velocity_right) {
//ROS_INFO("ENTER motion()");
if (enable!=last_state)
send_state(enable);
send_vel(velocity_left, velocity_right);
if (-1==gettimeofday(&last_motion, NULL)) {
perror("motion:gettimeofday()");
exit(1);
}
//ROS_INFO("RETURN motion()");
}
void PFDigitalPin::handle_message()
{
zmqpp::message_t msg;
std::string frame1;
sock_.receive(msg);
msg >> frame1;
bool ok = false;
if (frame1 == "ON")
ok = turn_on(&msg);
else if (frame1 == "OFF")
ok = turn_off();
else if (frame1 == "TOGGLE")
ok = toggle();
else if (frame1 == "STATE")
return send_state();
else // invalid cmd
ERROR("Invalid command received (" << frame1 << "). Potential missconfiguration !");
sock_.send(ok ? "OK" : "KO");
}
void motors_init(void) {
// Original (IdMind)
// char init1[] = "di\rencres 2064\ransw 0\rapl 0\rsor 0\rsp 30000\rlcc 5000\rlpc 8000\rac 30000\rpor16\rI30\rcontmod\rv0\r";
//char init2[] = "diprog\rprogseq\rseta1\ra1\rdelay80\rjpe2\rseta1\rdi\rjmp1\ra2\rdajnz3\ren\rseta2\rjmp1\ra3\rseta2\rjmp1\rend\renprog\r";
// Modified (yoda)
char init1[1024];
char init2[] = "diprog\rprogseq\rseta1\ra1\rdelay25\rjpe2\rseta1\rdi\rjmp1\ra2\rdajnz3\ren\rseta2\rjmp1\ra3\rseta2\rjmp1\rend\renprog\r";
sprintf(init1, "di\rencres 2064\ransw 0\rapl 0\rsor 0\rsp 30000\rlcc 5000\rlpc 8000\rac %d\rpor16\rI30\rcontmod\rv0\reepsav\r", accel);
// p[0].name = "/dev/ttyS0";
// p[1].name = "/dev/ttyS1";
for (int i=0 ; i<NP ; i++) {
port_open(i);
port_send(i, init1);
port_send(i, init2);
// BUG: port_send(i, "AC15\r");
}
send_state(true);
}
int /* O - Exit status */
main(int argc, /* I - Number of command-line arguments */
char *argv[]) /* I - Command-line arguments */
{
int i; /* Looping var */
int num_defaults; /* Number of default options */
cups_option_t *defaults; /* Default options */
char line[256], /* Command string */
command, /* Command code */
*dest, /* Pointer to destination */
*list, /* Pointer to list */
*agent, /* Pointer to user */
status; /* Status for client */
socklen_t hostlen; /* Size of client address */
http_addr_t hostaddr; /* Address of client */
char hostname[256], /* Name of client */
hostip[256], /* IP address */
*hostfamily; /* Address family */
int hostlookups; /* Do hostname lookups? */
#ifdef __APPLE__
vproc_transaction_t vtran = vproc_transaction_begin(NULL);
#endif /* __APPLE__ */
/*
* Don't buffer the output...
*/
setbuf(stdout, NULL);
/*
* Log things using the "cups-lpd" name...
*/
openlog("cups-lpd", LOG_PID, LOG_LPR);
/*
* Scan the command-line for options...
*/
num_defaults = 0;
defaults = NULL;
hostlookups = 1;
for (i = 1; i < argc; i ++)
if (argv[i][0] == '-')
{
switch (argv[i][1])
{
case 'h' : /* -h hostname[:port] */
if (argv[i][2])
cupsSetServer(argv[i] + 2);
else
{
i ++;
if (i < argc)
cupsSetServer(argv[i]);
else
syslog(LOG_WARNING, "Expected hostname string after -h option!");
}
break;
case 'o' : /* Option */
if (argv[i][2])
num_defaults = cupsParseOptions(argv[i] + 2, num_defaults,
&defaults);
else
{
i ++;
if (i < argc)
num_defaults = cupsParseOptions(argv[i], num_defaults,
&defaults);
else
syslog(LOG_WARNING, "Expected option string after -o option!");
}
break;
case 'n' : /* Don't do hostname lookups */
hostlookups = 0;
break;
default :
syslog(LOG_WARNING, "Unknown option \"%c\" ignored!", argv[i][1]);
break;
}
}
else
syslog(LOG_WARNING, "Unknown command-line option \"%s\" ignored!",
argv[i]);
/*
* Get the address of the client...
*/
hostlen = sizeof(hostaddr);
if (getpeername(0, (struct sockaddr *)&hostaddr, &hostlen))
{
syslog(LOG_WARNING, "Unable to get client address - %s", strerror(errno));
strlcpy(hostname, "unknown", sizeof(hostname));
}
else
{
httpAddrString(&hostaddr, hostip, sizeof(hostip));
if (hostlookups)
httpAddrLookup(&hostaddr, hostname, sizeof(hostname));
else
strlcpy(hostname, hostip, sizeof(hostname));
#ifdef AF_INET6
if (hostaddr.addr.sa_family == AF_INET6)
hostfamily = "IPv6";
else
#endif /* AF_INET6 */
hostfamily = "IPv4";
syslog(LOG_INFO, "Connection from %s (%s %s)", hostname, hostfamily,
hostip);
}
num_defaults = cupsAddOption("job-originating-host-name", hostname,
num_defaults, &defaults);
/*
* RFC1179 specifies that only 1 daemon command can be received for
* every connection.
*/
if (smart_gets(line, sizeof(line), stdin) == NULL)
{
/*
* Unable to get command from client! Send an error status and return.
*/
syslog(LOG_ERR, "Unable to get command line from client!");
putchar(1);
#ifdef __APPLE__
vproc_transaction_end(NULL, vtran);
#endif /* __APPLE__ */
return (1);
}
/*
* The first byte is the command byte. After that will be the queue name,
* resource list, and/or user name.
*/
if ((command = line[0]) == '\0')
dest = line;
else
dest = line + 1;
if (command == 0x02)
list = NULL;
else
{
for (list = dest; *list && !isspace(*list & 255); list ++);
while (isspace(*list & 255))
*list++ = '\0';
}
/*
* Do the command...
*/
switch (command)
{
default : /* Unknown command */
syslog(LOG_ERR, "Unknown LPD command 0x%02X!", command);
syslog(LOG_ERR, "Command line = %s", line + 1);
putchar(1);
status = 1;
break;
case 0x01 : /* Print any waiting jobs */
syslog(LOG_INFO, "Print waiting jobs (no-op)");
putchar(0);
status = 0;
break;
case 0x02 : /* Receive a printer job */
syslog(LOG_INFO, "Receive print job for %s", dest);
/* recv_print_job() sends initial status byte */
status = (char)recv_print_job(dest, num_defaults, defaults);
break;
case 0x03 : /* Send queue state (short) */
syslog(LOG_INFO, "Send queue state (short) for %s %s", dest, list);
/* no status byte for this command */
status = (char)send_state(dest, list, 0);
break;
case 0x04 : /* Send queue state (long) */
syslog(LOG_INFO, "Send queue state (long) for %s %s", dest, list);
/* no status byte for this command */
status = (char)send_state(dest, list, 1);
break;
case 0x05 : /* Remove jobs */
if (list)
{
/*
* Grab the agent and skip to the list of users and/or jobs.
*/
agent = list;
for (; *list && !isspace(*list & 255); list ++);
while (isspace(*list & 255))
*list++ = '\0';
syslog(LOG_INFO, "Remove jobs %s on %s by %s", list, dest, agent);
status = (char)remove_jobs(dest, agent, list);
}
else
status = 1;
putchar(status);
break;
}
syslog(LOG_INFO, "Closing connection");
closelog();
#ifdef __APPLE__
vproc_transaction_end(NULL, vtran);
#endif /* __APPLE__ */
return (status);
}
void loop() {
recieve_commands();
if (!docking && !auto_cover) {
execute_move();
interaction();
}
if (move_time == 0) {
if (stream) // @t
stream_data();
if (exp_auto) // @a
feb_experiment_scanning();//katie_auto();
if (auto_on ) // @u
katie_auto();
// if (auto_cover){ /// send state message at a regular interval during script
// int diff = millis() - picture_timer;
// if (diff > 3000) {
// send_state("picture timer");
// picture_timer = millis();
// }
// }
if (exp_stream ) // @e
// arduino_hub_stream(); // experiment_dec_2013
if (debug_stream ) // @g
stream_debug_sensors();
}
BUTTONS_STATE = COIFetchSingleSensor(BUTTONS);
if (PLAY_BUTTON(BUTTONS_STATE) == HIGH) {
delay(200);
COIChangeMode(CMD_MODE_PASSIVE);
play_toggle = true;
send_response("SAFE MODE ON", 1);
}
if ((CHARGING_SOURCE_STATE == 1 || CHARGING_SOURCE_STATE == 2) && play_toggle == true ) {
charge_toggle = true;
play_toggle = false;
send_response("CHARGING SOURCE DETECTED", 1);
}
if ((CHARGING_SOURCE_STATE == 0) && (OI_MODE_STATE == OI_PASSIVE) && (charge_toggle == true) && (play_toggle == false)){
COIChangeMode(CMD_MODE_FULL);
charge_toggle = false;
send_response("FULL MODE ON", 1);
}
update_battery_sensors();
charger();
if (!digitalRead(INPUT_20) && !auto_cover ) { // check to see if not in script so command does not interfere
//update_create_fast_sensors(); // so that collision detection works
BUMPS_WHEEL_DROPS_STATE = COIFetchSingleSensor(BUMPS_WHEELS);
//charger(); // detects charger (once this works move make nest it deeper in framework)
}
if ((BUMPS_WHEEL_DROPS_STATE > 0 && BUMPS_WHEEL_DROPS_STATE < 4) && bp == false) {
bp = true;
send_state("collision detected");
}
if ((BUMPS_WHEEL_DROPS_STATE == 0) && bp == true) {
bp = false;
send_state("bumper released");
}
//heading = update_header(heading, COIFetchSingleSensor(ANGLE_SINCE_LAST); // Mostly for testing, for now
delay(20); // serial gets overrun if no delay
}
main(int argc, char *argv[]) {
char *pgm;
char *server;
int i, j;
int id, nc;
int nstep = 10;
int nmodule = 5;
int running[MAX_MODULES], runstate[MAX_MODULES], nrunning;
pgm = argv[0];
++argv; --argc;
if (!argc) {
fprintf(stderr, "usage: %s server [nstep] [nmodule]\n", pgm);
exit(1);
}
server = argv[0];
++argv; --argc;
if (argc) {
nstep = atoi(argv[0]);
++argv; --argc;
}
if (argc) {
nmodule = atoi(argv[0]);
++argv; --argc;
}
if (argc) {
fprintf(stderr, "usage: %s server [nstep] [nmodule]\n", pgm);
exit(1);
}
event_verbose(0);
efi_init();
id = event_join(server, &nc);
if (!id) {
fprintf(stderr, "%s: couldn't join server %s\n", pgm, server);
exit(1);
}
fprintf(stderr, "%s: Joined %s as client id %d (%d clients)\n",
pgm, server, id, nc);
/* Don't receive any events */
event_select_type(0, 0, ET_MAX);
event_receive_enable(0);
/* Send loaded messages */
for (i = 0; i < nmodule; ++i) {
send_state(i, 2);
event_sleep(irand(100, 1000));
}
/* Send an idle messages */
for (i = 0; i < nmodule; ++i) {
send_state(i, 3);
event_sleep(irand(100, 1000));
}
/* Cycle run-send-idle for each step */
for (i = 0; i < nstep; ++i) {
/* Set all modules to IDLE */
for (j = 0; j < nmodule; ++j) {
running[j] = j;
runstate[j] = 3;
}
nrunning = nmodule;
/* Randomly cycle a module */
while (nrunning) {
j = irand(0, nrunning-1);
if (runstate[j] == 5) runstate[j] = 3;
else ++runstate[j];
send_state(running[j], runstate[j]);
/* Remove modules that have gone idle */
if (runstate[j] == 3) {
running[j] = running[nrunning-1];
runstate[j] = runstate[nrunning-1];
nrunning--;
}
event_sleep(irand(100, 2000));
}
send_frame(i+1, nstep);
printf("Finished frame %d\n", i);
}
/* Send an unload messages */
for (i = 0; i < nmodule; ++i) {
send_state(i, 1);
event_sleep(irand(100, 1000));
}
}
