Autonomous Controllable LEGO Car

1#include "Servo.h"
2#include "IRremote.h"
3#include "Stepper.h"
4#define  POTPIN 1
5#define IRRECIEVER 2
6#define NXTMOTORONE 6 // two directions for  each motor using motor driver l9110
7#define NXTMOTORTWO 10
8#define NXTMOTORONEBACK  5
9#define NXTMOTORTWOBACK 9
10#define WAITTIME 150 // time stopped in each of  4 directions or used for setting motors speeds to zero
11#define STEPPERDEGREERADIUS  135
12#define TRIGUS 13 // sends out U.S. pulse 
13#define BUTTONFORWARDONE A4  // push buttons uses as touch sensors
14#define BUTTONFORWARDTWO A5
15#define  BUTTONBACKWARDONE A2
16#define BUTTONBACKWARDTWO A3
17byte echoUS = 3; // recieves  U.S. pulse and uses time elapsed to determine distance
18long numberOfLoops = 0;  // used to run specific autonomous functions periodically such as checkNearby()  and checkSurroundings()
19int cmDistance = 0; // uses ping U.S. function to determine  distance
20int currentCm = 0;
21int turnTime; // 90 degree turn dependent on surface
22double  valPot; // value from potentiometer from 0 - 1023
23int motorSpeedPot; // valPot  converted to analog value for motors from 0 - 255
24double fractionOfFullSpeed;
25boolean  pathClear = false; // returns true if object within 65 cm and can cause clockwise  rotation, else no changes to straight path
26boolean backwardsMotion = false; //  control of either driving forward or backward
27int controlSpeed = 255; // used  in remote control with ir remote and reciever, default: fast, straight, and forward
28int  controlDir = 90;
29int controlDirVolume = 90;
30int controlDirVolIncrement = 10;
31boolean  controlBackwards = false;
32boolean autoOff = false; // stores two possibilities  of drive mode- autonomous or manual control (from ir remote)
33int distanceStepper[360];  // array to store maximum of distance from U.S. in each degree in a circle when  rotated using stepper motor
34int maxDistanceStepper;
35int degreeMaxDistanceStepper;
36boolean  manualStepper; // ir remote control of stepper motor
37boolean stepperClockwise  = false;
38int turnTimeFarthest; // time for one nxt motor to rotate car to allign  with degree which corresponds to farthest valid distance
39int stepperRadiusCount  = 0; // always turn stepper over input radius while moving forward 
40int stepDisplacement  = 0; // used to find current displacement and revert stepper to forward facing if  needed
41boolean stepperTurnDirection = false;
42int sameDegreeCount; // used  to check for repetitive distance readings and holds the longest set and associated  degree in variables
43int sameDegreeCountMax;
44int sameDegreeValueMax; 
45boolean  forwardButtonControl = false; // push buttons to trigger overriding backward or  forward movement when car crashes
46boolean backwardButtonControl = false;
47boolean  buttonDriverOverride = false; // for autonomous control using only buttons, no U.S.,  activated using 4 on ir remote
48boolean buttonDriveForward = true;
49byte buttonCycleCount  = 0;
50Servo myServo; 
51IRrecv irrecv(IRRECIEVER);
52decode_results results;
53Stepper  stepper(32, 12, 8, 11, 7); // specific arrangement of ports to allow for two-directional  movement of stepper motor, 32 is number of rotations by internal shaft- 2048 by  external shaft
54
55void setup(void) {
56  Serial.begin(57600);
57  Serial.setTimeout(1);
58  irrecv.enableIRIn(); // Start the receiver  
59  myServo.attach(4);
60  myServo.write(90);  // starts servo at 90 degrees
61  pinMode(NXTMOTORONE, OUTPUT); // motors connected  to output pins which support analog, forward drive motors
62  pinMode(NXTMOTORTWO,  OUTPUT);
63  pinMode(NXTMOTORONEBACK, OUTPUT); // backward drive motors
64  pinMode(NXTMOTORTWOBACK,  OUTPUT);
65  pinMode(TRIGUS, OUTPUT); // send sound pulse every 5 ms
66  pinMode(echoUS,  INPUT);  // detects time for sound pulse to return
67  pinMode(BUTTONFORWARDONE,  INPUT);
68  pinMode(BUTTONFORWARDTWO, INPUT);
69  pinMode(BUTTONBACKWARDONE, INPUT);
70  pinMode(BUTTONBACKWARDTWO, INPUT);
71  digitalWrite(NXTMOTORONEBACK, LOW); //  since input pullup has resistor at voltage, starts high and needs to be written  low
72  digitalWrite(NXTMOTORTWOBACK, LOW);
73}
74
75void forwardObstacleButton(void){  // go backwards, then rotate 180 degrees
76  halt(WAITTIME);
77  backwardsMotion  = true;
78  moveDirection(90, motorSpeedPot, 2 * turnTime, backwardsMotion);
79  moveDirection(0, motorSpeedPot, 5 * turnTime / 2, backwardsMotion);
80  backwardsMotion  = false; 
81}
82
83void backwardObstacleButton(void){ // go forward, then turn  around
84  halt(WAITTIME);
85  backwardsMotion = false;
86  moveDirection(90,  motorSpeedPot, 2 * turnTime, backwardsMotion);
87  moveDirection(0, motorSpeedPot,  2 * turnTime, backwardsMotion);
88}
89
90void stepDegree(double degree) { //  can turn stepper to any degree in circle in both directions
91  stepper.step((int)(1800.0  * degree / 360.0)); // 1800 provides more accurate turning than the stated 2048  
92}
93
94void stepperCircleDegreeCheck(void) { // take U.S. measurements every  degree over the course of a whole circle
95  stepDegree( -1 * stepDisplacement);  // ensure starting ultrasonic measurements from forward facing
96  stepDisplacement  = 0;  
97  for (int i = 0; i <= 360; ++i) {
98    cmDistance = ping(echoUS);
99    distanceStepper[i] = cmDistance; // stores each degree in 360 length array
100   // Serial.print("Dis at "); Serial.print(i); Serial.print(" deg: "); Serial.print(distanceStepper[i]);  Serial.print(" cm"); Serial.print("\
101");
102    stepDegree(1.0);
103    delay(17);  // takes maximum of 10 ms for ultrasonic reading
104  }
105  delay(100);
106  sameDegreeCount  = 0;  
107  sameDegreeCountMax = 0; 
108  maxDistanceStepper = distanceStepper[0];    
109  degreeMaxDistanceStepper = 0;
110  for (int i = 1; i <= 360; ++i) { //  if greater, replace max
111    if (distanceStepper[i] > maxDistanceStepper) {
112      maxDistanceStepper = distanceStepper[i];
113      degreeMaxDistanceStepper  = i;
114      sameDegreeCount = 0;
115    }
116    else if (distanceStepper[i] ==  maxDistanceStepper) { // if equal, count how many times degree equality is held  (how many degrees is farthest distance constant) 
117      ++sameDegreeCount;
118      if (sameDegreeCount > sameDegreeCountMax)
119      {
120        sameDegreeCountMax  = sameDegreeCount;
121        sameDegreeValueMax = i;
122      }
123    }
124    else  if (distanceStepper[i] < maxDistanceStepper){
125      sameDegreeCount = 0;
126    }
127  }
128  if (distanceStepper[sameDegreeValueMax] == maxDistanceStepper){  // check if max is equal to degree where most repetitive degree (typically 200 since  too far to create actual measurement due to timeout)
129    degreeMaxDistanceStepper  = sameDegreeValueMax - sameDegreeCountMax / 2; // find median degree in which farthest  data point is recorded and use it for motor rotation to position car to that degree
130  }
131 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\
132");  Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\
133");  Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\
134");
135  Serial.print("Max dis: "); Serial.print(maxDistanceStepper); Serial.print("  cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\
136");
137  stepDegree(-360.0); // return to original position
138  // stepDegree(degreeMaxDistanceStepper);  // returns to degree where farthest U.S. reading was taken
139}
140
141void stepperDegreeCheck(int  lowerBound, int higherBound){ // U.S. uses stepper to record each degree within  bounds
142  stepDegree( -1 * stepDisplacement); 
143  stepDisplacement = 0;  
144  stepDegree(lowerBound); // position stepper to start at lower bound
145  for (int  i = lowerBound; i <= higherBound; ++i) { // goes to lower bound, then takes one  degree incremental measurements until higher bound
146    cmDistance = ping(echoUS);
147    distanceStepper[i] = cmDistance;
148   // Serial.print("Dis at: "); Serial.print(i);  Serial.print(" deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm");  Serial.print("\
149");
150    stepDegree(1.0);
151    delay(17);
152  }
153  delay(100);
154  sameDegreeCount = 0;  
155  sameDegreeCountMax = 0; 
156  maxDistanceStepper =  distanceStepper[0];    
157  degreeMaxDistanceStepper = 0;
158  for (int i = 1;  i <= 360; ++i) { 
159    if (distanceStepper[i] > maxDistanceStepper) {
160      maxDistanceStepper  = distanceStepper[i];
161      degreeMaxDistanceStepper = i;
162      sameDegreeCount  = 0;
163    }
164    else if (distanceStepper[i] == maxDistanceStepper) { 
165      ++sameDegreeCount;
166      if (sameDegreeCount > sameDegreeCountMax)
167      {
168        sameDegreeCountMax  = sameDegreeCount;
169        sameDegreeValueMax = i;
170      }
171    }
172    else  if (distanceStepper[i] < maxDistanceStepper){
173      sameDegreeCount = 0;
174    }
175  }
176  if (distanceStepper[sameDegreeValueMax] == maxDistanceStepper){
177    degreeMaxDistanceStepper = sameDegreeValueMax - sameDegreeCountMax / 2; 
178  }
179 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\
180");  Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\
181");  Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\
182");
183  Serial.print("Dis max: "); Serial.print(maxDistanceStepper); Serial.print("  cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\
184");
185  stepDegree(-1 * higherBound); // returns to original position
186}
187
188void  stepperDegreeCheck(int lowerBound, int higherBound, int manualDegreeReturn, int  speedControl){ // adds control of degrees returns to avoid extra waiting, along  with stepper speed control
189  stepper.setSpeed(speedControl);
190  stepDegree(  -1 * stepDisplacement); 
191  stepDisplacement = 0;
192  stepDegree(lowerBound);
193  for (int i = lowerBound; i <= higherBound; ++i) { // creates array of U.S. measurement  values between limits
194    cmDistance = ping(echoUS);
195    distanceStepper[i]  = cmDistance;
196   // Serial.print("Dis at: "); Serial.print(i); Serial.print("  deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm"); Serial.print("\
197");
198    stepDegree(1.0);
199    delay(17);
200  }
201  delay(100);
202  sameDegreeCount  = 0;  
203  sameDegreeCountMax = 0; 
204  maxDistanceStepper = distanceStepper[lowerBound];    
205  degreeMaxDistanceStepper = lowerBound;
206  for (int i = ++lowerBound;  i <= higherBound; ++i) { 
207    if (distanceStepper[i] > maxDistanceStepper) {
208      maxDistanceStepper = distanceStepper[i];
209      degreeMaxDistanceStepper  = i;
210      sameDegreeCount = 0;
211    }
212    else if (distanceStepper[i] ==  maxDistanceStepper) {  
213      ++sameDegreeCount;
214      if (sameDegreeCount  > sameDegreeCountMax)
215      {
216        sameDegreeCountMax = sameDegreeCount;
217        sameDegreeValueMax = i;
218      }
219    }
220    else if (distanceStepper[i]  < maxDistanceStepper){
221      sameDegreeCount = 0;
222    }
223  }
224  if (distanceStepper[sameDegreeValueMax]  == maxDistanceStepper){ 
225    degreeMaxDistanceStepper = sameDegreeValueMax -  sameDegreeCountMax / 2; 
226  }
227 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount);  Serial.print("\
228"); Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax);  Serial.print("\
229"); Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax);  Serial.print("\
230");
231  Serial.print("Dis max: "); Serial.print(maxDistanceStepper);  Serial.print(" cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\
232");
233  stepDegree(manualDegreeReturn); // goes to specified end position
234}
235
236void  kick(boolean leftKick) {
237  halt(WAITTIME); // halt motion to kick
238  if (leftKick)  {
239    myServo.write(115); // backs up 25 degrees, delay 1/4 seconds, kicks to  full, hold for one second
240    delay(250);
241    myServo.write(0);
242    delay(1000);
243  }
244  else if (!leftKick) { // performs right kick
245    myServo.write(65);
246    delay(250);
247    myServo.write(180);
248    delay(1000);
249  }
250  myServo.write(90);  // resets servo to 90 (pointed up)
251}
252
253void halt(int duration) {
254  unsigned  long previousMillis = millis();
255  while ((previousMillis + duration) >= millis())  {
256    digitalWrite(NXTMOTORONE, LOW);
257    digitalWrite(NXTMOTORTWO, LOW);
258    digitalWrite(NXTMOTORONEBACK, LOW);
259    digitalWrite(NXTMOTORTWOBACK, LOW);
260  }
261}
262
263void motorHigh(boolean backwards) {
264  if (!backwards){
265    digitalWrite(NXTMOTORONE,  HIGH);
266    digitalWrite(NXTMOTORTWO, HIGH);
267    digitalWrite(NXTMOTORONEBACK,  LOW);
268    digitalWrite(NXTMOTORTWOBACK, LOW);    
269  }
270  else if (backwards){
271    digitalWrite(NXTMOTORONE, LOW);
272    digitalWrite(NXTMOTORTWO, LOW);
273    digitalWrite(NXTMOTORONEBACK,  HIGH);
274    digitalWrite(NXTMOTORTWOBACK, HIGH);        
275  }
276}
277
278void  moveDirection(int turnDegree, int speedIntensity, boolean backwards) { // turnDegree  from 0 to 180 (90 is straight), speedIntensity from 0 - 255 for analog motor, true/false  for backwards movement
279  // if degree is 0, left wheel turns (clockwise), if  degree is 180, right wheel turns (counterclockwise)
280  double totalDelay = 250.0;  // combined motor delay
281  double ratioLeft = ((double)(turnDegree)) / 180.0;  // degree determines how totalDelay is split up for each motor
282  double ratioRight  = 1.0 - ratioLeft;
283  int delayPtOne = (int)(ratioRight * totalDelay);
284  int  delayPtTwo = (int)(ratioLeft * totalDelay);
285  if (!backwards) {
286    digitalWrite(NXTMOTORONEBACK,  LOW); // run both motors for lowest time, then run only one motor for the remaining  time needed for that ratio
287    digitalWrite(NXTMOTORTWOBACK, LOW);
288    analogWrite(NXTMOTORONE,  speedIntensity);
289    analogWrite(NXTMOTORTWO, speedIntensity);
290    if (delayPtOne  > delayPtTwo) {
291      delay(delayPtTwo);
292      digitalWrite(NXTMOTORTWO, LOW);
293      delay(delayPtOne - delayPtTwo);
294    }
295    else if (delayPtOne < delayPtTwo)  {
296      delay(delayPtOne);
297      digitalWrite(NXTMOTORONE, LOW);
298      delay(delayPtTwo  - delayPtOne);
299    }
300    else if (delayPtOne == delayPtTwo) {
301      delay(totalDelay);
302    }
303    digitalWrite(NXTMOTORONE, LOW);
304    digitalWrite(NXTMOTORTWO, LOW);
305    delay(1);
306  }
307  else if (backwards) {
308    digitalWrite(NXTMOTORONE,  LOW);
309    digitalWrite(NXTMOTORTWO, LOW);
310    analogWrite(NXTMOTORONEBACK,  speedIntensity);
311    analogWrite(NXTMOTORTWOBACK, speedIntensity);
312    if  (delayPtOne > delayPtTwo) {
313      delay(delayPtTwo);
314      digitalWrite(NXTMOTORTWOBACK,  LOW);
315      delay(delayPtOne - delayPtTwo);
316    }
317    else if (delayPtOne  < delayPtTwo) {
318      delay(delayPtOne);
319      digitalWrite(NXTMOTORONEBACK,  LOW);
320      delay(delayPtTwo - delayPtOne);
321    }
322    else if (delayPtOne  == delayPtTwo) {
323      delay(totalDelay);
324    }
325    digitalWrite(NXTMOTORONE,  LOW);
326    digitalWrite(NXTMOTORTWO, LOW);
327    delay(1);
328  }
329}
330
331void  moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards)  { // only runs if pathClear
332  if (pathClear) {
333    moveDirection(turnDegree,  speedIntensity, backwards);
334  }
335  else {
336    moveDirection(0, speedIntensity,  backwards);
337  }
338}
339
340void moveDirection(int turnDegree, int speedIntensity,  boolean pathClear, boolean backwards, int stepperTurnRadius) { // runs motors under  path clear while turning stepper over input radius, not stimultaneosly
341  stepper.setSpeed(700);
342  moveDirection(turnDegree, speedIntensity, pathClear, backwards); // alternate  between moving motors forward and turning stepper motor
343  if (stepperRadiusCount  < stepperTurnRadius){ // turn +- stepperTurnRadius from forward facing
344    if  (stepperTurnDirection){
345      stepDegree(1.0); // use small stepper increments  so that autonomous can run for most of the time 
346    }
347    else if (!stepperTurnDirection){
348      stepDegree(-1.0);
349    }
350    ++stepperRadiusCount;
351  }
352  else if  (stepperRadiusCount < 2 * stepperTurnRadius){
353    if (!stepperTurnDirection){
354      stepDegree(1.0);
355    }
356    else if (stepperTurnDirection){
357      stepDegree(-1.0);
358    }
359    ++stepperRadiusCount;
360  }
361  else{
362    stepperRadiusCount =  0;
363    stepperTurnDirection = !stepperTurnDirection;
364  }
365}
366
367void  moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards,  int stepperTurnRadius, int stepperStepIndividual) { // adds step feature to move  specified degrees each time
368  stepper.setSpeed(700);
369  moveDirection(turnDegree,  speedIntensity, pathClear, backwards); 
370  motorHigh(backwards);
371  if (stepperRadiusCount  < stepperTurnRadius){ 
372    if (stepperTurnDirection){
373      stepDegree(stepperStepIndividual);  // use specified degree increment
374      stepDisplacement += stepperStepIndividual;  
375    }
376    else if (!stepperTurnDirection){
377      stepDegree(-1.0 * stepperStepIndividual);
378      stepDisplacement += (-1 * stepperStepIndividual);
379    }
380    stepperRadiusCount  += stepperStepIndividual;
381  }
382  else if (stepperRadiusCount < (2 * stepperTurnRadius)){
383    if (!stepperTurnDirection){
384      stepDegree(stepperStepIndividual);
385      stepDisplacement += stepperStepIndividual;  
386    }
387    else if (stepperTurnDirection){
388      stepDegree(-1.0 * stepperStepIndividual);
389      stepDisplacement += (-1  * stepperStepIndividual);
390    }
391    stepperRadiusCount += stepperStepIndividual;
392  }
393  else{
394    stepperRadiusCount = 0;
395    stepperTurnDirection = !stepperTurnDirection;
396  }
397 // Serial.print("stepperRadiusCount: "); Serial.print(stepperRadiusCount);  Serial.print("\
398");
399}
400
401void moveDirection(int turnDegree, int speedIntensity,  int duration, boolean backwards) { // runs for specific time duration regardless  of pathClear
402  unsigned long previousMillis = millis();
403  while ((previousMillis  + duration) >= millis()) {
404    moveDirection(turnDegree, speedIntensity, backwards);
405  }
406}
407
408void moveDirection(int turnDegree, int speedIntensity, boolean pathClear,  int duration, boolean backwards) { // runs for specific time duration if pathClear
409  unsigned long previousMillis = millis();
410  while ((previousMillis + duration)  >= millis()) {
411    moveDirection(turnDegree, speedIntensity, pathClear, backwards);
412  }
413}
414
415void checkSurroundings(void) {
416  halt(WAITTIME); // stop motor  rotation
417  stepDegree( -1 * stepDisplacement); // allign stepper to forward facing  before recording data
418  stepDisplacement = 0;
419  int cmDistanceNorth = ping(echoUS);
420  int cmDistanceMax = cmDistanceNorth;
421  String dirMaxDis = "NORTH"; // max  distance direction stored in string
422 // Serial.print("Dis no: "); Serial.print(cmDistanceNorth);  Serial.print("\
423");
424  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);  // rotate 1/4 of a circle
425  halt(WAITTIME);   // temporarily stop to ensure precise  measurement
426  int cmDistanceWest = ping(echoUS);
427 // Serial.print("Dis we:  "); Serial.print(cmDistanceWest); Serial.print("\
428");
429  if (cmDistanceWest  > cmDistanceMax) {
430    cmDistanceMax = cmDistanceWest;
431    dirMaxDis = "WEST";  // dirMaxDis replaced if needed
432  }
433  moveDirection(180, motorSpeedPot, turnTime,  backwardsMotion);
434  halt(WAITTIME);
435  int cmDistanceSouth = ping(echoUS);
436  // Serial.print("Dis so: "); Serial.print(cmDistanceSouth); Serial.print("\
437");
438  if (cmDistanceSouth > cmDistanceMax) {
439    cmDistanceMax = cmDistanceSouth;
440    dirMaxDis = "SOUTH";
441  }
442  moveDirection(180, motorSpeedPot, turnTime,  backwardsMotion);
443  halt(WAITTIME);
444  int cmDistanceEast = ping(echoUS);
445  // Serial.print("Dis ea: "); Serial.print(cmDistanceEast); Serial.print("\
446");
447  if (cmDistanceEast > cmDistanceMax) {
448    cmDistanceMax = cmDistanceEast;
449    dirMaxDis = "EAST";
450  }
451  moveDirection(180, motorSpeedPot, turnTime,  backwardsMotion);
452  halt(WAITTIME);   // back to facing north
453  Serial.print("Dis  max: "); Serial.print(cmDistanceMax); Serial.print(" in "); Serial.print(dirMaxDis);  Serial.print("\
454");
455  if (dirMaxDis.equals("NORTH")) { // returns to distance  with obstacle farthest in sight by associating dirMaxDis with direction
456    halt(WAITTIME);
457  }
458  else if (dirMaxDis.equals("WEST")) {
459    moveDirection(180, motorSpeedPot,  turnTime, backwardsMotion);
460  }
461  else if (dirMaxDis.equals("SOUTH")) {
462    moveDirection(180, motorSpeedPot, 2 * turnTime, backwardsMotion);
463  }
464  else if (dirMaxDis.equals("EAST")) {
465    moveDirection(180, motorSpeedPot,  3 * turnTime, backwardsMotion);
466  }
467}
468
469void checkNearby(double checkDegreeRadius)  { // takes three measurements, straight, + checkDegreeRadius, - checkDegreeRadius  
470  halt(WAITTIME);
471  int rotationTime = (int)(((double)turnTime / 90.0) *  checkDegreeRadius); // turnTime / 90 = time to turn 1 degree, multiply by input  degree to find total rotation time
472  stepDegree( -1 * stepDisplacement); 
473  stepDisplacement = 0;
474  int cmDistanceStraight = ping(echoUS);
475  int cmSideDistanceMax  = cmDistanceStraight;
476  String sideMaxDis = "STRAIGHT";
477 // Serial.print("Dis  str: "); Serial.print(cmDistanceStraight); Serial.print("\
478");
479  moveDirection(180,  motorSpeedPot, rotationTime, backwardsMotion);
480  halt(WAITTIME);
481  int cmDistanceLeft  = ping(echoUS);
482 // Serial.print("Dis left: "); Serial.print(cmDistanceLeft);  Serial.print("\
483");
484  if (cmDistanceLeft > cmSideDistanceMax) {
485    cmSideDistanceMax  = cmDistanceLeft;
486    sideMaxDis = "LEFT";
487  }
488  moveDirection(0, motorSpeedPot,  2 * rotationTime, backwardsMotion);
489  halt(WAITTIME);
490  int cmDistanceRight  = ping(echoUS);
491 // Serial.print("Dis right: "); Serial.print(cmDistanceRight);  Serial.print("\
492");
493  if (cmDistanceRight > cmSideDistanceMax) {
494    cmSideDistanceMax  = cmDistanceRight;
495    sideMaxDis = "RIGHT";
496  }
497  moveDirection(180,  motorSpeedPot, rotationTime, backwardsMotion);
498  halt(WAITTIME);
499  Serial.print("Dis  max: "); Serial.print(cmSideDistanceMax); Serial.print(" in "); Serial.print(sideMaxDis);  Serial.print("\
500");
501  if (sideMaxDis.equals("STRAIGHT")) { // goes either  straight, or left or right degree input depending on where the farthest distance  was measured
502    halt(WAITTIME);
503  }
504  else if (sideMaxDis.equals("LEFT"))  {
505    moveDirection(180, motorSpeedPot, rotationTime, backwardsMotion);
506  }
507  else if (sideMaxDis.equals("RIGHT")) {
508    moveDirection(0, motorSpeedPot,  rotationTime, backwardsMotion);
509  }
510}
511
512void buttonDriver(void){ // auto  control using only buttons
513  if (digitalRead(BUTTONFORWARDONE) == LOW || digitalRead(BUTTONFORWARDTWO)  == LOW){ // checks if any push button pushed
514    buttonDriveForward = false;
515    ++buttonCycleCount;
516    if (buttonCycleCount % 4 == 0){ // turn every 2 button  pushes
517      moveDirection(180, 255, 2 * turnTime, false); 
518      buttonCycleCount  = 0;
519    }
520  }
521  else if (digitalRead(BUTTONBACKWARDONE) == LOW || digitalRead(BUTTONBACKWARDTWO)  == LOW){
522    buttonDriveForward = true;
523    ++buttonCycleCount;
524    if  (buttonCycleCount % 2 == 0){
525      moveDirection(0, 255, turnTime, false); 
526      buttonCycleCount = 0;
527    }
528  }
529  if (buttonDriveForward){ // move  in opposite direction of push after pushed impulse
530    moveDirection(90, 255,  false);
531  }
532  else if (!buttonDriveForward){
533    moveDirection(90, 255,  true);
534  }
535}
536
537void autonomous(void){
538  unsigned long currentMillis  = millis();
539  ++numberOfLoops; // used to make checkSurroundings and checkNearby  which run periodically
540
541  /*valPot = analogRead(POTPIN);
542    motorSpeedPot  = (int)(valPot / 4.01); // ratio to convert pot reading to analog
543    if (motorSpeedPot  > 255){ // ensures max speed is 255 and min speed is 0
544    motorSpeedPot = 255;
545    }
546    else if (motorSpeedPot < 0){
547    motorSpeedPot = 0;
548    }*/
549  motorSpeedPot = 255; // valPot changed and varied around 255, sometimes going  to zero and stopping autonomous car functions
550  fractionOfFullSpeed = ((double)motorSpeedPot)  / 255.0;
551  turnTime = (int)(1350.0 / fractionOfFullSpeed); // for carpet 1350
552
553  backwardsMotion = false;
554  cmDistance = ping(echoUS);
555  currentCm = cmDistance;
556  double randomTurnDegreeRadius = (double)(random(12, 37));
557  
558  if (currentCm  <= 8) { // if object within 8 cm, robot can't do anything other than kick to defend  itself, then turns stepper 360 degrees and moves to that position
559    Serial.print("Dis:  "); Serial.print(currentCm); Serial.print("\
560");    
561    kick(false);
562    backwardsMotion = true;
563    moveDirection(90, motorSpeedPot, 2 * turnTime,  backwardsMotion); // back up robot
564    halt(WAITTIME);
565    stepperCircleDegreeCheck();
566    kick(false);
567    int turnTimeFarthest = (turnTime / 90) * degreeMaxDistanceStepper;
568   // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\
569");
570    backwardsMotion = false;
571    moveDirection(180, motorSpeedPot, turnTimeFarthest,  backwardsMotion); // turn to degree where saw farthest with U.S. circle rotation  from stepper
572  }
573  else if (currentCm <= 16) { // if object within 8 to 16  cm
574    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\
575");    
576    kick(false);
577    backwardsMotion = true;
578    moveDirection(90,  motorSpeedPot, 4 * turnTime / 3, backwardsMotion); 
579    halt(WAITTIME);
580    stepDegree(-1  * STEPPERDEGREERADIUS); 
581    stepperDegreeCheck(0, 2 * STEPPERDEGREERADIUS, -1  * STEPPERDEGREERADIUS, 600);
582    int turnTimeInitial = (turnTime / 90) * (STEPPERDEGREERADIUS);  // time for motors to turn depending on which angle stepper says max distance is
583    backwardsMotion = false;
584    // turn to degree where saw farthest with U.S.  circle rotation from stepper  
585    if (degreeMaxDistanceStepper < STEPPERDEGREERADIUS){  // for east side of measurements 
586      turnTimeFarthest = (turnTime / 90) *  (STEPPERDEGREERADIUS - degreeMaxDistanceStepper); // calculate turnTimeFarthest  (stepper only uses positive degreees), and drive motors to either direction      
587     // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\
588");
589      moveDirection(0, motorSpeedPot, turnTimeFarthest, backwardsMotion); 
590    }
591    else if (degreeMaxDistanceStepper > STEPPERDEGREERADIUS){ // for west side of  measurements
592      turnTimeFarthest = (turnTime / 90) * (degreeMaxDistanceStepper  - STEPPERDEGREERADIUS);   
593     // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest);  Serial.print("\
594");
595      moveDirection(180, motorSpeedPot, turnTimeFarthest,  backwardsMotion);
596    }
597  }
598  else if (currentCm <= 28) { // checkSurroundings  if object within 16 to 28 
599    Serial.print("Dis: "); Serial.print(currentCm);  Serial.print("\
600");
601    checkSurroundings();
602  }
603  else if (currentCm  <= 44) { // checkNearby if object within 28 to 44 cm to find a slight deviation
604    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\
605");
606    checkNearby(randomTurnDegreeRadius);
607  }
608  else if (currentCm <= 53) {  // pathClear false between 44 to 53 cm so robot rotates counterclockwise
609    Serial.print("Dis:  "); Serial.print(currentCm); Serial.print("\
610");
611    pathClear = false;
612  }
613  else {
614    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\
615");
616   // Serial.print("Speed: "); Serial.print(motorSpeedPot); Serial.print("\
617");
618    pathClear = true; // goes straight if no object within 52 cm is detected
619  }
620  
621  // interval of repeat is random within boundaries
622  if (numberOfLoops  % random(70, 141) == 0) { // checkSurroundings once every random or 140 loops
623    checkSurroundings(); // sometimes doesn't run
624  }
625  else if (numberOfLoops  % random(35, 71) == 0) { // once every random or 35 loops, checks +-15 degrees or  random setting
626    randomTurnDegreeRadius = (double)(random(18, 37)); // average  of 27 degrees turn radius for three distance checks
627    checkNearby(randomTurnDegreeRadius);
628  }
629  else {
630    moveDirection(90, motorSpeedPot, pathClear, backwardsMotion,  56, 7); // drive north at motorSpeedPot if pathClear with background stepper turning  over radius of 56 degrees with 8-7 degree increments
631  }
632}
633
634void volumeUpIRDirControl(void){
635  if (controlDirVolume <= (180 - controlDirVolIncrement)){ // increment 5 degrees,  or specified increment, every time volume up pressed until 180 degrees
636    controlDirVolume  += controlDirVolIncrement;
637    controlDir = controlDirVolume;
638  }
639  else{
640    controlDir = 180;
641  }
642}
643
644void volumeDownIRDirControl(void){ // not  using volume down key, called when channel up key pressed
645  if (controlDirVolume  >= controlDirVolIncrement){ // decrease specific degree increment
646    controlDirVolume  -= controlDirVolIncrement;
647    controlDir = controlDirVolume;  
648    }
649  else{
650    controlDir = 0;
651  }
652}
653
654void remoteInput(void) { // data  of ir reciever from ir transmitor remote from sony tv remote
655  manualStepper  = false; // only true when their specific button pushed
656  buttonDriverOverride  = false; 
657  switch (results.value) // robot actions for pushing buttons 
658  { 
659    case 0xA90: controlSpeed = 0;                     controlBackwards =  false;             stepDegree(-1 * stepDisplacement); stepDisplacement = 0; autoOff  = !(autoOff); break; // power button: change between autopilot and manual modes
660    case 0x910: controlSpeed = 0;                                                                                                                                          break;  // 0 button: stop nxt motion
661    case 0x710: controlSpeed = 0;                                                                                                                                          break;  // guide button: stop nxt motion (same as above)
662    case 0x110: controlSpeed  = 255; controlDir = 0;                                                                                                                        break;  // 9 button: 0 degrees
663    case 0xA10: controlSpeed = 255; controlDir = 30;                                                                                                                       break;  // 6 button: 30 degrees
664    case 0x410: controlSpeed = 255; controlDir = 60;                                                                                                                       break;  // 3 button: 60 degrees
665    case 0x810: controlSpeed = 255; controlDir = 90;                                                                                                                       break;  // 2 button: 90 degrees (straight)
666    case 0x10:  controlSpeed = 255; controlDir  = 120;                                                                                                                      break;  // 1 button: 120 degrees
667    case 0xC10: controlSpeed = 255; controlDir = 150;                                                                                                                      break;  // 4 button: 150 degrees
668    case 0x610: controlSpeed = 255; controlDir = 180;                                                                                                                      break;  // 7 button: 180 degrees
669    case 0x210: controlSpeed = 255; controlDir = 90;  controlBackwards = false;                                                                                            break;  // 5 button: north (forward)
670    case 0xE10: controlSpeed = 255; controlDir =  90;  controlBackwards = true;                                                                                             break;  // 8 button: south (backward) 
671    case 0x2D0: controlSpeed = 255;                                                         volumeUpIRDirControl();                                                        break; // left arrow button:  increase left turn angle
672    case 0xCD0:  controlSpeed = 255;                                                        volumeDownIRDirControl();                                                      break; // vol down button:  kick servo forward
673    case 0x2F0: controlSpeed = 255; controlDir = 90;                                        controlDirVolume  = 90;                                                         break; // up arrow  button: reset volume turn degree to forward (90 degrees)    
674    case 0xC90:  controlSpeed = 0;                                                           kick(false);                                                                   break; // right  arrow button: increase right turn angle
675    case 0x890: controlSpeed = 0;                                                           kick(true);                                                                    break; // ch  down button: kick servo backward
676    case 0x490: controlSpeed = 0;                                                           manualStepper  = true; stepperClockwise = false;                                break; // volume  up button: turn stepper counterclockwise
677    case 0x90: controlSpeed = 0;                                                            manualStepper  = true; stepperClockwise = true;                                 break; // channel  up button: turn stepper clockwise
678    case 0x3B0: controlSpeed = 0;                                                           stepDegree(-1  * stepDisplacement); stepDisplacement = 0;                       break; // freeze  button: reset ultrasonic sensor to facing forward by moving stepper
679    case  0xA70: controlSpeed = 0;                     controlBackwards = !controlBackwards;                                                                                break;  // center of arrows button: change directions from backwards to forwards and vise  versa
680    case 0x270: controlSpeed = 0;                                                           buttonDriverOverride  = true;                                                   break; // picture button:  autonomous movemment dependent only on push buttons on bumpers  
681  }
682  irrecv.resume();
683}
684
685void  loop(void) {
686  stepper.setSpeed(450); // max speed around 700, various functions  manipulate stepper speed
687  if (digitalRead(BUTTONFORWARDONE) == LOW || digitalRead(BUTTONFORWARDTWO)  == LOW){ // read button state (pushed or not)
688    forwardButtonControl = true;
689  }
690  else if (digitalRead(BUTTONBACKWARDONE) == LOW || digitalRead(BUTTONBACKWARDTWO)  == LOW){
691    backwardButtonControl = true;
692  }
693  if (!autoOff) { // if  autonomous, check for remoteInput or run autonomous mode
694    if (irrecv.decode(&results))  {
695      remoteInput();
696    }
697    else {
698      if (forwardButtonControl){  // if push button pushed, autonomous reacts quickly, else runs autonomous 
699        forwardObstacleButton();
700        forwardButtonControl = false;
701      }
702      else if (backwardButtonControl){  
703        backwardObstacleButton();
704        backwardButtonControl = false;
705      }
706      else{
707        autonomous();        
708      }
709    }
710  }
711  else if (autoOff) { // if autonomous off, check for another remoteInput  or run based on previous remoteInput
712    if (irrecv.decode(&results)) {
713      remoteInput();
714    }
715    else {
716      if (!buttonDriverOverride){
717        if (!manualStepper)  { // rotate stepper if stated, or run remoteInput task
718          moveDirection(controlDir,  controlSpeed, controlBackwards);
719        }
720        else if (manualStepper)  { 
721          if (stepperClockwise) {
722            stepDegree(-1.0);
723            --stepDisplacement;
724          }
725          else if (!stepperClockwise) { 
726            stepDegree(1.0);
727            ++stepDisplacement;
728          }
729        }
730      }
731      else  if (buttonDriverOverride){
732        buttonDriver();
733      }
734    }
735  }
736}
737
738int  ping(byte echoUS)
739{
740  long duration;
741  long cm;
742  pinMode(TRIGUS, OUTPUT);
743  digitalWrite(TRIGUS, LOW); // give short low pulse to ensure clean high pulse
744  delayMicroseconds(2);
745  digitalWrite(TRIGUS, HIGH);
746  delayMicroseconds(5);
747  digitalWrite(TRIGUS, LOW);
748  pinMode(echoUS, INPUT);
749  duration = pulseIn(echoUS,  HIGH, 10000);  // limits pulse check to 10 ms avoid stuttering (bug due to ultrasonic  sensor not recieving pulse back on long distances since not strong enough)
750  cm  = microsecondsToCentimeters(duration);
751  if (cm == 0) {
752    cm = 200; // returns  high distance of 200 if nothing found, actual zero cm is never read, lowest reading  seems to be around 3 cm
753  }
754  return (int)cm ; // int type used with ping  to determine cmDistance
755}
756
757long microsecondsToCentimeters(long microseconds)
758{
759  return microseconds / 58; // convert time to distance
760}
761
762// TO DO/ FIX:
763//  function which allows robot to go back to starting place (integrator) constantly  calculating 2d position
764// fix potentiometer analogIn: not consistent values,  leads to one motor off, only controls speed of one motor when used, try interrupt
765//  l9110 motor only drives one motor with analog values and one motor with digital  values (even when analog in code)
766// make millis non blocking: run stepper at  same time as nxt motors, not rapid alteration
767// test to find exact number for  steps to make a circle- less than 2048 when fast rotation with no measurements;  then 2048 when making measurements
768// find a way to know if motors hung up and  not rotating- find power consumption
769// in remote mode, doesn't stop moving backwards  when stepper pressed
770// recenter robot to degree found when moving staight and  sees object, then run avoidance task, backDir = false
771// use bluetooth module  to tranfer data