/* Snakebot brain board code. RCN started 4/25/2017 Modified 12/16/2017 -- light signals changed Modified 12/19/2017 -- Fixed IR sensor reads Modified 12/19/2017 -- Added steering test Yirgatest (modifications on 12/21 got lost somehow) 12/23/2017 -- Modified to steer away from wall-like obstacles encountered at shallow angle 12/26/2017 -- Slight code fix. Now can go down hall without hitting walls. 12/26/2017 -- Made application of steering bias gradual to smooth out motion 12/27/2017 -- Mods to support eventual better navigation 12/27/2017 -- Added support for ultrasonic sensor */ #include #include #define SERVO1_PIN 3 Servo servo1; // Servo command positions in PWM microseconds. 1500 = center/straight. // On gearbox, 1 degree = about 10.36 usec, 10 usec = about .964 degrees, +-1%. // Another measurement gave 300us = 28.5 degrees, about the same. #define CENTER_POS 1500 #define MAX_POS 1750 #define MIN_POS 1250 int cur_pos = CENTER_POS; int new_pos = CENTER_POS; int cur_direction = 1; #define INT_LED 13 #define RED_LED 5 #define YELLOW_LED 6 #define GREEN_LED 7 #define BLUE_LED 8 // Set up SoftwareSerial port for forward movement // Turns out software serial is unreliable with this program. // Individual bits seem to fail randomly on one in 10-20 transfers. // Possibly due to interference between interrupts and all the timing commands. //#define rx_fwd 14 // (MISO) //#define tx_fwd 15 // (SCK) //SoftwareSerial fwd_serial(rx_fwd, tx_fwd); byte serial_in_lsb; byte serial_in_msb; byte serial_out_lsb; byte serial_out_msb; #define SYNCH_PIN A0 // Broadcast synch signal from brain if we need it #define COMMAND_PIN1 A1 // fwd/stop/adjust etc. #define COMMAND_PIN2 A2 #define COMMAND_PIN3 A3 // Commands // These represent the raw bits of the command lines, and cannot be arbitrarily changed. #define CMD_STOP_FWD 0 // 000 #define CMD_STOP_REV 1 // 001 #define CMD_ADJUST_FWD 2 // 010 #define CMD_ADJUST_REV 3 // 011 #define CMD_MOVE_FWD 4 // 100 #define CMD_MOVE_REV 5 // 101 #define CMD_NO_INPUT 7 // 111 All channels pullup when no signal is present int cur_command = CMD_STOP_FWD; int prev_command = CMD_STOP_FWD; unsigned long command_stable = 0; // How many cycles has current command been stable // States #define ST_WAIT 10 #define ST_LOAD_STRAIGHT 20 #define ST_LOAD_SINUSOID 21 #define ST_ADJUST 30 #define ST_SLITHER_FWD 40 #define ST_SLITHER_RIGHT 41 #define ST_SLITHER_LEFT 42 #define ST_BACKUP 50 unsigned long wait_count = 0; unsigned long load_count = 0; unsigned long adjust_count = 0; unsigned long fwd_count = 0; unsigned long right_count = 0; unsigned long left_count = 0; unsigned long backup_count = 0; bool sinusoid_loaded = false; bool straight_loaded = false; // More state stuff int current_state = ST_WAIT; unsigned long current_state_count = 0; //#define ST_STOP_FWD 10 //#define ST_STOP_REV 20 //#define ST_ADJUST_FWD 30 //#define ST_ADJUST_REV 40 //#define ST_MOVE_FWD 50 //#define ST_MOVE_REV 60 //#define ST_FWD_TO_REV 70 //#define STREV_TO_FWD 80 // Heartbeat timing #define HEARTBEAT_USEC 20000ul // 20 milliseconds = 50Hz unsigned long loop_start_usec; unsigned long work_done_usec; unsigned long usec_used; unsigned long delay_usec; unsigned long delay_ms; unsigned int remainder_usec; unsigned int count1200 = 0; unsigned int count160 = 0; unsigned int count50 = 0; unsigned int count5 = 0; unsigned int count2 = 0; // Joint angles are held in a pre-computed array representing // an approximate sinusoid. By shifting through this table, // a wave is propagated down the snake. // Biasing the values up or down will produce a left or right turn respectively // Initially, starting at index 0, the head turns towards the right. // Snake makes about 2 feet of progress per cycle. // Movement left and right of center is 4-5 inches. Adding a couple inches // on each side gives a swath of a little over a foot. #define WAVE_SIZE 160 #define CENTER_INDEX_L 0 // Head points left of direction of travel, center of swath #define LEFTMOST_INDEX 40 // Head points in direction of travel, left side of swath #define CENTER_INDEX_R 80 // Head points right of direction of travel, center of swath #define RIGHTMOST_INDEX 120 // Head points in direction of travel, right side of swath int wave_array[WAVE_SIZE]; int wave_count = 0; int wave_index = 0; int n_loads = 0; // Turning is accomplished by biasing the wave_array values values up or down. // (+) turns left, (-) right. // Empirically, a bias of 40us (~4 degrees) maintained over a full cycle (16 segments), // changes the course by about 40 degrees (not 64 - there seems to be slippage etc.) // 60 usec bias gives 60 degrees per cycle. // 80 usec gives 80-90 degrees per cycle, and is mechanical maximum. int trim_bias = 0; // For correcting any intrinsic turning tendency. int steering_bias = 0; int steering_bias_goal = 0; int steering_countdown = 0; // must get to 0 before steering bias can be changed again // Touch sensors in tongue #define TONGUE_LEFT_PIN A4 #define TONGUE_RIGHT_PIN A5 // Read gives a value from 0 to 1023. Voltage increases from // about 1/2 Vcc in neutral state to about 2/3 Vcc when bent. // Set threshold at about 1/4 of this difference (1/24 of total range) // or 1024/24 ~= 40 // Neutral values (means) are measured at startup. #define TONGUE_CONTACT_THRESH 40 int tongue_left_mean = 0; int tongue_right_mean = 0; int tongue_left_diff = 0; int tongue_right_diff = 0; bool tongue_left_contact = false; bool tongue_right_contact = false; // Left and right infrared sensors. Good out to about 80cm // Value declines non-linearly as distance increases. #define IR_LEFT_PIN A9 // Pin 9 #define IR_RIGHT_PIN A10 // Pin 10 #define IR_10_CM 450 // on 0-1023 scale #define IR_15_CM 330 #define IR_20_CM 260 #define IR_30_CM 180 #define IR_50_CM 130 #define IR_60_CM 100 #define IR_80_CM 80 int ir_left_val = 0; int ir_right_val = 0; int ir_val_leftmost = 0; // Keep track last readings at leftmost and rightmost head tilt int ir_val_rightmost = 0; // Gives an idea of distance to walls/obstacles left and right // Forward HC-SR04 ultrasonic sensor // #define USOUND_TRIGGER_PIN 11 #define USOUND_ECHO_PIN 12 unsigned long usound_delay_usec = 50000L; int usound_range_cm = 345; int usound_flash_interval = 50; // in heartbeat cycles int usound_counter = 0; // User input buttons #define BUTTON1_PIN 2 #define BUTTON2_PIN 4 int button1_cur_state = LOW; int button1_prev_state = LOW; int button2_cur_state = LOW; int button2_prev_state = LOW; bool button1_signal = false; bool button2_signal = false; //---------------------------------------------------------------------------------------------------- // the setup function runs once when you press reset or power the board void setup() { int i,j,k; long ilong; float joint_angle[160]; float i_to_radians = 2.0 * 3.14159 / 160.0; float radians_to_usec = 10.0 * 360.0 / (2.0 * 3.14159); // about 10 usec/deg pinMode(INT_LED, OUTPUT); pinMode(RED_LED, OUTPUT); pinMode(YELLOW_LED, OUTPUT); pinMode(GREEN_LED, OUTPUT); pinMode(BLUE_LED, OUTPUT); pinMode(COMMAND_PIN1, OUTPUT); pinMode(COMMAND_PIN2, OUTPUT); pinMode(COMMAND_PIN3, OUTPUT); pinMode(SYNCH_PIN, OUTPUT); pinMode(BUTTON1_PIN, INPUT); pinMode(BUTTON2_PIN, INPUT); pinMode(TONGUE_LEFT_PIN, INPUT); pinMode(TONGUE_RIGHT_PIN, INPUT); pinMode(IR_LEFT_PIN, INPUT); pinMode(IR_RIGHT_PIN, INPUT); pinMode(USOUND_TRIGGER_PIN, OUTPUT); pinMode(USOUND_ECHO_PIN, INPUT); // Initialize slither drive tables // Cycle in 160 steps (16 segments) yielding approximate sinusoid // with inflection angle of 45 degrees. // center is at 0 and 80, , right-most at 40, left-most at 120 i_to_radians = 2.0 * 3.14159 / 160.0; for(i = 0; i < 160; i++) { joint_angle[i] = atan(cos(i_to_radians * (float)(i + 5))) - atan(cos(i_to_radians * (float)(i - 5))) ; wave_array[i] = 1500 + (int)(radians_to_usec * joint_angle[i]); if(wave_array[i] > 1800) wave_array[i] = 1800; // safety if(wave_array[i] < 1200) wave_array[i] = 1200; } } //---------------------------------------------------------------------------------------------------------------------- // the loop function runs over and over again forever void loop() { int i, j, k; // initialization/startup stuff // Do fancy startup light-blinking stuff digitalWrite(BLUE_LED, HIGH); delay(100); digitalWrite(BLUE_LED, LOW); delay(150); digitalWrite(BLUE_LED, HIGH); delay(100); digitalWrite(BLUE_LED, LOW); delay(150); digitalWrite(BLUE_LED, HIGH); delay(100); digitalWrite(BLUE_LED, LOW); delay(150); digitalWrite(YELLOW_LED, HIGH); delay(3000); digitalWrite(YELLOW_LED, LOW); delay(250); digitalWrite(GREEN_LED, HIGH); delay(100); digitalWrite(GREEN_LED, LOW); delay(150); digitalWrite(GREEN_LED, HIGH); delay(100); digitalWrite(GREEN_LED, LOW); delay(150); digitalWrite(GREEN_LED, HIGH); delay(100); digitalWrite(GREEN_LED, LOW); delay(150); digitalWrite(RED_LED, HIGH); delay(3000); digitalWrite(RED_LED, LOW); delay(250); // Sample the tongue input voltages over a second to get setpoints tongue_left_mean = 0; tongue_right_mean = 0; for( i = 0; i < 8; i++) { tongue_left_mean += analogRead(TONGUE_LEFT_PIN); tongue_right_mean += analogRead(TONGUE_RIGHT_PIN); delay(125); } tongue_left_mean = tongue_left_mean / 8; tongue_right_mean= tongue_right_mean / 8; // Attach servos servo1.attach(SERVO1_PIN, 900, 2100); // Range of HiTec HS-5070MH servo ?? delay(50); // Set up the built-in serial port // For micro, calls must be to Serial1 to access Rx and Tx pins Serial1.begin(115200); while(!Serial1); // Set up SoftwareSerial port for forward movement // Which turns out to be unreliable, possibly due to dense timing calls //pinMode(rx_fwd, INPUT_PULLUP); // Attempt to avoid random reads from floating rx connection //pinMode(tx_fwd, OUTPUT); //fwd_serial.begin(57600); // Max supported baud rate //fwd_serial.listen(); // Empty buffer and start listening for input on the fwd_serial port //while(!fwd_serial.isListening()); // Loop until port activates wave_count = 0; count1200 = 0; count160 = 0; count50 = 0; count5 = 0; count2 = 0; new_pos = 1500; current_state = ST_WAIT; while(true) // ***** Start local infinite loop running at about 50 Hz ***** { loop_start_usec = micros(); // ------------------------------------------------------------------------ // LED state signalling. // For ease in coding LED signals, shut them all off. Some will be turned back on // almost immediately, in which case the flicker is not visible to the eye. digitalWrite(RED_LED, LOW); digitalWrite(YELLOW_LED, LOW); digitalWrite(GREEN_LED, LOW); digitalWrite(BLUE_LED, LOW); // Flash double red (lub-dub) once per second (50, 20ms beats) if(count50 >= 0 && count50 < 5) digitalWrite(RED_LED, HIGH); if(count50 >= 13 && count50 < 18) digitalWrite(RED_LED, HIGH); // Turn on yellow LED if tongue is in contact with an object if(tongue_left_contact || tongue_right_contact) digitalWrite(YELLOW_LED, HIGH); // Turn on blue and green LEDs to indicate object sensed by left or right IR sensor //if(ir_left_val > IR_20_CM) digitalWrite(BLUE_LED, HIGH); //if(ir_right_val > IR_20_CM) digitalWrite(GREEN_LED, HIGH); // Turn on blue LED to indicate preload in progress if(current_state == ST_LOAD_STRAIGHT || current_state == ST_LOAD_SINUSOID) digitalWrite(BLUE_LED, HIGH); // Flash yellow ~once per second to indicate forward mode if(cur_command == CMD_MOVE_FWD) { if(count50 >= 10 && count50 < 15) digitalWrite(YELLOW_LED, HIGH); } // FLASH fast yellow pattern (5x per second) to indicate adjust mode if(cur_command == CMD_ADJUST_FWD) { if(count50 >= 0 && count50 < 5 ) digitalWrite(YELLOW_LED, HIGH); if(count50 >= 10 && count50 < 15) digitalWrite(YELLOW_LED, HIGH); if(count50 >= 20 && count50 < 25) digitalWrite(YELLOW_LED, HIGH); if(count50 >= 30 && count50 < 35) digitalWrite(YELLOW_LED, HIGH); if(count50 >= 40 && count50 < 45) digitalWrite(YELLOW_LED, HIGH); } // Flash green ~once per second to indicate right steer //if(steering_bias >= 5) //{ // if(count50 >= 10 && count50 < 15) digitalWrite(GREEN_LED, HIGH); //} // Flash blue ~once per second to indicate left steer //if(steering_bias <= -5) //{ // if(count50 >= 10 && count50 < 15) digitalWrite(BLUE_LED, HIGH); //} // Flash blue with variable period to indicate distance detected by ultrasound sensor // Testing only if(usound_counter == 0) digitalWrite(BLUE_LED, HIGH); //-------------------------------------------------------------------------------- // Gracefully update steering bias if(steering_bias_goal > steering_bias) { if(steering_bias_goal - steering_bias <= 10) steering_bias = steering_bias_goal; else steering_bias += 10; } else if(steering_bias_goal < steering_bias) { if(steering_bias - steering_bias_goal <= 10) steering_bias = steering_bias_goal; else steering_bias -= 10; } if(steering_countdown > 0) steering_countdown--; else steering_countdown = 0; // Read various sensors // Also in state routines at the moment. // Extra reads to tongue and ir are non-problematic, but an extra, untested // call of read_buttons() can cause a press-release to be missed //read_tongue(); // Commented out for usound test, which works, of the snake... //read_ir(); if(count5 == 0) read_usound(); // Every 5 cycles //read_buttons(); // Branch on current state if(current_state == ST_WAIT) do_wait(); else if(current_state == ST_LOAD_STRAIGHT) do_load_straight(); else if(current_state == ST_LOAD_SINUSOID) do_load_sinusoid(); else if(current_state == ST_ADJUST) do_adjust(); else if(current_state == ST_SLITHER_FWD) do_slither_fwd(); write_command(); // Should not be needed, but here for backup/debugging // Wait until time to start next heartbeat cycle work_done_usec = micros(); if(work_done_usec > loop_start_usec) usec_used = work_done_usec - loop_start_usec; else usec_used = (0xFFFFFFFFul - loop_start_usec) + work_done_usec; // usec counter loops after ~70 min if(usec_used >= HEARTBEAT_USEC) delay_usec = 0; else delay_usec = (HEARTBEAT_USEC - usec_used); if(delay_usec < 15000) delayMicroseconds((unsigned int)delay_usec); else { delay_ms = delay_usec/1000; remainder_usec = (unsigned int)(delay_usec - (delay_ms * 1000)); delay(delay_ms); // because delayMicroseconds does not work for values > 16383 delayMicroseconds(remainder_usec); } count1200++; if(count1200 >= 1200) count1200 = 0; count160++; if(count160 >= 160) count160 = 0; count50++; if(count50 >= 50) count50 = 0; count5++; if(count5 >= 5) count5 = 0; count2++; if(count2 >= 2) count2 = 0; usound_counter++; if(usound_counter >= usound_flash_interval) usound_counter = 0; } // End local infinite loop } // End loop() function //---------------------------------------------------------------------------------------------- //********************************************************************************************** //---------------------------------------------------------------------------------------------- void write_command() // Writes the current command to the control lines // Refers to global variable cur_command { if(cur_command & 0x1) digitalWrite(COMMAND_PIN1, HIGH); else digitalWrite(COMMAND_PIN1, LOW); if((cur_command >> 1) & 0x1) digitalWrite(COMMAND_PIN2, HIGH); else digitalWrite(COMMAND_PIN2, LOW); if((cur_command >> 2) & 0x1) digitalWrite(COMMAND_PIN3, HIGH); else digitalWrite(COMMAND_PIN3, LOW); } //---------------------------------------------------------------------------------------------- void read_buttons() // Looks for press-release on buttons // Note that second call in a cycle will erase previous result { button1_prev_state = button1_cur_state; button1_cur_state = digitalRead(BUTTON1_PIN); if(button1_prev_state == HIGH && button1_cur_state == LOW) button1_signal = true; else button1_signal = false; button2_prev_state = button2_cur_state; button2_cur_state = digitalRead(BUTTON2_PIN); if(button2_prev_state == HIGH && button2_cur_state == LOW) button2_signal = true; else button2_signal = false; } void read_tongue() // Reads the tongue flex sensors { int tongue_left_val, tongue_right_val; tongue_left_val = analogRead(TONGUE_LEFT_PIN); tongue_left_diff = tongue_left_val - tongue_left_mean; if(tongue_left_diff > TONGUE_CONTACT_THRESH) tongue_left_contact = true; else { tongue_left_contact = false; // leaky integrator dynamic update of mean if needed //tongue_left_mean = (15 * tongue_left_mean + tongue_left_val) /16; } tongue_right_val = analogRead(TONGUE_RIGHT_PIN); tongue_right_diff = tongue_right_val - tongue_right_mean; if(tongue_right_diff > TONGUE_CONTACT_THRESH) tongue_right_contact = true; else { tongue_right_contact = false; //tongue_right_mean = (15 * tongue_right_mean + tongue_right_val) /16; } } void read_ir() // Reads the IR sensors { ir_left_val = analogRead(IR_LEFT_PIN); // Take best shot at sensing wall/barrier to left if(wave_count == CENTER_INDEX_L) ir_val_leftmost = ir_left_val; // Take best shot at sensing wall/barrier to right ir_right_val = analogRead(IR_RIGHT_PIN); if(wave_count == CENTER_INDEX_R) ir_val_rightmost = ir_right_val; } void read_usound() // Triggers and reads the HC-SR04 untrasonic senosr { // Write trigger pulse digitalWrite(USOUND_TRIGGER_PIN, LOW); delayMicroseconds(5); digitalWrite(USOUND_TRIGGER_PIN, HIGH); delayMicroseconds(10); digitalWrite (USOUND_TRIGGER_PIN, LOW); delayMicroseconds(5); // Read delay // Default timeout for pulseIn() call is 1 s (1,000,000 us). // With a 20 ms heartbeat, we don't want to wait that long so we // add a custom timeout, here 50 ms, about 16m (8m out and back) // flight time, and pulse only every 5 cycles (100 ms) // Tryng to get things to work, but so far no luck // Works for short distances, but longest return is 5/8m; about 3200us usound_delay_usec = pulseIn(USOUND_ECHO_PIN, HIGH, 50000L); // Returns 0 on timeout. if(usound_delay_usec == 0) usound_delay_usec = 50000L; // Compute range in cm usound_range_cm = usound_delay_usec / 58; // Approximate distance at 29us/cm // variable flash period for testing purposes if(usound_range_cm > 200) usound_flash_interval = 100; else if(usound_range_cm < 10) usound_flash_interval = 5; else usound_flash_interval = usound_range_cm / 2; } //-------------------------------------------------------------------------------------------------------------------- // Routines implementing differential details of various states void do_wait() { if(current_state != ST_WAIT) return; // If we got here by mistake... current_state_count++; wait_count++; cur_command = CMD_STOP_FWD; write_command(); // Check for button presses (human input) // Handle state transitions // Button1 invokes linear load and preform, // Button2 invokes slither forward if(!button1_signal && !button2_signal) return; // we are done if(button1_signal) { if(!straight_loaded) // { current_state = ST_LOAD_STRAIGHT; current_state_count = 0; load_count = 0; n_loads = 0; sinusoid_loaded = false; straight_loaded = false; // Redundant at the moment... return; } else // if(straight_loaded), adjust to straight position { current_state = ST_ADJUST; current_state_count = 0; adjust_count = 0; wave_count = 0; return; } } else if(button2_signal) // start slithering { current_state = ST_SLITHER_FWD; current_state_count = 0; fwd_count = 0; // wave_count = 0; // keep old wave count if nothing else has happened straight_loaded = false; return; } } void do_load_straight() { if(current_state != ST_LOAD_STRAIGHT) return; // If we got here by mistake... current_state_count++; load_count++; cur_command = CMD_STOP_FWD; write_command(); // Transmit center position value to built-in serial port // It takes 2 bytes to transmit a position (900 - 2100). // They are transmitted LSB (least significant byte) first. serial_out_lsb = (byte)(CENTER_POS & 0xFF); serial_out_msb = (byte)((CENTER_POS >> 8) & 0xFF); Serial1.write(serial_out_lsb); Serial1.write(serial_out_msb); n_loads++; if(n_loads >= WAVE_SIZE + 80) // Go back to wait when done { current_state = ST_WAIT; current_state_count = 0; wait_count = 0; straight_loaded = true; sinusoid_loaded = false; return; } else { return; // stay in load_straight } } void do_load_sinusoid() { int load_pos; if(current_state != ST_LOAD_SINUSOID) return; // If we got here by mistake... current_state_count++; load_count++; cur_command = CMD_STOP_FWD; write_command(); // Transmit wave aray value to built-in serial port // It takes 2 bytes to transmit a position (900 - 2100). // They are transmitted LSB (least significant byte) first. load_pos = wave_array[wave_index]; wave_index++; if(wave_index >= WAVE_SIZE) wave_index = 0; // cycle serial_out_lsb = (byte)(load_pos & 0xFF); serial_out_msb = (byte)((load_pos >> 8) & 0xFF); Serial1.write(serial_out_lsb); Serial1.write(serial_out_msb); n_loads++; if(n_loads >= WAVE_SIZE + 80) // Go back to wait when done { current_state = ST_WAIT; current_state_count = 0; wait_count = 0; straight_loaded = false; sinusoid_loaded = true; } } void do_adjust() // Adjust process is occasionally buggy for some reason // Likely something in the joint code. // Sometimes freezes out command reception, requiring board restart... { if(current_state != ST_ADJUST) return; // If we got here by mistake... current_state_count++; adjust_count++; cur_command = CMD_ADJUST_FWD; write_command(); // Check for button presses (human input) // and handle state transitions if(!button1_signal && !button2_signal) return; // we are done // return to wait when user instructs. // eventually want to time out as well... else if(button1_signal || button2_signal) //redundant { current_state = ST_WAIT; current_state_count = 0; wait_count = 0; wave_count = 0; // We've adjusted, so old position is meaningless return; } } void do_slither_fwd() // Get next servo position (several) times a second and push to serial port { int out_index, out_pos, servo_com; if(current_state != ST_SLITHER_FWD) return; // If we got here by mistake... current_state_count++; fwd_count++; cur_command = CMD_MOVE_FWD; write_command(); new_pos = wave_array[wave_count] + trim_bias + steering_bias; out_index = wave_count - 10; if(out_index < 0) out_index += WAVE_SIZE; wave_count++; if(wave_count >= WAVE_SIZE) wave_count = 0; // Set head servo to new position cur_pos = new_pos; servo_com = 1500 + (1500 - cur_pos); // head servo is inverted in our convention servo1.writeMicroseconds(servo_com); // Transmit out position value to built-in serial port // It takes 2 bytes to transmit a position (900 - 2100). // They are transmitted LSB (least significant byte) first. out_pos = wave_array[out_index] + trim_bias + steering_bias; serial_out_lsb = (byte)(out_pos & 0xFF); serial_out_msb = (byte)((out_pos >> 8) & 0xFF); Serial1.write(serial_out_lsb); Serial1.write(serial_out_msb); // Handle updates and state transitions // Check for button presses (human input) // If either button is pushed, stop and wait if(button1_signal || button2_signal) { steering_bias = 0; steering_bias_goal = 0; steering_countdown = 0; current_state = ST_WAIT; current_state_count = 0; wait_count = 0; return; } // Check for collision (sensor read called in main loop) // If the tongue has touched something, stop and wait if(tongue_left_contact || tongue_right_contact) { steering_bias = 0; steering_bias_goal = 0; steering_countdown = 0; current_state = ST_WAIT; current_state_count = 0; wait_count = 0; return; } // Check side-looking IR sensors. (sensor read called in main loop) // If an obstacle is detected within 20 cm on both right or left sides, stop. if(ir_left_val >= IR_20_CM && ir_right_val >= IR_20_CM) // Obstacles on both sides. Stop { steering_bias = 0; steering_bias_goal = 0; steering_countdown = 0; current_state = ST_WAIT; current_state_count = 0; wait_count = 0; return; } // Otherwise, if a turn is not currently being executed // and an obstacle is detected right or left, // initiate a turn away from the obstacle. // A bias of 40 for 60 cycles produces a turn of about 15 degrees. if(steering_countdown <= 0) { if(ir_left_val >= IR_20_CM && steering_countdown <= 0) // obstacle left { steering_bias_goal = -40; steering_countdown = 60; } else if(ir_right_val >= IR_20_CM && steering_countdown <= 0 ) // obstacle right { // Bias angle leftward by 40 usec steering_bias_goal = 40; steering_countdown = 60; } else // Turn off the bias when the turn is competed { steering_bias_goal = 0; } } // Keep on slithering { return; } }