Permalink
Fetching contributors…
Cannot retrieve contributors at this time
921 lines (838 sloc) 25.4 KB
/*****************************************************************************
* ART CONTROLLER
*
* Copyright 2011 Ricky Ng-Adam
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/*****************************************************************************
* ARDUINO LIBS
*****************************************************************************/
#include <Servo.h>
#include <limits.h>
/*****************************************************************************
* NY LIBS
*****************************************************************************/
#include "types.h"
#include "Ultrasonic.h"
/*****************************************************************************
* LOGGING
*****************************************************************************/
enum telemetry_types {
ULTRASONIC_SENSORS,
STATE_CHANGE,
SENSOR_SERVO_POSITION_CHANGE,
CAR_GO_COMMAND,
CAR_SUSPEND_COMMAND,
MAX_TELEMETRY_TYPE,
};
/*char* telemetry_types_names[] = {
"ULTRASONIC_SENSORS",
"STATE_CHANGE",
"SENSOR_SERVO_POSITION_CHANGE",
"CAR_GO_COMMAND",
"CAR_SUSPEND_COMMAND",
};*/
#define DEBUG
#define TELEMETRY
#define ERROR_SERIAL_LOGGING
#undef TRACE_SERIAL_LOGGING
#ifdef DEBUG
#define DEBUG_PRINT(str) \
Serial.print(millis()); \
Serial.print(": "); \
Serial.print(__FUNCTION__); \
Serial.print(':'); \
Serial.print(__LINE__); \
Serial.print(' '); \
Serial.println(str);
#else
#define DEBUG_PRINT(str)
#endif
#ifdef TELEMETRY
#define LOG_TELEMETRY(type, source, value) \
Serial.print("|"); \
Serial.print(millis()); \
Serial.print(","); \
Serial.print((int)type); \
Serial.print(","); \
Serial.print((int)source); \
Serial.print(","); \
Serial.println((int)value);
#else
#define LOG_TELEMETRY(type, source, value)
#endif
#ifdef ERROR_SERIAL_LOGGING
#define LOG_BAD_STATE(value) \
Serial.print(millis()); \
Serial.print(": "); \
Serial.print(__FUNCTION__); \
Serial.print(':'); \
Serial.print(__LINE__); \
Serial.print(' '); \
Serial.println((int)value);
#define LOG_ERROR(message, context, expected, actual) \
Serial.print("ERROR:"); \
Serial.print(millis()); \
Serial.print(":"); \
Serial.print(message); \
Serial.print(" context:"); \
Serial.print((int)context); \
Serial.print(" expected:"); \
Serial.print((int)expected); \
Serial.print(" actual:"); \
Serial.println((int)actual);
#else
#define LOG_ERROR(message, source, expected, actual)
#define LOG_BAD_STATE(value)
#endif
#ifdef TRACE_SERIAL_LOGGING
#define LOG_TRACE4(message1, value1, message2, value2) \
Serial.print("TRACE:"); \
Serial.print(millis()); \
Serial.print(":"); \
Serial.print(message1); \
Serial.print((int)value1); \
Serial.print(message2); \
Serial.println((int)value2);
#define LOG_TRACE2(message1, value1) \
Serial.print("TRACE:"); \
Serial.print(millis()); \
Serial.print(":"); \
Serial.print(message1); \
Serial.println((int)value1);
#else
#define LOG_TRACE4(message1, value1, message2, value2)
#define LOG_TRACE2(message1, value1)
#endif
/*****************************************************************************
* ARDUINO PINS MAPPING
*****************************************************************************/
// ultrasonic
pin_t ULTRASONIC_REVERSE_TRIG = 2;
pin_t ULTRASONIC_REVERSE_ECHO = 3;
pin_t ULTRASONIC_FORWARD_TRIG = 4;
pin_t ULTRASONIC_FORWARD_ECHO = 5;
// various directions we can go to
pin_t FORWARD_PIN = 6; // the number of the LED pin
pin_t REVERSE_PIN = 7;
pin_t LEFT_PIN = 8;
pin_t RIGHT_PIN = 9;
pin_t PUSHBUTTON_PIN = 10;
// servo
pin_t SENSOR_SERVO_PIN = 12;
/*****************************************************************************
* TIMING RELATED
*****************************************************************************/
enum waits_types {
WAIT_FOR_SERVO_TO_TURN,
WAIT_FOR_ECHO,
WAIT_FOR_ROBOT_TO_MOVE,
WAIT_FOR_BUTTON_REREAD,
WAIT_ARRAY_SIZE, // always last...
};
time_ms_t timed_operation_initiated_millis[WAIT_ARRAY_SIZE]; // 4 bytes * 4 = 16 bytes
duration_ms_t timed_operation_desired_wait_millis[WAIT_ARRAY_SIZE]; // 2 bytes * 2 = 4 bytes
/*
record current time to compare to
*/
void start_timed_operation(enum_t index, duration_ms_t duration) {
timed_operation_initiated_millis[index] = millis();
timed_operation_desired_wait_millis[index] = duration;
LOG_TRACE4("Timer added type ", index, " wait in millis:", duration);
}
/*
Check whether the timer has expired
if expired, returns true else returns false
*/
boolean timed_operation_expired(enum_t index) {
if((millis() - timed_operation_initiated_millis[index]) < timed_operation_desired_wait_millis[index]) {
//DEBUG_PRINT("not expired");
return false;
}
//DEBUG_PRINT("expired");
return true;
}
/*****************************************************************************
* SENSOR SERVO
*****************************************************************************/
/*
We're sweeping the servo either left or right
DFR15 METAL GEAR SERVO
Voltage: +4.8-7.2V
Current: 180mA(4.8V);220mA(6V)
Speed(no load):0.17 s/60 degree (4.8V);0.25 s/60 degree (6.0V)
Torque:10Kg·cm(4.8V) 12KG·cm(6V) 15KG·cm(7.2V)
Temperature:0-60 Celcius degree
Size:40.2 x 20.2 x 43.2mm
Weight:48g
*/
constant_t SERVO_TURN_RATE_MS_PER_DEGREE = 3;
Servo sensor_servo;
duration_ms_t expected_wait_millis(turn_rate_t turn_rate, angle_t initial_angle, angle_t desired_angle) {
angle_t delta;
duration_ms_t return_value;
if(initial_angle == desired_angle) {
return_value = 0;
}
else if(initial_angle > desired_angle) {
return_value = turn_rate * (initial_angle - desired_angle);
}
else {
return_value = turn_rate * (desired_angle - initial_angle);
}
return return_value;
}
void update_servo_position(angle_t desired_sensor_servo_angle) {
duration_ms_t wait_millis;
switch(desired_sensor_servo_angle) {
case 0:
case 45:
case 90:
case 135:
// OK!
break;
default:
LOG_BAD_STATE(desired_sensor_servo_angle);
return;
break;
}
if(sensor_servo.read() != desired_sensor_servo_angle) {
// calculate expected wait BEFORE going there...
wait_millis = expected_wait_millis(SERVO_TURN_RATE_MS_PER_DEGREE, sensor_servo.read(), desired_sensor_servo_angle);
sensor_servo.write(desired_sensor_servo_angle); // tell servo to go to position in variable 'pos'
start_timed_operation(WAIT_FOR_SERVO_TO_TURN, wait_millis);
LOG_TELEMETRY(SENSOR_SERVO_POSITION_CHANGE, sensor_servo.read(), desired_sensor_servo_angle);
}
else {
LOG_TRACE2("Requesting sensor servo position already set", desired_sensor_servo_angle);
}
}
/*****************************************************************************
* SENSORS
*****************************************************************************/
/*
HC-SR04 Ultrasonic sensor
effectual angle: <15°
ranging distance : 2cm – 500 cm
resolution : 0.3 cm
*/
constant_t SENSOR_ARC_DEGREES = 15; // 180, 90, 15 all divisible by 15
constant_t SENSOR_PRECISION_CM = 1;
large_constant_t SENSOR_MAX_RANGE_CM = 500;
// speed of sound at sea level = 340.29 m / s
// spec range is 5m * 2 (return) = 10m
// 10 / 341 = ~0.029
large_constant_t SPEED_OF_SOUND_CM_PER_S = 34000;
constant_t SENSOR_MINIMAL_WAIT_ECHO_MILLIS = 50; //(SENSOR_MAX_RANGE_CM*2*1000)/SPEED_OF_SOUND_CM_PER_S;
/*
SENSOR_LEFT(135) SENSOR_FRONT (90) SENSOR_RIGHT (45)
-----------
| ^ |
| |
SENSOR_LEFT_SIDE(0) | | SENSOR_RIGHT_SIDE (0)
| |
| |
| |
-----------
SENSOR_BACK_LEFT(45) SENSOR_BACK(90) SENSOR_BACK_RIGHT(135)
*/
enum sensor_position {
SENSOR_LEFT,
SENSOR_FRONT,
SENSOR_RIGHT,
SENSOR_LEFT_SIDE,
SENSOR_RIGHT_SIDE,
SENSOR_BACK_LEFT,
SENSOR_BACK,
SENSOR_BACK_RIGHT,
NUMBER_READINGS,
};
angle_t sensor_position_to_servo_angle[] = {
135, // SENSOR_LEFT
90, // SENSOR_FRONT
45, // SENSOR_RIGHT
0, // SENSOR_LEFT_SIDE
0, // SENSOR_RIGHT_SIDE
45, // SENSOR_BACK_LEFT
90, // SENSOR BACK
135, // SENSOR_BACK_RIGHT
};
enum ultrasonics {
ULTRASONIC_FORWARD,
ULTRASONIC_REVERSE,
ULTRASONIC_DIRECTION_ARRAY_SIZE,
};
enum_t sensor_array_read_next;
sensor_reading_t sensor_distance_readings_cm[NUMBER_READINGS]; // 8 sensors * 4 bytes = 48 bytes
constant_t NO_READING = 0;
Ultrasonic sensor_forward = Ultrasonic(ULTRASONIC_FORWARD_TRIG, ULTRASONIC_FORWARD_ECHO);
Ultrasonic sensor_reverse = Ultrasonic(ULTRASONIC_REVERSE_TRIG, ULTRASONIC_REVERSE_ECHO);
enum_t current_max_sensor() {
enum_t max_sensor = SENSOR_LEFT;
for(loop_t i=1;i<NUMBER_READINGS; i++) {
if(sensor_distance_readings_cm[i] >= sensor_distance_readings_cm[max_sensor]) {
max_sensor = i;
}
}
return max_sensor;
}
distance_cm_t current_max_distance_cm() {
return sensor_distance_readings_cm[current_max_sensor()];
}
distance_cm_t update_sensor_value(enum_t sensor, distance_cm_t measured_value) {
// only update if the value is different beyond precision of sensor
if(abs(measured_value - sensor_distance_readings_cm[sensor]) > SENSOR_PRECISION_CM) {
sensor_distance_readings_cm[sensor] = measured_value;
}
return sensor_distance_readings_cm[sensor];
}
distance_cm_t read_sensor(enum_t sensor, Ultrasonic& sensor_object) {
if(!timed_operation_expired(WAIT_FOR_SERVO_TO_TURN)) {
return NO_READING;
}
if(!timed_operation_expired(WAIT_FOR_ECHO)) {
return NO_READING;
}
if(sensor_servo.read() != sensor_position_to_servo_angle[sensor]) {
LOG_ERROR("read_sensor() servo current_position does not match sensor desired angle", sensor, sensor_position_to_servo_angle[sensor], sensor_servo.read());
return NO_READING;
}
distance_cm_t return_value = update_sensor_value(sensor, sensor_object.Ranging());
LOG_TELEMETRY(ULTRASONIC_SENSORS, sensor, return_value);
start_timed_operation(WAIT_FOR_ECHO, SENSOR_MINIMAL_WAIT_ECHO_MILLIS);
return return_value;
}
/*****************************************************************************
* RC CAR RELATED
*****************************************************************************/
constant_t ROBOT_TURN_RATE_PER_SECOND = 90;
constant_t SAFE_DISTANCE_LARGE_TURN = 50; // distance between table and wall and minus size of robot (when robot is stuck...)
constant_t SAFE_DISTANCE_SMALL_TURN = 25; // one car length...
large_constant_t MAX_TIME_UNIT_MILLIS = 3000;
large_constant_t MIN_TIME_UNIT_MILLIS = 500;
constant_t CAR_LENGTH = 25;
constant_t CAR_WIDTH = 15;
enum commands {
FORWARD,
REVERSE,
LEFT,
RIGHT,
// COMBINED
FORWARD_LEFT,
FORWARD_RIGHT,
REVERSE_LEFT,
REVERSE_RIGHT,
};
bitmask8_t current_command = 0;
/*
Makes sure that exclusive directions are prohibited
*/
void _go(enum_t dir) {
switch(dir) {
case FORWARD:
digitalWrite(REVERSE_PIN, LOW);
digitalWrite(FORWARD_PIN, HIGH);
break;
case REVERSE:
digitalWrite(FORWARD_PIN, LOW);
digitalWrite(REVERSE_PIN, HIGH);
break;
case LEFT:
digitalWrite(RIGHT_PIN, LOW);
digitalWrite(LEFT_PIN, HIGH);
break;
case RIGHT:
digitalWrite(LEFT_PIN, LOW);
digitalWrite(RIGHT_PIN, HIGH);
break;
default:
LOG_BAD_STATE(dir);
break;
}
}
void _suspend(enum_t dir) {
switch(dir) {
case FORWARD:
digitalWrite(FORWARD_PIN, LOW);
break;
case REVERSE:
digitalWrite(REVERSE_PIN, LOW);
break;
case LEFT:
digitalWrite(LEFT_PIN, LOW);
break;
case RIGHT:
digitalWrite(RIGHT_PIN, LOW);
break;
default:
LOG_BAD_STATE(dir);
break;
}
}
/*
example:
dir is REVERSE (value 1)
so the dir_bitmask is 1 shifted by 1 to the left (0010)
if current_command is 1010 (or similar), then skip
otherwise (1000), execute and set current_command = 1000 | 0010 = 1010
*/
void go(enum_t dir) {
bitmask8_t dir_bitmask = 1 << dir;
if(!(current_command & dir_bitmask)) {
_go(dir);
current_command = current_command | dir_bitmask;
LOG_TELEMETRY(CAR_GO_COMMAND, current_command, dir);
}
}
void suspend(enum_t dir) {
bitmask8_t dir_bitmask = 1 << dir;
if(current_command & dir_bitmask) {
_suspend(dir);
current_command = current_command ^ dir_bitmask;
LOG_TELEMETRY(CAR_SUSPEND_COMMAND, current_command, dir);
}
}
void full_stop() {
suspend(FORWARD);
suspend(REVERSE);
suspend(LEFT);
suspend(RIGHT);
}
/*****************************************************************************
* INTERNAL STATES
*****************************************************************************/
enum states {
// start state
INITIAL,
FULL_SWEEP,
// units advancement
FORWARD_LEFT_UNIT,
FORWARD_UNIT,
FORWARD_RIGHT_UNIT,
REVERSE_LEFT_UNIT,
REVERSE_UNIT,
REVERSE_RIGHT_UNIT,
// decision making
STANDSTILL_DECISION,
// end state
STUCK,
STOP,
SMALL_TURN_CCW,
SMALL_TURN_CW,
};
enum_t current_state = STOP;
enum_t previous_state = FORWARD_UNIT;
/*****************************************************************************
* SETUP
*****************************************************************************/
void setup() {
// make sure we get all error message output
Serial.begin(9600);
Serial.println("------------------");
Serial.println("ART STARTED ");
Serial.println("Setting Arduino pins");
// these map to the contact switches on the RF
pinMode(FORWARD_PIN, OUTPUT);
pinMode(REVERSE_PIN, OUTPUT);
pinMode(LEFT_PIN, OUTPUT);
pinMode(RIGHT_PIN, OUTPUT);
//buttons
pinMode(PUSHBUTTON_PIN, INPUT);
Serial.println("Full stop");
full_stop();
Serial.println("Servo init");
sensor_servo.attach(SENSOR_SERVO_PIN);
sensor_servo.write(90);
delay(500);
Serial.println("Timers init");
for(loop_t i=0; i<WAIT_ARRAY_SIZE; i++) {
timed_operation_initiated_millis[i] = 0;
timed_operation_desired_wait_millis[i] = 0;
}
Serial.print("ART SETUP COMPLETED ");
Serial.println(millis());
}
/*****************************************************************************
* DECISION HELPERS
*****************************************************************************/
duration_ms_t get_forward_time_millis() {
1000;
}
duration_ms_t get_backward_time_millis() {
1000;
}
boolean is_safe_large_turn(enum_t sensor) {
return sensor_distance_readings_cm[sensor] >= SAFE_DISTANCE_LARGE_TURN;
}
boolean is_safe_small_turn(enum_t sensor) {
return sensor_distance_readings_cm[sensor] >= SAFE_DISTANCE_SMALL_TURN;
}
boolean is_greater(enum_t sensor_a, enum_t sensor_b) {
return sensor_distance_readings_cm[sensor_a] > sensor_distance_readings_cm[sensor_b];
}
/*****************************************************************************
* STATE INITS AND HANDLERS
*****************************************************************************/
/*
*/
/*****************************************************************************
STANDSTILL DECISION
Find out where we get the best distance range and go towards that
If all the distances in front are unsafe, return REVERSE
*****************************************************************************/
void init_standstill_decision() {
full_stop();
current_state = STANDSTILL_DECISION;
}
enum_t standstill_decision() {
// FORWARD
if(is_safe_large_turn(SENSOR_FRONT)) { // we can do a turn if we want by going forward
if(is_greater(SENSOR_FRONT, SENSOR_LEFT) && is_greater(SENSOR_FRONT, SENSOR_RIGHT)) {
return FORWARD_UNIT;
}
else if(is_safe_large_turn(SENSOR_LEFT) && is_greater(SENSOR_LEFT, SENSOR_RIGHT)) {
// left side is most promising
return FORWARD_LEFT_UNIT;
}
else if(is_safe_large_turn(SENSOR_RIGHT) && is_greater(SENSOR_RIGHT, SENSOR_LEFT)) {
// right side is most promising
return FORWARD_RIGHT_UNIT;
}
else {
// not greater but still safe...
return FORWARD_UNIT;
}
}
// Can't turn around safely, try small turns
if(is_safe_small_turn(SENSOR_LEFT) && is_safe_small_turn(SENSOR_BACK_RIGHT)) {
return SMALL_TURN_CCW;
}
else if(is_safe_small_turn(SENSOR_RIGHT) && is_safe_small_turn(SENSOR_BACK_LEFT)) {
return SMALL_TURN_CW;
}
// REVERSE
// forward isn't working out, let us see if reverse shows more promise so we can turn to be towards the most promising side...
// or keep turning to follow through on a previous turn
if(is_safe_large_turn(SENSOR_BACK)) {
// favor to help in a previous turn
if(is_safe_large_turn(SENSOR_LEFT) && previous_state == FORWARD_LEFT_UNIT) {
return REVERSE_LEFT_UNIT;
}
if(is_safe_large_turn(SENSOR_RIGHT) && previous_state == FORWARD_RIGHT_UNIT) {
return REVERSE_RIGHT_UNIT;
}
// ... other, select side with best potential
if(is_safe_large_turn(SENSOR_LEFT) && is_greater(SENSOR_LEFT_SIDE, SENSOR_RIGHT_SIDE)) {
// left side is most promising
return REVERSE_LEFT_UNIT;
}
else if(is_safe_large_turn(SENSOR_RIGHT) && is_greater(SENSOR_RIGHT_SIDE, SENSOR_LEFT_SIDE)) {
// right side is most promising
return REVERSE_RIGHT_UNIT;
}
}
// forward/reverse choice ...
if(is_safe_large_turn(SENSOR_FRONT)) {
if(is_greater(SENSOR_FRONT, SENSOR_BACK)) {
return FORWARD_UNIT;
}
else {
// otherwise, readings_reverse is safe and longest
return REVERSE_UNIT;
}
}
if(is_safe_large_turn(SENSOR_BACK)) {
return REVERSE_UNIT;
}
return NO_READING; // we're stuck, nothing safe!
}
/*****************************************************************************
* SENSOR FULL SWEEP
*****************************************************************************/
void init_full_sweep() {
full_stop();
sensor_array_read_next = SENSOR_FRONT;
update_servo_position(sensor_position_to_servo_angle[SENSOR_FRONT]);
current_state = FULL_SWEEP;
}
boolean full_sweep() {
// we check if we have an updated value here
distance_cm_t read_value = NO_READING;
switch(sensor_array_read_next) {
case SENSOR_LEFT:
case SENSOR_FRONT:
case SENSOR_RIGHT:
case SENSOR_RIGHT_SIDE:
read_value = read_sensor(sensor_array_read_next, sensor_forward);
break;
case SENSOR_BACK_RIGHT:
case SENSOR_BACK_LEFT:
case SENSOR_BACK:
case SENSOR_LEFT_SIDE:
read_value = read_sensor(sensor_array_read_next, sensor_reverse);
break;
default:
LOG_BAD_STATE(sensor_array_read_next);
break;
}
if(read_value != NO_READING) {
switch(sensor_array_read_next) {
case SENSOR_FRONT:
sensor_array_read_next = SENSOR_BACK;
break;
case SENSOR_BACK:
sensor_array_read_next = SENSOR_LEFT;
break;
case SENSOR_LEFT:
sensor_array_read_next = SENSOR_BACK_RIGHT;
break;
case SENSOR_BACK_RIGHT:
sensor_array_read_next = SENSOR_RIGHT;
break;
case SENSOR_RIGHT:
sensor_array_read_next = SENSOR_BACK_LEFT;
break;
case SENSOR_BACK_LEFT:
sensor_array_read_next = SENSOR_RIGHT_SIDE;
break;
case SENSOR_RIGHT_SIDE:
sensor_array_read_next = SENSOR_LEFT_SIDE;
break;
case SENSOR_LEFT_SIDE:
sensor_array_read_next = SENSOR_FRONT;
return true; // completed sweep!
break;
default:
LOG_BAD_STATE(sensor_array_read_next);
break;
}
update_servo_position(sensor_position_to_servo_angle[sensor_array_read_next]);
if(sensor_array_read_next == SENSOR_FRONT) {
return true;
}
}
return false;
}
/*****************************************************************************
* STUCK
*****************************************************************************/
void init_stuck() {
full_stop();
current_state = STUCK;
}
/*****************************************************************************
* SMALL RADIUS TURN
*****************************************************************************/
distance_cm_t target_distance_cm = SAFE_DISTANCE_LARGE_TURN;
enum_t small_turn_state;
void init_small_turn(enum_t small_turn_type) {
full_stop();
update_servo_position(sensor_position_to_servo_angle[SENSOR_FRONT]);
target_distance_cm = current_max_distance_cm();
current_state = small_turn_type;
if(small_turn_type == SMALL_TURN_CCW) {
small_turn_state = FORWARD_LEFT;
} else {
small_turn_state = FORWARD_RIGHT;
}
}
boolean handle_small_turn(enum_t small_turn_type) {
if(read_sensor(SENSOR_FRONT, sensor_forward) > (target_distance_cm - CAR_LENGTH/2) || !is_safe_small_turn(SENSOR_FRONT)) {
full_stop();
return true;
}
if(!timed_operation_expired(WAIT_FOR_ROBOT_TO_MOVE)) {
return false;
}
switch(small_turn_state) {
case FORWARD_LEFT:
go(FORWARD);
go(LEFT);
small_turn_state = REVERSE_RIGHT;
break;
case REVERSE_RIGHT:
go(REVERSE);
go(RIGHT);
small_turn_state = FORWARD_LEFT;
break;
case FORWARD_RIGHT:
go(FORWARD);
go(RIGHT);
small_turn_state = REVERSE_LEFT;
break;
case REVERSE_LEFT:
go(REVERSE);
go(LEFT);
small_turn_state = FORWARD_RIGHT;
break;
default:
LOG_BAD_STATE(small_turn_state);
break;
}
start_timed_operation(WAIT_FOR_ROBOT_TO_MOVE, MIN_TIME_UNIT_MILLIS);
return false;
}
/*****************************************************************************
* SMALL STEPS (UNIT) MOVEMENTS WITH TIMER
*****************************************************************************/
void init_direction_unit(enum_t decision) {
full_stop();
update_servo_position(sensor_position_to_servo_angle[SENSOR_FRONT]);
switch(decision) {
case FORWARD_LEFT_UNIT:
go(LEFT);
break;
case FORWARD_RIGHT_UNIT:
go(RIGHT);
break;
case REVERSE_LEFT_UNIT:
go(RIGHT);
break;
case REVERSE_RIGHT_UNIT:
go(LEFT);
break;
case REVERSE_UNIT:
case FORWARD_UNIT:
// do nothing, we will decide below what to do
break;
default:
LOG_BAD_STATE(decision);
break;
}
previous_state = current_state;
current_state = decision;
enum_t dir;
duration_ms_t time_millis;
switch(decision) {
case FORWARD_LEFT_UNIT:
case FORWARD_RIGHT_UNIT:
case FORWARD_UNIT:
time_millis = get_forward_time_millis();
dir = FORWARD;
break;
case REVERSE_LEFT_UNIT:
case REVERSE_RIGHT_UNIT:
case REVERSE_UNIT:
time_millis = get_backward_time_millis();
dir = REVERSE;
break;
default:
LOG_BAD_STATE(decision);
break;
}
LOG_TRACE2("Will move for (ms): ", time_millis);
start_timed_operation(WAIT_FOR_ROBOT_TO_MOVE, time_millis);
go(dir);
}
void handle_unit(enum_t sensor, Ultrasonic& sensor_object) {
//if(timed_operation_expired(WAIT_FOR_ROBOT_TO_MOVE)) {
// init_full_sweep();
//}
if(read_sensor(sensor, sensor_object) != NO_READING) {
if(!is_safe_large_turn(SENSOR_FRONT)) {
init_full_sweep();
}
}
}
/*****************************************************************************
* STOP BUTTON
*****************************************************************************/
void check_button() {
// read the state of the pushbutton value:
if(!timed_operation_expired(WAIT_FOR_BUTTON_REREAD)) {
return;
}
boolean buttonState = digitalRead(PUSHBUTTON_PIN) == HIGH?true:false;
// check if the pushbutton is pressed.
// if it is, the buttonState is HIGH:
if (buttonState) {
DEBUG_PRINT("Button pressed!");
if(current_state == STOP) {
current_state = INITIAL;
}
else {
full_stop();
current_state = STOP;
update_servo_position(sensor_position_to_servo_angle[SENSOR_FRONT]);
}
start_timed_operation(WAIT_FOR_BUTTON_REREAD, 1000);
}
}
/*****************************************************************************
* MAIN STATE MACHINE LOOP
*****************************************************************************/
void loop(){
enum_t initial_state = current_state;
enum_t decision;
check_button();
switch(current_state) {
// initial; this initiates what type of sub-state-machine we want to use
// unit: move one unit, scan, decide, move one unit, scan, decide, move one unit...
case INITIAL:
init_full_sweep(); // change this to change sub-state-machine
break;
case FULL_SWEEP:
if(full_sweep()) {
// one sweep completed
init_standstill_decision();
}
break;
case STANDSTILL_DECISION:
decision = standstill_decision();
if(decision != NO_READING) {
if(decision == SMALL_TURN_CW || decision == SMALL_TURN_CCW) {
init_small_turn(decision);
}
else {
init_direction_unit(decision);
}
}
else {
init_stuck();
}
break;
case FORWARD_UNIT:
case FORWARD_LEFT_UNIT:
case FORWARD_RIGHT_UNIT:
handle_unit(SENSOR_FRONT, sensor_forward);
break;
case REVERSE_UNIT:
case REVERSE_LEFT_UNIT:
case REVERSE_RIGHT_UNIT:
handle_unit(SENSOR_BACK, sensor_reverse);
break;
case SMALL_TURN_CW:
case SMALL_TURN_CCW:
if(handle_small_turn(current_state)) {
// small turn completed with target max distance
init_full_sweep();
}
break;
case STOP:
// do nothing...
if(millis() % 1000 == 0) {
Serial.print('.');
}
break;
case STUCK:
init_full_sweep();
break;
default:
LOG_BAD_STATE(current_state);
break;
}
if(initial_state != current_state) {
LOG_TELEMETRY(STATE_CHANGE, initial_state, current_state);
}
}