/*
* Entry routine to this whole set of contouring module.
*/
struct gnuplot_contours *
contour(int num_isolines, struct iso_curve *iso_lines)
{
int i;
int num_of_z_levels; /* # Z contour levels. */
double *zlist;
poly_struct *p_polys, *p_poly;
edge_struct *p_edges, *p_edge;
double z = 0;
double z0 = 0;
double dz = 0;
struct gnuplot_contours *save_contour_list;
/* HBB FIXME 20050804: The number of contour_levels as set by 'set
* cnrparam lev inc a,b,c' is almost certainly wrong if z axis is
* logarithmic */
num_of_z_levels = contour_levels;
interp_kind = contour_kind;
contour_list = NULL;
/*
* Calculate min/max values :
*/
calc_min_max(num_isolines, iso_lines,
&x_min, &y_min, &z_min, &x_max, &y_max, &z_max);
/*
* Generate list of edges (p_edges) and list of triangles (p_polys):
*/
gen_triangle(num_isolines, iso_lines, &p_polys, &p_edges);
crnt_cntr_pt_index = 0;
if (contour_levels_kind == LEVELS_AUTO) {
if (nonlinear(&Z_AXIS)) {
z_max = eval_link_function(Z_AXIS.linked_to_primary, z_max);
z_min = eval_link_function(Z_AXIS.linked_to_primary, z_min);
}
dz = fabs(z_max - z_min);
if (dz == 0)
return NULL; /* empty z range ? */
/* Find a tic step that will generate approximately the
* desired number of contour levels. The "* 2" is historical.
* */
dz = quantize_normal_tics(dz, ((int) contour_levels + 1) * 2);
z0 = floor(z_min / dz) * dz;
num_of_z_levels = (int) floor((z_max - z0) / dz);
if (num_of_z_levels <= 0)
return NULL;
}
/* Build a list of contour levels */
zlist = gp_alloc(num_of_z_levels * sizeof(double), NULL);
for (i = 0; i < num_of_z_levels; i++) {
switch (contour_levels_kind) {
case LEVELS_AUTO:
z = z0 + (i+1) * dz;
z = CheckZero(z,dz);
if (nonlinear(&Z_AXIS))
z = eval_link_function((&Z_AXIS), z);
break;
case LEVELS_INCREMENTAL:
z = contour_levels_list[0] + i * contour_levels_list[1];
break;
case LEVELS_DISCRETE:
z = contour_levels_list[i];
break;
}
zlist[i] = z;
}
/* Sort the list high-to-low if requested */
if (contour_sortlevels)
qsort(zlist, num_of_z_levels, sizeof(double), reverse_sort);
/* Create contour line for each z value in the list */
for (i = 0; i < num_of_z_levels; i++) {
z = zlist[i];
contour_level = z;
save_contour_list = contour_list;
gen_contours(p_edges, z, x_min, x_max, y_min, y_max);
if (contour_list != save_contour_list) {
contour_list->isNewLevel = 1;
/* Nov-2011 Use gprintf rather than sprintf so that LC_NUMERIC is used */
gprintf(contour_list->label, sizeof(contour_list->label),
contour_format, 1.0, z);
contour_list->z = z;
}
}
/* Free all contouring related temporary data. */
free(zlist);
while (p_polys) {
p_poly = p_polys->next;
free(p_polys);
p_polys = p_poly;
}
while (p_edges) {
p_edge = p_edges->next;
free(p_edges);
p_edges = p_edge;
}
return contour_list;
}
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();
}
