/* Returns the statHash */ ScmObj Scm_ProfilerRawResult(void) { ScmVM *vm = Scm_VM(); if (vm->prof == NULL) return SCM_FALSE; if (vm->prof->state == SCM_PROFILER_INACTIVE) return SCM_FALSE; if (vm->prof->state == SCM_PROFILER_RUNNING) Scm_ProfilerStop(); if (vm->prof->errorOccurred > 0) { Scm_Warn("profiler: An error has been occurred during saving profiling samples. The result may not be accurate"); } Scm_ProfilerCountBufferFlush(vm); /* collect samples in the current buffer */ collect_samples(vm->prof); /* collect samples in the saved file */ off_t off; SCM_SYSCALL(off, lseek(vm->prof->samplerFd, 0, SEEK_SET)); if (off == (off_t)-1) { Scm_ProfilerReset(); Scm_Error("profiler: seek failed in retrieving sample data"); } for (;;) { ssize_t r = read(vm->prof->samplerFd, vm->prof->samples, sizeof(ScmProfSample[1]) * SCM_PROF_SAMPLES_IN_BUFFER); if (r <= 0) break; vm->prof->currentSample = r / sizeof(ScmProfSample[1]); collect_samples(vm->prof); } vm->prof->currentSample = 0; #if defined(GAUCHE_WINDOWS) if (vm->prof->samplerFd >= 0) { close(vm->prof->samplerFd); vm->prof->samplerFd = -1; unlink(vm->prof->samplerFileName); } #else /* !GAUCHE_WINDOWS */ if (ftruncate(vm->prof->samplerFd, 0) < 0) { Scm_SysError("profiler: failed to truncate temporary file"); } #endif /* !GAUCHE_WINDOWS */ return SCM_OBJ(vm->prof->statHash); }
/* Returns the statHash */ ScmObj Scm_ProfilerRawResult(void) { ScmVM *vm = Scm_VM(); if (vm->prof == NULL) return SCM_FALSE; if (vm->prof->state == SCM_PROFILER_INACTIVE) return SCM_FALSE; if (vm->prof->state == SCM_PROFILER_RUNNING) Scm_ProfilerStop(); if (vm->prof->errorOccurred > 0) { Scm_Warn("profiler: An error has been occurred during saving profiling samples. The result may not be accurate"); } Scm_ProfilerCountBufferFlush(vm); /* collect samples in the current buffer */ collect_samples(vm->prof); /* collect samples in the saved file */ off_t off; SCM_SYSCALL(off, lseek(vm->prof->samplerFd, 0, SEEK_SET)); if (off == (off_t)-1) { Scm_ProfilerReset(); Scm_Error("profiler: seek failed in retrieving sample data"); } ScmObj sampler_port = Scm_MakePortWithFd(SCM_FALSE, SCM_PORT_INPUT, vm->prof->samplerFd, SCM_PORT_BUFFER_FULL, FALSE); for (;;) { ssize_t r = read(vm->prof->samplerFd, vm->prof->samples, sizeof(ScmProfSample[1]) * SCM_PROF_SAMPLES_IN_BUFFER); if (r <= 0) break; vm->prof->currentSample = r / sizeof(ScmProfSample[1]); collect_samples(vm->prof); } vm->prof->currentSample = 0; if (ftruncate(vm->prof->samplerFd, 0) < 0) { Scm_SysError("profiler: failed to truncate temporary file"); } return SCM_OBJ(vm->prof->statHash); }
void main(void) { int num_behaviors=5; // number of behaviors U32 robot_sched_duration=100; // minimum time spent for each main loop (in ms) char *title="Mover"; // program title to display U32 max_iterations=65536; // maximum number of iterations of main loop U32 sched_tick=0; // store system timer for robot sleep // Tone Control Constants U32 tone_enabled_freq=2500; // Hz U32 tone_silenced_freq=0; // Hz U32 tone_enabled_dur=100; // ms // wheel control constants S8 stop_speed=0; S8 fwd_speed=30; S8 fastrot_speed=40; S8 slowrot_speed=20; S8 seek_speed=30; // Idle Behavior Variables bool tone_active=FALSE; U32 tone_timestamp=0; // Follow Line Behavior Variables U8 sampleSize=5; SensorReading light_readings[sampleSize]; SensorReading light_min; SensorReading light_max; // Open-Claws Behavior and Grasp-Object Behavior Variables SensorReading touched; U32 claw_timestamp=0; U32 claw_oldtstamp=0; U32 claw_position=0; U32 claw_oldpos=0; bool claw_closed; U8 claw_movement; // Claw Control Constants // Open and Close Speeds are directional (signed) // WARNING: Be conservative with the Open and Close Speeds, since we // run the motors until it hits the limiters and the Tachometer stops changing. // Using too high a speed may pop the claw assembly! S8 open_speed=10; S8 close_speed=-20; // Move-Object Behavior Variables SensorReading sound_levels[sampleSize]; SensorReading sound_min; SensorReading sound_max; U8 sound_following_state=0; U8 sound_level_trigger=0; // Actuator Port Assignments Actuators actuators; setActuators(&actuators, ARM, RIGHT_WHEEL, LEFT_WHEEL ); // Sensor Port Assignments and configure Sensor lightSensor, touchSensor, soundSensor; //current robot actuator state ActuatorState current_actuation={MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}; //previous robot actuator state ActuatorState previous_actuation={MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}; //behavior_actuations ActuatorState behavior_actuations[num_behaviors]; //array of available states ActuatorState availableStates[14]= { //bbr_behavior_inhibited {MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}, //bbr_idle_on {MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, tone_enabled_freq, tone_enabled_dur}, //bbr_idle_off {MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, tone_silenced_freq, tone_enabled_dur}, //bbr_follow_line_black {MOTOR_INHIBITED_BYTE, FALSE, fwd_speed, FALSE, fwd_speed, FALSE, ROBOT_FWD, tone_silenced_freq, robot_sched_duration}, //bbr_follow_line_edge {MOTOR_INHIBITED_BYTE, FALSE, slowrot_speed, FALSE, stop_speed, FALSE, ROBOT_CCW, tone_silenced_freq, robot_sched_duration}, //bbr_follow_line_white {MOTOR_INHIBITED_BYTE, FALSE, stop_speed, FALSE, fastrot_speed, FALSE, ROBOT_CW, tone_silenced_freq, robot_sched_duration}, // Open Claws {open_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Close Claws (Grasp Object) {close_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Unlock Claws {stop_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Lock Claws {stop_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Move-Object Behavior Actuation Outputs // idle {close_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}, // waiting {close_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // seeking {close_speed, TRUE, seek_speed, TRUE, stop_speed, TRUE, ROBOT_SEEK, tone_silenced_freq, robot_sched_duration}, // follow {close_speed, TRUE, fwd_speed, TRUE, fwd_speed, TRUE, ROBOT_FWD, tone_silenced_freq, robot_sched_duration} }; // // program initialization // nx_proginit(); nx_progtitle(title); nx_systick_wait_ms(1000); // configure sensors configureSensor(&touchSensor, TOUCH, ONE); configureSensor(&soundSensor, SOUND, TWO); configureSensor(&lightSensor, LIGHT, FOUR); light_led_enable(&lightSensor); // Open-Claws Behavior and Grasp-Object Behavior initialization touched.reading.touched=FALSE; // force claw opening on init via Open-Claws behavior claw_closed=TRUE; // claw closed boolean claw_oldpos=0; // Initialize Old Position = 0 (default initial position after nx__motors_init) claw_oldtstamp=0; // Initialize Claw Position Old Timestamp = 0 claw_movement= CLAW_STOPPED; // initialize current tick sched_tick=nx_systick_get_ms(); // initialize behavior_actuations initActuatorStates(behavior_actuations, num_behaviors); while(max_iterations--) { collect_samples(sampleSize, &lightSensor, light_readings, &soundSensor, sound_levels, NULL, NULL, NULL, NULL); calc_min_max(light_readings, sampleSize, &light_min, &light_max); calc_min_max(sound_levels, sampleSize, &sound_min, &sound_max); getSensorReading(&touchSensor, &touched); // concerned about touched or not get_claw_status(actuators, &claw_position, &claw_timestamp); // turn off light to save energy if follow line is not supposed to work if (touched.reading.touched==FALSE) { light_led_enable(&lightSensor); } else { light_led_disable(&lightSensor); } // dispatcher bbr_idle(&tone_timestamp, &tone_active, behavior_actuations, availableStates); bbr_follow_line(&light_min, &light_max, behavior_actuations, availableStates); bbr_open_claws(&claw_movement, &touched, &claw_closed, &claw_timestamp, &claw_oldtstamp, &claw_position, &claw_oldpos, ¤t_actuation,&actuators, behavior_actuations, availableStates); bbr_grasp_object(&claw_movement, &touched, &claw_closed, &claw_timestamp, &claw_oldtstamp, &claw_position, &claw_oldpos, ¤t_actuation, &actuators, behavior_actuations, availableStates); bbr_move_object(&sound_min, &sound_max, &touched, claw_closed, &sound_following_state, &sound_level_trigger, behavior_actuations, availableStates); // arbiters arbiters(num_behaviors, &actuators, ¤t_actuation, behavior_actuations); // actuators controllers(&actuators, ¤t_actuation, &previous_actuation); sleep_robot(&sched_tick, robot_sched_duration); } nx_progshutdown(); }
void main(void) { int num_behaviors=6; // number of behaviors U32 robot_sched_duration=100; // minimum time spent for each main loop (in ms) char *title="Tribot"; // program title to display U32 max_iterations=65536; // maximum number of iterations of main loop U32 sched_tick=0; // store system timer for robot sleep // Tone Control Constants U32 tone_enabled_freq=2500; // Hz U32 tone_silenced_freq=0; // Hz U32 tone_enabled_dur=100; // ms U32 tone_obstacle_freq=750; // Hz // wheel control constants S8 stop_speed=0; S8 fwd_speed=30; S8 fastrot_speed=40; S8 slowrot_speed=20; S8 seek_speed=30; // Idle Behavior Variables bool tone_active=FALSE; U32 tone_timestamp=0; // Follow Line Behavior Variables U8 sampleSize=5; SensorReading light_readings[sampleSize]; SensorReading light_min; SensorReading light_max; // Open-Claws Behavior and Grasp-Object Behavior Variables SensorReading touched; U32 claw_timestamp=0; U32 claw_oldtstamp=0; U32 claw_position=0; U32 claw_oldpos=0; bool claw_closed; U8 claw_movement; // Claw Control Constants // Open and Close Speeds are directional (signed) // WARNING: Be conservative with the Open and Close Speeds, since we // run the motors until it hits the limiters and the Tachometer stops changing. // Using too high a speed may pop the claw assembly! S8 open_speed=10; S8 close_speed=-20; // Move-Object Behavior Variables SensorReading sound_levels[sampleSize]; SensorReading sound_min; SensorReading sound_max; U8 sound_following_state=0; U8 sound_level_trigger=0; // Avoid Object behavior variables bool obstacle_detected=FALSE; bool obstacle_tone_active=FALSE; U32 obstacle_tone_timestamp=0; struct PID_Control pid; SensorReading distance; distance.reading.analog=0; // Actuator Port Assignments Actuators actuators; setActuators(&actuators, ARM, RIGHT_WHEEL, LEFT_WHEEL ); // Sensor Port Assignments and configure Sensor lightSensor, touchSensor, soundSensor, ulsSensor; //current robot actuator state ActuatorState current_actuation={MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}; //previous robot actuator state ActuatorState previous_actuation={MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}; //behavior_actuations ActuatorState behavior_actuations[num_behaviors]; //array of available states ActuatorState availableStates[16]= { //bbr_behavior_inhibited {MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}, //bbr_idle_on {MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, tone_enabled_freq, tone_enabled_dur}, //bbr_idle_off {MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, MOTOR_INHIBITED_BYTE, FALSE, ROBOT_STOP, tone_silenced_freq, tone_enabled_dur}, //bbr_follow_line_black {MOTOR_INHIBITED_BYTE, FALSE, fwd_speed, FALSE, fwd_speed, FALSE, ROBOT_FWD, tone_silenced_freq, robot_sched_duration}, //bbr_follow_line_edge {MOTOR_INHIBITED_BYTE, FALSE, slowrot_speed, FALSE, stop_speed, FALSE, ROBOT_CCW, tone_silenced_freq, robot_sched_duration}, //bbr_follow_line_white {MOTOR_INHIBITED_BYTE, FALSE, stop_speed, FALSE, fastrot_speed, FALSE, ROBOT_CW, tone_silenced_freq, robot_sched_duration}, // Open Claws {open_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Close Claws (Grasp Object) {close_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Unlock Claws {stop_speed, FALSE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Lock Claws {stop_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // Move-Object Behavior Actuation Outputs // idle {close_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, TONE_INHIBITED, TONE_INHIBITED}, // waiting {close_speed, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_STOP, tone_silenced_freq, robot_sched_duration}, // seeking {close_speed, TRUE, seek_speed, TRUE, stop_speed, TRUE, ROBOT_SEEK, tone_silenced_freq, robot_sched_duration}, // follow {close_speed, TRUE, fwd_speed, TRUE, fwd_speed, TRUE, ROBOT_FWD, tone_silenced_freq, robot_sched_duration}, // Avoid-Obstacle Behavior Actuation Outputs // avoid obstacle template, tone on {MOTOR_INHIBITED_BYTE, TRUE, stop_speed, TRUE, stop_speed, TRUE, ROBOT_AVOID, tone_obstacle_freq, tone_enabled_dur}, // avoid obstacle template, tone off {MOTOR_INHIBITED_BYTE, TRUE, MOTOR_INHIBITED_BYTE, TRUE, stop_speed, TRUE, ROBOT_AVOID, tone_silenced_freq, tone_enabled_dur} }; // // program initialization // nx_proginit(); nx_progtitle(title); nx_systick_wait_ms(1000); // configure sensors configureSensor(&touchSensor, TOUCH, ONE); configureSensor(&soundSensor, SOUND, TWO); configureSensor(&ulsSensor, RADAR, THREE); configureSensor(&lightSensor, LIGHT, FOUR); light_led_enable(&lightSensor); // Open-Claws Behavior and Grasp-Object Behavior initialization touched.reading.touched=FALSE; // force claw opening on init via Open-Claws behavior claw_closed=TRUE; // claw closed boolean claw_oldpos=0; // Initialize Old Position = 0 (default initial position after nx__motors_init) claw_oldtstamp=0; // Initialize Claw Position Old Timestamp = 0 claw_movement= CLAW_STOPPED; // Avoid-Obstacle Behavior initialization distance.reading.analog=RADAR_DIST_ERR; // initialize to unknown distance obstacle_tone_active=FALSE; obstacle_detected=FALSE; // PID Configuration Parameters // FIXME: Need to be changed to actual values // KP, KI, KD have the range of 16-bit integer values [MIN_SHORT,MAX_SHORT] // This is multiplied by alpha to give 32-bit resolution values // alphaKP, alphaKI, alphaKD BEFORE initializing the PID Controller // The value of alpha is 2^SCALING_SHIFT, where SCALING_SHIFT is 16 // which gives alpha = 65536 // // Applying Kc = 3, T = 100 ms, Pc = 1 s, alpha = 65536 //#define KP 127795 // alphaKP = alpha x 0.65 x Kc = 127795 //#define KI 25559 // alphaKI = alpha x Kp x T / (0.5 x Pc) = 25559 //#define KD 153354 // alphaKD = alpha x Kp x 0.12 x Pc / T = 153354 //#define STEADY_STATE_THRESHOLD 1 // 0 = disabled, !0 = Stop PID Controller after x iterations // init pid initPID(&pid, 127795, 25559, 153354, 1); // initialize current tick sched_tick=nx_systick_get_ms(); // initialize behavior_actuations initActuatorStates(behavior_actuations, num_behaviors); while(max_iterations--) { collect_samples(sampleSize, &lightSensor, light_readings, &soundSensor, sound_levels, NULL, NULL, NULL, NULL); calc_min_max(light_readings, sampleSize, &light_min, &light_max); calc_min_max(sound_levels, sampleSize, &sound_min, &sound_max); getSensorReading(&touchSensor, &touched); // concerned about touched or not getSensorReading(&ulsSensor, &distance); // concerned about range in cm get_claw_status(actuators, &claw_position, &claw_timestamp); // turn off light to save energy if follow line is not supposed to work if (touched.reading.touched==FALSE) { light_led_enable(&lightSensor); } else { light_led_disable(&lightSensor); } // dispatcher bbr_idle(&tone_timestamp, &tone_active, behavior_actuations, availableStates); bbr_follow_line(&light_min, &light_max, behavior_actuations, availableStates); bbr_open_claws(&claw_movement, &touched, &claw_closed, &claw_timestamp, &claw_oldtstamp, &claw_position, &claw_oldpos, ¤t_actuation,&actuators, behavior_actuations, availableStates); bbr_grasp_object(&claw_movement, &touched, &claw_closed, &claw_timestamp, &claw_oldtstamp, &claw_position, &claw_oldpos, ¤t_actuation, &actuators, behavior_actuations, availableStates); bbr_move_object(&sound_min, &sound_max, &touched, claw_closed, &sound_following_state, &sound_level_trigger, behavior_actuations, availableStates); bbr_avoid_obstacle(&obstacle_detected, &distance, &obstacle_tone_timestamp, &obstacle_tone_active, &pid, &actuators, behavior_actuations, availableStates); // arbiters arbiters(num_behaviors, &actuators, ¤t_actuation, behavior_actuations); // actuators controllers(&actuators, ¤t_actuation, &previous_actuation); sleep_robot(&sched_tick, robot_sched_duration); } nx_progshutdown(); }