Skip to content
Newer
Older
100644 693 lines (633 sloc) 18.7 KB
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
1 #include <Ultrasonic.h>
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
2 #include <Servo.h>
3 #include <limits.h>
4
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
5 enum LOG_LEVELS {
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
6 ERROR,
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
7 INFO,
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
8 DEBUG,
9 TRACE,
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
10 };
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
11 int LOG_LEVEL = DEBUG;
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
12
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
13 // Arduino pins mapping
14
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
15 // various directions we can go to
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
16 const int FORWARD = 2; // the number of the LED pin
17 const int REVERSE = 3;
18 const int LEFT = 4;
19 const int RIGHT = 5;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
20 // ultrasonic
5b6ed40 revert change to ART_Control3
Ricky Ng-Adam authored Jan 1, 2011
21 const int ULTRASONIC_TRIG = 8;
22 const int ULTRASONIC_ECHO = 9;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
23 // servo
5b6ed40 revert change to ART_Control3
Ricky Ng-Adam authored Jan 1, 2011
24 const int SENSOR_SERVO = 13;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
25 // analog adjustments
26 const int FORWARD_POT = A5;
27 const int BACKWARD_POT = A4;
28 // buttons
29 const int PUSHBUTTON = 7;
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
30
31 /*
32 We're sweeping the servo either left or right
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
33 DFR15 METAL GEAR SERVO
34 Voltage: +4.8-7.2V
35 Current: 180mA(4.8V);220mA(6V)
36 Speed(no load):0.17 s/60 degree (4.8V);0.25 s/60 degree (6.0V)
37 Torque:10Kg·cm(4.8V) 12KG·cm(6V) 15KG·cm(7.2V)
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
38 Temperature:0-60 Celcius degree
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
39 Size:40.2 x 20.2 x 43.2mm
40 Weight:48g
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
41 */
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
42 const int SERVO_TURN_RATE_PER_SECOND = 100; // 100 = 60/(0.2*3) where 3 is caused by load?
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
43
44 /*
45 HC-SR04 Ultrasonic sensor
46 effectual angle: <15°
47 ranging distance : 2cm – 500 cm
48 resolution : 0.3 cm
49 */
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
50 const int SENSOR_LOOKING_FORWARD_ANGLE = 90;
51 // rotating counterclockwise...
52 const int SENSOR_LOOKING_LEFT_ANGLE = 135;
53 const int SENSOR_LOOKING_RIGHT_ANGLE = 45;
54 const int SENSOR_ARC_DEGREES = 15; // 180, 90, 15 all divisible by 15
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
55 const int MAXIMUM_SENSOR_SERVO_ANGLE = 180;
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
56 const int MINIMUM_SENSOR_SERVO_ANGLE = 0;
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
57 const int NUMBER_READINGS = MAXIMUM_SENSOR_SERVO_ANGLE/SENSOR_ARC_DEGREES; // 180/15 = 12 reading values in front
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
58 const int SENSOR_LOOKING_FORWARD_READING_INDEX = SENSOR_LOOKING_FORWARD_ANGLE/SENSOR_ARC_DEGREES; // 90/15 = 6
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
59 const int SENSOR_PRECISION_CM = 1;
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
60 const int SENSOR_MAX_RANGE_CM = 500;
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
61 // speed of sound at sea level = 340.29 m / s
62 // spec range is 5m * 2 (return) = 10m
63 // 10 / 341 = ~0.029
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
64 const int SPEED_OF_SOUND_CM_PER_S = 34000;
65 const int SENSOR_MINIMAL_WAIT_ECHO_MILLIS = (SENSOR_MAX_RANGE_CM*2*1000)/SPEED_OF_SOUND_CM_PER_S;
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
66
67 /*
68 Robot related information
69 */
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
70 const int ROBOT_TURN_RATE_PER_SECOND = 90;
448d043 adding new controller for platform #3
Ricky Ng-Adam authored Jan 1, 2011
71 const int SAFE_DISTANCE = 75;
72 const int CRITICAL_DISTANCE = 30;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
73 const int MAX_TIME_UNIT_MILLIS = 3000;
322575b bump up minimal time
Ricky Ng-Adam authored Jan 1, 2011
74 const int MIN_TIME_UNIT_MILLIS = 500;
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
75 enum states {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
76 // start state
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
77 INITIAL = 'I',
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
78 FULL_SWEEP = 'S',
79 QUICK_SWEEP = 'Q',
80 // dynamic requires fast sensor readings
81 DYNAMIC_FORWARD = 'F',
82 DYNAMIC_REVERSE = 'B',
83 DYNAMIC_TURN = 'T',
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
84 DECISION = 'D',
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
85 // units advancement
86 FORWARD_LEFT_UNIT = '1',
87 FORWARD_UNIT = '2',
88 FORWARD_RIGHT_UNIT = '3',
89 REVERSE_LEFT_UNIT = '4',
90 REVERSE_UNIT = '5',
91 REVERSE_RIGHT_UNIT = '6',
92 // decision making
93 QUICK_DECISION = '?',
94 REVERSE_QUICK_DECISION = '<',
95
96 // end state
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
97 STUCK = 'K',
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
98 STOP = '.'
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
99 };
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
100
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
101 // Global variable
102 // this contains readings from a sweep
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
103 int sensor_distance_readings_cm[NUMBER_READINGS];
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
104 const int NO_READING = -1;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
105 char current_state = STOP;
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
106 // contains target angle
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
107 int turn_towards;
52af5a8 make sure we a least do a minimum...
Ricky Ng-Adam authored Jan 1, 2011
108 int current_max_distance = SAFE_DISTANCE; // needs a value for backward
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
109
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
110 enum {
111 WAIT_FOR_SERVO_TO_TURN,
112 WAIT_FOR_ROBOT_TO_TURN,
113 WAIT_FOR_ECHO,
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
114 WAIT_FOR_ROBOT_TO_MOVE,
115 WAIT_FOR_ROBOT_TO_ADVANCE_UNIT,
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
116 WAIT_FOR_BUTTON_REREAD,
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
117 WAIT_ARRAY_SIZE, // always last...
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
118 };
119
120 int timed_operation_initiated_millis[WAIT_ARRAY_SIZE];
121 int timed_operation_desired_wait_millis[WAIT_ARRAY_SIZE];
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
122
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
123 // variables to store the servo current and desired angle
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
124 int current_sensor_servo_angle = 0;
125
126 // Two useful objects...
127 Servo sensor_servo; // create servo object to control a servo
128 Ultrasonic sensor = Ultrasonic(ULTRASONIC_TRIG, ULTRASONIC_ECHO);
129
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
130 void setup() {
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
131 // these map to the contact switches on the RF
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
132 pinMode(FORWARD, OUTPUT);
133 pinMode(REVERSE, OUTPUT);
134 pinMode(LEFT, OUTPUT);
135 pinMode(RIGHT, OUTPUT);
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
136 // input potentiometer
137 pinMode(FORWARD_POT, INPUT);
138 pinMode(BACKWARD_POT, INPUT);
139 //buttons
140 pinMode(PUSHBUTTON, INPUT);
141
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
142 full_stop();
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
143 sensor_servo.attach(SENSOR_SERVO);
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
144 for(int i=0; i<WAIT_ARRAY_SIZE; i++) {
145 timed_operation_initiated_millis[i] = 0;
146 timed_operation_desired_wait_millis[i] = 0;
147 }
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
148 safe_update_servo_position(SENSOR_LOOKING_FORWARD_ANGLE);
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
149 Serial.begin(9600);
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
150 }
151
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
152 /*
153 Makes sure that exclusive directions are prohibited
154 */
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
155 void go(int dir) {
156 switch(dir) {
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
157 case FORWARD:
158 digitalWrite(REVERSE, LOW);
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
159 digitalWrite(FORWARD, HIGH);
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
160 break;
161 case REVERSE:
162 digitalWrite(FORWARD, LOW);
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
163 digitalWrite(REVERSE, HIGH);
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
164 break;
165 case LEFT:
166 digitalWrite(RIGHT, LOW);
167 digitalWrite(LEFT, HIGH);
168 break;
169 case RIGHT:
170 digitalWrite(LEFT, LOW);
171 digitalWrite(RIGHT, HIGH);
172 break;
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
173 }
174 }
175
176 void suspend(int dir) {
177 digitalWrite(dir, LOW);
178 }
179
180 void full_stop() {
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
181 digitalWrite(FORWARD, LOW);
182 digitalWrite(REVERSE, LOW);
183 digitalWrite(LEFT, LOW);
184 digitalWrite(RIGHT, LOW);
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
185 }
186
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
187 /*
188 record current time to compare to
189 */
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
190 void start_timed_operation(int index, int duration) {
191 timed_operation_initiated_millis[index] = millis();
192 timed_operation_desired_wait_millis[index] = duration;
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
193 if(LOG_LEVEL >= TRACE) {
194 Serial.print("Timer added type ");
195 Serial.print(index);
196 Serial.print(" wait in millis:");
197 Serial.println(duration);
198 }
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
199 }
200
201 /*
202 Check whether the timer has expired
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
203 if expired, returns true else returns false
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
204 */
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
205 boolean timed_operation_expired(int index) {
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
206 int current_time = millis();
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
207 if((current_time - timed_operation_initiated_millis[index]) < timed_operation_desired_wait_millis[index]) {
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
208 return false;
209 }
210 return true;
211 }
212
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
213 int expected_wait_millis(int turn_rate, int initial_angle, int desired_angle) {
214 int delta;
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
215 if(initial_angle < 0 || desired_angle < 0) {
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
216 return 0;
217 } else if(initial_angle == desired_angle) {
218 return 0;
219 } else if(initial_angle > desired_angle) {
220 delta = initial_angle - desired_angle;
221 } else {
222 delta = desired_angle - initial_angle;
223 }
224 return (float(delta)/float(turn_rate))*1000.0;
225 }
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
226
227 int convert_reading_index(int angle) {
228 return angle/SENSOR_ARC_DEGREES;
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
229 }
230
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
231 int read_sensor() {
232 if(!timed_operation_expired(WAIT_FOR_SERVO_TO_TURN)) {
233 return NO_READING;
234 }
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
235
236 if(!timed_operation_expired(WAIT_FOR_ECHO)) {
237 return NO_READING;
238 }
239
240 int measured_value = sensor.Ranging(CM);
241
242 start_timed_operation(WAIT_FOR_ECHO, SENSOR_MINIMAL_WAIT_ECHO_MILLIS);
243
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
244 int index = convert_reading_index(current_sensor_servo_angle);
245 // only update if the value is different beyond precision of sensor
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
246 if(abs(measured_value - sensor_distance_readings_cm[index]) > SENSOR_PRECISION_CM) {
247 sensor_distance_readings_cm[index] = measured_value;
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
248 }
249 if(LOG_LEVEL >= INFO) {
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
250 Serial.print("Sensor reading:");
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
251 Serial.print(current_sensor_servo_angle);
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
252 Serial.print(":");
253 Serial.println(sensor_distance_readings_cm[index]);
254 }
255 return sensor_distance_readings_cm[index];
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
256 }
257
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
258 int get_last_reading_for_angle(int angle) {
259 return sensor_distance_readings_cm[convert_reading_index(angle)];
260 }
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
261
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
262 int get_forward_time_millis() {
52af5a8 make sure we a least do a minimum...
Ricky Ng-Adam authored Jan 1, 2011
263 int max_time = map(analogRead(FORWARD_POT), 0, 1024, MIN_TIME_UNIT_MILLIS, MAX_TIME_UNIT_MILLIS);
264 int time = map(current_max_distance, 0, SENSOR_MAX_RANGE_CM, MIN_TIME_UNIT_MILLIS, max_time);
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
265 return time;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
266 }
267
268 int get_backward_time_millis() {
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
269 // same logic because we don't have a front sensor...
52af5a8 make sure we a least do a minimum...
Ricky Ng-Adam authored Jan 1, 2011
270 int max_time = map(analogRead(BACKWARD_POT), 0, 1024, MIN_TIME_UNIT_MILLIS, MAX_TIME_UNIT_MILLIS);
271 int time = map(current_max_distance, 0, SENSOR_MAX_RANGE_CM, MIN_TIME_UNIT_MILLIS, max_time);
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
272 return time;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
273 }
274
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
275 /*
276 Initialize sweep (setting state and position sensor to be ready)
277 */
278 void init_sweep() {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
279 current_state = FULL_SWEEP;
280 safe_update_servo_position(MINIMUM_SENSOR_SERVO_ANGLE);
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
281 }
282
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
283 /*
284 Read the current angle and store it in the readings array
285 Find the next angle that has no reading
286 If found, set the desired angle to that and return false
287 If not found, returns true
288 */
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
289 boolean sensor_sweep() {
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
290 // read current value
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
291 if(read_sensor() == NO_READING) {
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
292 return false;
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
293 }
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
294
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
295 // we have a valid value, so move to the next position
296 int desired_sensor_servo_angle = current_sensor_servo_angle + SENSOR_ARC_DEGREES;
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
297
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
298 // we've completed from MINIMUM_SENSOR_SERVO_ANGLE to MAXIMUM_SENSOR_SERVO_ANGLE
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
299 if(desired_sensor_servo_angle > MAXIMUM_SENSOR_SERVO_ANGLE) {
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
300 return true;
301 }
302
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
303 // we always use read_sensor() before reaching here
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
304 update_servo_position(desired_sensor_servo_angle);
305 // keep doing the sweep
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
306 return false;
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
307 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
308
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
309 /*
310 make sure that the servo is at target position before returning
311 if you don't use this, you need to check yourself that the timer has expired...
312 */
313 void safe_update_servo_position(int desired_sensor_servo_angle) {
314 update_servo_position(desired_sensor_servo_angle);
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
315 while(!timed_operation_expired(WAIT_FOR_SERVO_TO_TURN)) {
fc48946 two bugs fixed:
Ricky Ng-Adam authored Jan 1, 2011
316 check_button();
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
317 }
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
318 }
319
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
320 void update_servo_position(int desired_sensor_servo_angle) {
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
321 if(current_sensor_servo_angle != desired_sensor_servo_angle) {
322 sensor_servo.write(desired_sensor_servo_angle-1); // tell servo to go to position in variable 'pos'
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
323
324 int wait_millis = expected_wait_millis(SERVO_TURN_RATE_PER_SECOND, current_sensor_servo_angle, desired_sensor_servo_angle);
325 start_timed_operation(WAIT_FOR_SERVO_TO_TURN, wait_millis);
326
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
327 current_sensor_servo_angle = desired_sensor_servo_angle;
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
328 if(LOG_LEVEL >= DEBUG) {
329 Serial.print("SERVO:");
330 Serial.println(desired_sensor_servo_angle);
331 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
332 }
333 }
334
335 boolean potential_collision() {
fac44e4 adding concept of safe and critical distance
Ricky Ng-Adam authored Dec 30, 2010
336 return sensor_distance_readings_cm[SENSOR_LOOKING_FORWARD_READING_INDEX] <= SAFE_DISTANCE;
337 }
338
339 boolean imminent_collision() {
340 return sensor_distance_readings_cm[SENSOR_LOOKING_FORWARD_READING_INDEX] <= CRITICAL_DISTANCE;
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
341 }
342
343 int find_best_direction_degrees() {
344 int longest_value = -1;
345 int longest_index = -1;
346 for(int i=0; i<NUMBER_READINGS; i++) {
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
347 if(sensor_distance_readings_cm[i] > longest_value && sensor_distance_readings_cm[i] >= SAFE_DISTANCE) {
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
348 longest_value = sensor_distance_readings_cm[i];
349 longest_index = i;
350 }
351 }
352 if(longest_index != -1) {
353 return longest_index * SENSOR_ARC_DEGREES;
354 }
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
355 return NO_READING;
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
356 }
357
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
358 /*
359 Find out where we get the best distance range and go towards that
360 If all the distances in front are unsafe, return REVERSE
361 */
362 int quick_decision() {
363 // one of the value has been updated, check to see if we should go left or right
364 // or just keep going forward
365 int left_value = get_last_reading_for_angle(SENSOR_LOOKING_LEFT_ANGLE);
366 int right_value = get_last_reading_for_angle(SENSOR_LOOKING_RIGHT_ANGLE);
367 int forward_value = get_last_reading_for_angle(SENSOR_LOOKING_FORWARD_ANGLE);
368
369 if(left_value > forward_value && left_value > right_value) {
7307ce2 fix bug: always returning reverse (if ... ;)
Ricky Ng-Adam authored Jan 1, 2011
370 if(left_value < SAFE_DISTANCE) {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
371 return REVERSE;
7307ce2 fix bug: always returning reverse (if ... ;)
Ricky Ng-Adam authored Jan 1, 2011
372 }
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
373 current_max_distance = left_value;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
374 return LEFT;
375 } else if(right_value > forward_value && right_value > left_value) {
7307ce2 fix bug: always returning reverse (if ... ;)
Ricky Ng-Adam authored Jan 1, 2011
376 if(right_value < SAFE_DISTANCE) {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
377 return REVERSE;
7307ce2 fix bug: always returning reverse (if ... ;)
Ricky Ng-Adam authored Jan 1, 2011
378 }
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
379 current_max_distance = right_value;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
380 return RIGHT;
381 } else {
7307ce2 fix bug: always returning reverse (if ... ;)
Ricky Ng-Adam authored Jan 1, 2011
382 if(forward_value < SAFE_DISTANCE) {
383 return REVERSE;
384 }
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
385 current_max_distance = forward_value;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
386 return FORWARD;
387 }
388 }
389
390 /*
391 completes once the sensor has readings left, right and center
392 */
393 boolean check_left;
52af5a8 make sure we a least do a minimum...
Ricky Ng-Adam authored Jan 1, 2011
394 int quick_sweep_number_readings = 0; // use modulo 4 to get every three readings
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
395 void init_quick_sweep() {
396 full_stop();
397 check_left = true; // always want to go left first
398 current_state = QUICK_SWEEP;
399 safe_update_servo_position(SENSOR_LOOKING_FORWARD_ANGLE);
400 }
401
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
402 boolean quick_sweep() {
403 // we check if we have an updated value here
404 if(read_sensor() != NO_READING) {
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
405 quick_sweep_number_readings++;
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
406 // we have an updated value, if it's a center value
407 // move the servo to get left and right readings
408 if(current_sensor_servo_angle == SENSOR_LOOKING_FORWARD_ANGLE) {
409 // go left or right depending on check_left
410 if(check_left) {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
411 // inside read_sensor() which guarantees servo timer expired
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
412 update_servo_position(SENSOR_LOOKING_LEFT_ANGLE);
413 } else {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
414 // inside read_sensor() which guarantees servo timer expired
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
415 update_servo_position(SENSOR_LOOKING_RIGHT_ANGLE);
416 }
417 } else {
418 // check left toggles between true and false here
419 check_left = !check_left;
420 // return to center; this mean we read twice as often forward than left or right
421 update_servo_position(SENSOR_LOOKING_FORWARD_ANGLE);
422 }
423 return true;
424 }
425 return false;
426 }
427
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
428 void init_turn() {
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
429 full_stop();
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
430 current_state = DYNAMIC_TURN;
431 safe_update_servo_position(SENSOR_LOOKING_FORWARD_ANGLE);
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
432 // 0-90 is to the right
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
433 if(turn_towards < SENSOR_LOOKING_FORWARD_ANGLE) {
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
434 go(RIGHT);
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
435 }
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
436 // 90 to 180 is to the left
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
437 else {
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
438 go(LEFT);
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
439 }
440 go(FORWARD);
ad9deb1 Sweeping activates at proper times
Ricky Ng-Adam authored Dec 31, 2010
441 int expected_wait = expected_wait_millis(ROBOT_TURN_RATE_PER_SECOND, SENSOR_LOOKING_FORWARD_ANGLE, turn_towards);
442 start_timed_operation(WAIT_FOR_ROBOT_TO_TURN, expected_wait);
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
443 if(LOG_LEVEL >= DEBUG) {
444 Serial.print("Waiting for robot to turn millis: ");
445 Serial.println(expected_wait);
446 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
447 }
448
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
449 void init_go_forward() {
450 full_stop();
451 current_state = DYNAMIC_FORWARD;
452 safe_update_servo_position(SENSOR_LOOKING_FORWARD_ANGLE);
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
453 }
454
455 void init_decision() {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
456 full_stop();
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
457 current_state = DECISION;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
458 safe_update_servo_position(SENSOR_LOOKING_FORWARD_ANGLE);
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
459 }
460
461 void init_stuck() {
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
462 full_stop();
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
463 current_state = STUCK;
464 }
465
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
466 void init_quick_decision() {
467 full_stop();
468 current_state = QUICK_DECISION;
469 }
6fb3528 added reverse with direction based on last forward direction
Ricky Ng-Adam authored Jan 1, 2011
470 int last_forward_state = FORWARD_UNIT;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
471 void init_direction_unit(int decision) {
472 full_stop();
473 if(decision == REVERSE) {
474 current_state = REVERSE_QUICK_DECISION;
475 return; // switching state
476 }
477
478 switch(decision) {
fda69a8 same missing case in switch statement error in other function!
Ricky Ng-Adam authored Jan 1, 2011
479 case LEFT:
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
480 current_state = FORWARD_LEFT_UNIT;
481 go(LEFT);
482 break;
fda69a8 same missing case in switch statement error in other function!
Ricky Ng-Adam authored Jan 1, 2011
483 case RIGHT:
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
484 current_state = FORWARD_RIGHT_UNIT;
485 go(RIGHT);
486 break;
fda69a8 same missing case in switch statement error in other function!
Ricky Ng-Adam authored Jan 1, 2011
487 case FORWARD:
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
488 current_state = FORWARD_UNIT;
489 break;
fda69a8 same missing case in switch statement error in other function!
Ricky Ng-Adam authored Jan 1, 2011
490 default:
491 if(LOG_LEVEL >= ERROR) {
492 Serial.print("BAD STATE IN init_direction_unit:");
493 Serial.println(decision);
494 }
495 break;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
496 }
6fb3528 added reverse with direction based on last forward direction
Ricky Ng-Adam authored Jan 1, 2011
497 last_forward_state = current_state;
1e41fb4 some debugging statements
Ricky Ng-Adam authored Jan 1, 2011
498 int forward_time_millis = get_forward_time_millis();
499 if(LOG_LEVEL>=INFO) {
500 Serial.print("Will move for (ms): ");
501 Serial.println(forward_time_millis);
502 }
503 start_timed_operation(WAIT_FOR_ROBOT_TO_ADVANCE_UNIT, forward_time_millis);
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
504 go(FORWARD);
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
505 }
506
507 void init_direction_reverse_unit(int dir) {
508 full_stop();
509 switch(dir) {
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
510 case LEFT:
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
511 current_state = REVERSE_LEFT_UNIT;
6fb3528 added reverse with direction based on last forward direction
Ricky Ng-Adam authored Jan 1, 2011
512 go(LEFT);
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
513 break;
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
514 case RIGHT:
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
515 current_state = REVERSE_RIGHT_UNIT;
6fb3528 added reverse with direction based on last forward direction
Ricky Ng-Adam authored Jan 1, 2011
516 go(RIGHT);
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
517 break;
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
518 case REVERSE:
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
519 current_state = REVERSE_UNIT;
520 break;
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
521 default:
522 if(LOG_LEVEL >= ERROR) {
523 Serial.print("BAD STATE IN init_direction_reverse_unit:");
524 Serial.println(dir);
525 }
526 break;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
527 }
1e41fb4 some debugging statements
Ricky Ng-Adam authored Jan 1, 2011
528 int backward_time_millis = get_backward_time_millis();
529 if(LOG_LEVEL>=INFO) {
530 Serial.print("Will move for (ms): ");
531 Serial.println(backward_time_millis);
532 }
533 start_timed_operation(WAIT_FOR_ROBOT_TO_ADVANCE_UNIT, backward_time_millis);
1f3f5e7 add a scaling based on the distance detected...
Ricky Ng-Adam authored Jan 1, 2011
534 go(REVERSE);
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
535 }
536
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
537 void check_button() {
538 // read the state of the pushbutton value:
539 if(!timed_operation_expired(WAIT_FOR_BUTTON_REREAD)) {
540 return;
541 }
542 int buttonState = digitalRead(PUSHBUTTON);
543 // check if the pushbutton is pressed.
544 // if it is, the buttonState is HIGH:
545 if (buttonState == HIGH) {
546 if(LOG_LEVEL >= INFO) {
547 Serial.println("Button pressed!");
548 }
549 if(current_state == STOP) {
550 current_state = INITIAL;
551 } else {
fc48946 two bugs fixed:
Ricky Ng-Adam authored Jan 1, 2011
552 full_stop();
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
553 current_state = STOP;
554 }
555 start_timed_operation(WAIT_FOR_BUTTON_REREAD, 1000);
556 }
557 }
558
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
559 void loop(){
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
560 int initial_state = current_state;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
561 check_button();
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
562 switch(current_state) {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
563 // initial; this initiates what type of sub-state-machine we want to use
564 // dynamic: more complex states that are supposed to adjust while moving
565 // unit: move one unit, scan, decide, move one unit, scan, decide, move one unit...
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
566 case INITIAL:
567 // wait for the first reading...
568 if(read_sensor() != NO_READING) {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
569 init_quick_sweep(); // change this to change sub-state-machine
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
570 }
571 break;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
572
573 // dynamic states
574 case FULL_SWEEP:
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
575 if(sensor_sweep()) {
576 // sweep completed, decision time!
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
577 init_decision();
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
578 } // else keep sweeping!
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
579 break;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
580 case DYNAMIC_FORWARD:
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
581 if(potential_collision()) {
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
582 // we're going to crash into something, stop and find an alternative
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
583 init_sweep();
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
584 } else {
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
585 go(FORWARD);
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
586 if(quick_sweep()) {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
587 suspend(LEFT);
588 suspend(RIGHT);
589 int decision = quick_decision();
590 if(decision == REVERSE) {
591 init_stuck();
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
592 } else {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
593 go(decision);
06a4b21 This version has problem as it constantly switches back to sweep...
Ricky Ng-Adam authored Dec 31, 2010
594 }
595 }
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
596 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
597 break;
598 case DECISION:
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
599 // we want to turn towards the longest opening
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
600 turn_towards = find_best_direction_degrees();
601 if(turn_towards != NO_READING) {
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
602 init_turn();
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
603 }
604 else {
f82d3cd quick sweep + precise sweep seem to work
Ricky Ng-Adam authored Dec 31, 2010
605 init_stuck();
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
606 }
607 break;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
608 case DYNAMIC_TURN:
609 read_sensor();
610 if(imminent_collision()) {
611 init_stuck();
612 }
613 if(timed_operation_expired(WAIT_FOR_ROBOT_TO_TURN)) {
614 // we've turned! reset and try to move forward now
615 init_go_forward();
616 }
617 break;
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
618 case STUCK:
09c70c4 added reverse when getting stuck...
Ricky Ng-Adam authored Dec 31, 2010
619 go(REVERSE);
620 if(quick_sweep()) {
621 if(!potential_collision()) {
622 init_sweep();
623 }
624 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
625 break;
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
626
627 // unit behavior
628 case QUICK_SWEEP:
629 if(quick_sweep()) {
630 // sweep has read one more reading
52af5a8 make sure we a least do a minimum...
Ricky Ng-Adam authored Jan 1, 2011
631 if(quick_sweep_number_readings % 4 == 0) {
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
632 // we've completed three readings
633 init_quick_decision();
634 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
635 }
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
636 break;
637 case QUICK_DECISION:
638 init_direction_unit(quick_decision());
639 break;
640 case REVERSE_QUICK_DECISION:
641 // no sensors to tell us if we can reverse
642 // or what direction to prefer so we always do...
6fb3528 added reverse with direction based on last forward direction
Ricky Ng-Adam authored Jan 1, 2011
643 switch(last_forward_state) {
644 case FORWARD_LEFT_UNIT:
645 init_direction_reverse_unit(RIGHT);
646 break;
647 case FORWARD_RIGHT_UNIT:
648 init_direction_reverse_unit(LEFT);
649 break;
650 case FORWARD_UNIT:
651 init_direction_reverse_unit(REVERSE);
652 break;
653 }
654
95ff2c9 added unit by unit advance and decision making
Ricky Ng-Adam authored Jan 1, 2011
655 break;
656 case FORWARD_UNIT:
657 case FORWARD_LEFT_UNIT:
658 case FORWARD_RIGHT_UNIT:
659 case REVERSE_UNIT:
660 case REVERSE_LEFT_UNIT:
661 case REVERSE_RIGHT_UNIT:
662 // all directions work the same: we wait!
663 if(timed_operation_expired(WAIT_FOR_ROBOT_TO_ADVANCE_UNIT)) {
664 init_quick_sweep();
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
665 }
666 break;
4f64e16 added potentiometers to parametrize forward/backward time unit easily
Ricky Ng-Adam authored Jan 1, 2011
667 case STOP:
668 // do nothing...
669 if(LOG_LEVEL >= INFO) {
670 if(millis() % 1000 == 0) {
671 Serial.print('.');
672 }
673 }
674 break;
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
675 default:
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
676 if(LOG_LEVEL >= ERROR) {
3e2e95a two bugs fixed while testing static:
Ricky Ng-Adam authored Jan 1, 2011
677 Serial.print("BAD STATE IN main loop:");
678 Serial.println(current_state);
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
679 }
ecb57a3 Servo/Sensor is by default 90 degrees to the front
Ricky Ng-Adam authored Dec 30, 2010
680 break;
681 }
682
683 if(initial_state != current_state) {
ac4fa52 fix missing last 180 degrees reading...
Ricky Ng-Adam authored Dec 31, 2010
684 if(LOG_LEVEL >= INFO) {
685 Serial.print("INITIAL STATE:");
686 Serial.print((char)initial_state);
687 Serial.print(" FINAL STATE:");
688 Serial.println((char)current_state);
689 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
690 }
659b92f New car untested program (copy of ART_Control2) and BUILD instruction…
Ricky Ng-Adam authored Dec 29, 2010
691 }
50627b9 It compiles!!
Ricky Ng-Adam authored Dec 30, 2010
692
Something went wrong with that request. Please try again.