Components and supplies
Through Hole Resistor, 470 ohm
High Brightness LED, White
Custom PCB
L9110S H Bridge Stepper Motor Dual DC Driver
Tactile Switch, Top Actuated
Alligator Clips
Arduino UNO
5 mm LED: Red
SG90 Micro-servo motor
Rotary potentiometer (generic)
HX1838 Universal IR Infrared Remote Control Receiver Module
Ultrasonic Sensor - HC-SR04 (Generic)
Mindstorms NXT Programming Brick / Kit
Jumper wires (generic)
Resistor 10k ohm
28YBJ-48 DC 5V 4 Phase 5 Wire Stepper Motor With ULN2003 Driver Board
Tools and machines
Wire Stripper & Cutter, 18-10 AWG / 0.75-4mm² Capacity Wires
Mastech MS8217 Autorange Digital Multimeter
Soldering iron (generic)
Solder Wire, Lead Free
Multitool, Screwdriver
Apps and platforms
Arduino IDE
Project description
Code
Autonomous Controllable LEGO Car Code
arduino
Code for car. There are still some issues to be fixed, and some other potential changes, which I am working on (these can be seen in the comments located at the end).
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
Comments
Only logged in users can leave comments