task main() { waitForStart(); //wait1Msec(20000); nMotorEncoder[Right] = 0; Forward(3100,70); nMotorEncoder[mLift] = 0; while(nMotorEncoder[mLift] < 3800) { motor[mLift] = 50; } motor[mLift] = 0; Forward(700,70); wait1Msec(1000); Turn(720,100,right); while (getSensorReading()>2){ motor[Right] = 70; motor[Left] = 70; } motor[Right]=0; motor[Left]=0; motor[mDispenser] = -50; wait1Msec(200); motor[mDispenser] = 50; wait1Msec(200); motor[mDispenser] = 0; wait1Msec(500); Forward(5700,100); motor[mIntake] = 100; wait10Msec(10); motor[mIntake] = 0; while (getSensorReading>2){ motor[Right] = 70; motor[Left] = 70; } motor[Right]=0; motor[Left]=0; motor[mDispenser] = -50; wait1Msec(200); motor[mDispenser] = 50; wait1Msec(200); motor[mDispenser] = 0; wait1Msec(500); Backward(1700,70); wait1Msec(500); }
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(); }