achyuta_yadunandan
Published © GPL3+

Autonomous Controllable LEGO Car

Autonomous car, built from a LEGO frame, which responds to nearby objects and can also be manually controlled via remote control.

IntermediateShowcase (no instructions)104
Autonomous Controllable LEGO Car

Things used in this project

Hardware components

Arduino UNO & Genuino UNO
Arduino UNO & Genuino UNO
A Mega board could be used since the current setup with the UNO uses all the pins with no room for additional changes.
×1
NXT Programming Brick / Kit
LEGO MindStorms NXT Programming Brick / Kit
This contains all the bricks to build the frame, as well as the two motors used to drive the car. 3d printed frames or rc toy car frames can be used alternatives to the LEGO frame and brushed DC motors can be used as alternatives to the two LEGO nxt motors.
×1
Ultrasonic Sensor - HC-SR04 (Generic)
Ultrasonic Sensor - HC-SR04 (Generic)
×1
28YBJ-48 DC 5V 4 Phase 5 Wire Stepper Motor With ULN2003 Driver Board
Used to rotate the ultrasonic sensor up to 360 degrees and hold position
×1
SG90 Micro-servo motor
SG90 Micro-servo motor
Used to make the "kick" action
×1
L9110S H Bridge Stepper Motor Dual DC Driver
Alternate can be two individual l9110 ic's
×1
HX1838 Universal IR Infrared Remote Control Receiver Module
An ir transmitter can also be bought. I used a Sony TV remote as my transmitter.
×1
Custom PCB
Custom PCB
Minimum 5 * 10 grid to create bumpers
×2
Tactile Switch, Top Actuated
Tactile Switch, Top Actuated
Used in case ultrasonic sensor fails due to high angle reading
×4
High Brightness LED, White
High Brightness LED, White
For front bumper
×1
5 mm LED: Red
5 mm LED: Red
For back bumper
×1
Rotary potentiometer (generic)
Rotary potentiometer (generic)
For speed control
×1
Through Hole Resistor, 470 ohm
Through Hole Resistor, 470 ohm
For the 2 LEDs on both the front and back bumpers
×2
Resistor 10k ohm
Resistor 10k ohm
For each button (2 on each bumper)
×4
Jumper wires (generic)
Jumper wires (generic)
×20
Alligator Clips
Alligator Clips
×6

Software apps and online services

Arduino IDE
Arduino IDE

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)
Solder Wire, Lead Free
Solder Wire, Lead Free
Wire Stripper & Cutter, 18-10 AWG / 0.75-4mm² Capacity Wires
Wire Stripper & Cutter, 18-10 AWG / 0.75-4mm² Capacity Wires
Multitool, Screwdriver
Multitool, Screwdriver
Mastech MS8217 Autorange Digital Multimeter
Digilent Mastech MS8217 Autorange Digital Multimeter
Helpful to check connections

Story

Read more

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).
#include "Servo.h"
#include "IRremote.h"
#include "Stepper.h"
#define POTPIN 1
#define IRRECIEVER 2
#define NXTMOTORONE 6 // two directions for each motor using motor driver l9110
#define NXTMOTORTWO 10
#define NXTMOTORONEBACK 5
#define NXTMOTORTWOBACK 9
#define WAITTIME 150 // time stopped in each of 4 directions or used for setting motors speeds to zero
#define STEPPERDEGREERADIUS 135
#define TRIGUS 13 // sends out U.S. pulse 
#define BUTTONFORWARDONE A4 // push buttons uses as touch sensors
#define BUTTONFORWARDTWO A5
#define BUTTONBACKWARDONE A2
#define BUTTONBACKWARDTWO A3
byte echoUS = 3; // recieves U.S. pulse and uses time elapsed to determine distance
long numberOfLoops = 0; // used to run specific autonomous functions periodically such as checkNearby() and checkSurroundings()
int cmDistance = 0; // uses ping U.S. function to determine distance
int currentCm = 0;
int turnTime; // 90 degree turn dependent on surface
double valPot; // value from potentiometer from 0 - 1023
int motorSpeedPot; // valPot converted to analog value for motors from 0 - 255
double fractionOfFullSpeed;
boolean pathClear = false; // returns true if object within 65 cm and can cause clockwise rotation, else no changes to straight path
boolean backwardsMotion = false; // control of either driving forward or backward
int controlSpeed = 255; // used in remote control with ir remote and reciever, default: fast, straight, and forward
int controlDir = 90;
int controlDirVolume = 90;
int controlDirVolIncrement = 10;
boolean controlBackwards = false;
boolean autoOff = false; // stores two possibilities of drive mode- autonomous or manual control (from ir remote)
int distanceStepper[360]; // array to store maximum of distance from U.S. in each degree in a circle when rotated using stepper motor
int maxDistanceStepper;
int degreeMaxDistanceStepper;
boolean manualStepper; // ir remote control of stepper motor
boolean stepperClockwise = false;
int turnTimeFarthest; // time for one nxt motor to rotate car to allign with degree which corresponds to farthest valid distance
int stepperRadiusCount = 0; // always turn stepper over input radius while moving forward 
int stepDisplacement = 0; // used to find current displacement and revert stepper to forward facing if needed
boolean stepperTurnDirection = false;
int sameDegreeCount; // used to check for repetitive distance readings and holds the longest set and associated degree in variables
int sameDegreeCountMax;
int sameDegreeValueMax; 
boolean forwardButtonControl = false; // push buttons to trigger overriding backward or forward movement when car crashes
boolean backwardButtonControl = false;
boolean buttonDriverOverride = false; // for autonomous control using only buttons, no U.S., activated using 4 on ir remote
boolean buttonDriveForward = true;
byte buttonCycleCount = 0;
Servo myServo; 
IRrecv irrecv(IRRECIEVER);
decode_results results;
Stepper 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

void setup(void) {
  Serial.begin(57600);
  Serial.setTimeout(1);
  irrecv.enableIRIn(); // Start the receiver  
  myServo.attach(4);
  myServo.write(90); // starts servo at 90 degrees
  pinMode(NXTMOTORONE, OUTPUT); // motors connected to output pins which support analog, forward drive motors
  pinMode(NXTMOTORTWO, OUTPUT);
  pinMode(NXTMOTORONEBACK, OUTPUT); // backward drive motors
  pinMode(NXTMOTORTWOBACK, OUTPUT);
  pinMode(TRIGUS, OUTPUT); // send sound pulse every 5 ms
  pinMode(echoUS, INPUT);  // detects time for sound pulse to return
  pinMode(BUTTONFORWARDONE, INPUT);
  pinMode(BUTTONFORWARDTWO, INPUT);
  pinMode(BUTTONBACKWARDONE, INPUT);
  pinMode(BUTTONBACKWARDTWO, INPUT);
  digitalWrite(NXTMOTORONEBACK, LOW); // since input pullup has resistor at voltage, starts high and needs to be written low
  digitalWrite(NXTMOTORTWOBACK, LOW);
}

void forwardObstacleButton(void){ // go backwards, then rotate 180 degrees
  halt(WAITTIME);
  backwardsMotion = true;
  moveDirection(90, motorSpeedPot, 2 * turnTime, backwardsMotion);
  moveDirection(0, motorSpeedPot, 5 * turnTime / 2, backwardsMotion);
  backwardsMotion = false; 
}

void backwardObstacleButton(void){ // go forward, then turn around
  halt(WAITTIME);
  backwardsMotion = false;
  moveDirection(90, motorSpeedPot, 2 * turnTime, backwardsMotion);
  moveDirection(0, motorSpeedPot, 2 * turnTime, backwardsMotion);
}

void stepDegree(double degree) { // can turn stepper to any degree in circle in both directions
  stepper.step((int)(1800.0 * degree / 360.0)); // 1800 provides more accurate turning than the stated 2048 
}

void stepperCircleDegreeCheck(void) { // take U.S. measurements every degree over the course of a whole circle
  stepDegree( -1 * stepDisplacement); // ensure starting ultrasonic measurements from forward facing
  stepDisplacement = 0;  
  for (int i = 0; i <= 360; ++i) {
    cmDistance = ping(echoUS);
    distanceStepper[i] = cmDistance; // stores each degree in 360 length array
   // Serial.print("Dis at "); Serial.print(i); Serial.print(" deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm"); Serial.print("\n");
    stepDegree(1.0);
    delay(17); // takes maximum of 10 ms for ultrasonic reading
  }
  delay(100);
  sameDegreeCount = 0;  
  sameDegreeCountMax = 0; 
  maxDistanceStepper = distanceStepper[0];    
  degreeMaxDistanceStepper = 0;
  for (int i = 1; i <= 360; ++i) { // if greater, replace max
    if (distanceStepper[i] > maxDistanceStepper) {
      maxDistanceStepper = distanceStepper[i];
      degreeMaxDistanceStepper = i;
      sameDegreeCount = 0;
    }
    else if (distanceStepper[i] == maxDistanceStepper) { // if equal, count how many times degree equality is held (how many degrees is farthest distance constant) 
      ++sameDegreeCount;
      if (sameDegreeCount > sameDegreeCountMax)
      {
        sameDegreeCountMax = sameDegreeCount;
        sameDegreeValueMax = i;
      }
    }
    else if (distanceStepper[i] < maxDistanceStepper){
      sameDegreeCount = 0;
    }
  }
  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)
    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
  }
 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\n"); Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\n"); Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\n");
  Serial.print("Max dis: "); Serial.print(maxDistanceStepper); Serial.print(" cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\n");
  stepDegree(-360.0); // return to original position
  // stepDegree(degreeMaxDistanceStepper); // returns to degree where farthest U.S. reading was taken
}

void stepperDegreeCheck(int lowerBound, int higherBound){ // U.S. uses stepper to record each degree within bounds
  stepDegree( -1 * stepDisplacement); 
  stepDisplacement = 0;  
  stepDegree(lowerBound); // position stepper to start at lower bound
  for (int i = lowerBound; i <= higherBound; ++i) { // goes to lower bound, then takes one degree incremental measurements until higher bound
    cmDistance = ping(echoUS);
    distanceStepper[i] = cmDistance;
   // Serial.print("Dis at: "); Serial.print(i); Serial.print(" deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm"); Serial.print("\n");
    stepDegree(1.0);
    delay(17);
  }
  delay(100);
  sameDegreeCount = 0;  
  sameDegreeCountMax = 0; 
  maxDistanceStepper = distanceStepper[0];    
  degreeMaxDistanceStepper = 0;
  for (int i = 1; i <= 360; ++i) { 
    if (distanceStepper[i] > maxDistanceStepper) {
      maxDistanceStepper = distanceStepper[i];
      degreeMaxDistanceStepper = i;
      sameDegreeCount = 0;
    }
    else if (distanceStepper[i] == maxDistanceStepper) { 
      ++sameDegreeCount;
      if (sameDegreeCount > sameDegreeCountMax)
      {
        sameDegreeCountMax = sameDegreeCount;
        sameDegreeValueMax = i;
      }
    }
    else if (distanceStepper[i] < maxDistanceStepper){
      sameDegreeCount = 0;
    }
  }
  if (distanceStepper[sameDegreeValueMax] == maxDistanceStepper){
    degreeMaxDistanceStepper = sameDegreeValueMax - sameDegreeCountMax / 2; 
  }
 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\n"); Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\n"); Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\n");
  Serial.print("Dis max: "); Serial.print(maxDistanceStepper); Serial.print(" cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\n");
  stepDegree(-1 * higherBound); // returns to original position
}

void stepperDegreeCheck(int lowerBound, int higherBound, int manualDegreeReturn, int speedControl){ // adds control of degrees returns to avoid extra waiting, along with stepper speed control
  stepper.setSpeed(speedControl);
  stepDegree( -1 * stepDisplacement); 
  stepDisplacement = 0;
  stepDegree(lowerBound);
  for (int i = lowerBound; i <= higherBound; ++i) { // creates array of U.S. measurement values between limits
    cmDistance = ping(echoUS);
    distanceStepper[i] = cmDistance;
   // Serial.print("Dis at: "); Serial.print(i); Serial.print(" deg: "); Serial.print(distanceStepper[i]); Serial.print(" cm"); Serial.print("\n");
    stepDegree(1.0);
    delay(17);
  }
  delay(100);
  sameDegreeCount = 0;  
  sameDegreeCountMax = 0; 
  maxDistanceStepper = distanceStepper[lowerBound];    
  degreeMaxDistanceStepper = lowerBound;
  for (int i = ++lowerBound; i <= higherBound; ++i) { 
    if (distanceStepper[i] > maxDistanceStepper) {
      maxDistanceStepper = distanceStepper[i];
      degreeMaxDistanceStepper = i;
      sameDegreeCount = 0;
    }
    else if (distanceStepper[i] == maxDistanceStepper) {  
      ++sameDegreeCount;
      if (sameDegreeCount > sameDegreeCountMax)
      {
        sameDegreeCountMax = sameDegreeCount;
        sameDegreeValueMax = i;
      }
    }
    else if (distanceStepper[i] < maxDistanceStepper){
      sameDegreeCount = 0;
    }
  }
  if (distanceStepper[sameDegreeValueMax] == maxDistanceStepper){ 
    degreeMaxDistanceStepper = sameDegreeValueMax - sameDegreeCountMax / 2; 
  }
 // Serial.print("sameDegreeCount: "); Serial.print(sameDegreeCount); Serial.print("\n"); Serial.print("sameDegreeCountMax: "); Serial.print(sameDegreeCountMax); Serial.print("\n"); Serial.print("sameDegreeValueMax: "); Serial.print(sameDegreeValueMax); Serial.print("\n");
  Serial.print("Dis max: "); Serial.print(maxDistanceStepper); Serial.print(" cm at deg "); Serial.print(degreeMaxDistanceStepper); Serial.print("\n");
  stepDegree(manualDegreeReturn); // goes to specified end position
}

void kick(boolean leftKick) {
  halt(WAITTIME); // halt motion to kick
  if (leftKick) {
    myServo.write(115); // backs up 25 degrees, delay 1/4 seconds, kicks to full, hold for one second
    delay(250);
    myServo.write(0);
    delay(1000);
  }
  else if (!leftKick) { // performs right kick
    myServo.write(65);
    delay(250);
    myServo.write(180);
    delay(1000);
  }
  myServo.write(90); // resets servo to 90 (pointed up)
}

void halt(int duration) {
  unsigned long previousMillis = millis();
  while ((previousMillis + duration) >= millis()) {
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    digitalWrite(NXTMOTORONEBACK, LOW);
    digitalWrite(NXTMOTORTWOBACK, LOW);
  }
}

void motorHigh(boolean backwards) {
  if (!backwards){
    digitalWrite(NXTMOTORONE, HIGH);
    digitalWrite(NXTMOTORTWO, HIGH);
    digitalWrite(NXTMOTORONEBACK, LOW);
    digitalWrite(NXTMOTORTWOBACK, LOW);    
  }
  else if (backwards){
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    digitalWrite(NXTMOTORONEBACK, HIGH);
    digitalWrite(NXTMOTORTWOBACK, HIGH);        
  }
}

void 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
  // if degree is 0, left wheel turns (clockwise), if degree is 180, right wheel turns (counterclockwise)
  double totalDelay = 250.0; // combined motor delay
  double ratioLeft = ((double)(turnDegree)) / 180.0; // degree determines how totalDelay is split up for each motor
  double ratioRight = 1.0 - ratioLeft;
  int delayPtOne = (int)(ratioRight * totalDelay);
  int delayPtTwo = (int)(ratioLeft * totalDelay);
  if (!backwards) {
    digitalWrite(NXTMOTORONEBACK, LOW); // run both motors for lowest time, then run only one motor for the remaining time needed for that ratio
    digitalWrite(NXTMOTORTWOBACK, LOW);
    analogWrite(NXTMOTORONE, speedIntensity);
    analogWrite(NXTMOTORTWO, speedIntensity);
    if (delayPtOne > delayPtTwo) {
      delay(delayPtTwo);
      digitalWrite(NXTMOTORTWO, LOW);
      delay(delayPtOne - delayPtTwo);
    }
    else if (delayPtOne < delayPtTwo) {
      delay(delayPtOne);
      digitalWrite(NXTMOTORONE, LOW);
      delay(delayPtTwo - delayPtOne);
    }
    else if (delayPtOne == delayPtTwo) {
      delay(totalDelay);
    }
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    delay(1);
  }
  else if (backwards) {
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    analogWrite(NXTMOTORONEBACK, speedIntensity);
    analogWrite(NXTMOTORTWOBACK, speedIntensity);
    if (delayPtOne > delayPtTwo) {
      delay(delayPtTwo);
      digitalWrite(NXTMOTORTWOBACK, LOW);
      delay(delayPtOne - delayPtTwo);
    }
    else if (delayPtOne < delayPtTwo) {
      delay(delayPtOne);
      digitalWrite(NXTMOTORONEBACK, LOW);
      delay(delayPtTwo - delayPtOne);
    }
    else if (delayPtOne == delayPtTwo) {
      delay(totalDelay);
    }
    digitalWrite(NXTMOTORONE, LOW);
    digitalWrite(NXTMOTORTWO, LOW);
    delay(1);
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards) { // only runs if pathClear
  if (pathClear) {
    moveDirection(turnDegree, speedIntensity, backwards);
  }
  else {
    moveDirection(0, speedIntensity, backwards);
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards, int stepperTurnRadius) { // runs motors under path clear while turning stepper over input radius, not stimultaneosly
  stepper.setSpeed(700);
  moveDirection(turnDegree, speedIntensity, pathClear, backwards); // alternate between moving motors forward and turning stepper motor
  if (stepperRadiusCount < stepperTurnRadius){ // turn +- stepperTurnRadius from forward facing
    if (stepperTurnDirection){
      stepDegree(1.0); // use small stepper increments so that autonomous can run for most of the time 
    }
    else if (!stepperTurnDirection){
      stepDegree(-1.0);
    }
    ++stepperRadiusCount;
  }
  else if (stepperRadiusCount < 2 * stepperTurnRadius){
    if (!stepperTurnDirection){
      stepDegree(1.0);
    }
    else if (stepperTurnDirection){
      stepDegree(-1.0);
    }
    ++stepperRadiusCount;
  }
  else{
    stepperRadiusCount = 0;
    stepperTurnDirection = !stepperTurnDirection;
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, boolean backwards, int stepperTurnRadius, int stepperStepIndividual) { // adds step feature to move specified degrees each time
  stepper.setSpeed(700);
  moveDirection(turnDegree, speedIntensity, pathClear, backwards); 
  motorHigh(backwards);
  if (stepperRadiusCount < stepperTurnRadius){ 
    if (stepperTurnDirection){
      stepDegree(stepperStepIndividual); // use specified degree increment
      stepDisplacement += stepperStepIndividual;  
    }
    else if (!stepperTurnDirection){
      stepDegree(-1.0 * stepperStepIndividual);
      stepDisplacement += (-1 * stepperStepIndividual);
    }
    stepperRadiusCount += stepperStepIndividual;
  }
  else if (stepperRadiusCount < (2 * stepperTurnRadius)){
    if (!stepperTurnDirection){
      stepDegree(stepperStepIndividual);
      stepDisplacement += stepperStepIndividual;  
    }
    else if (stepperTurnDirection){
      stepDegree(-1.0 * stepperStepIndividual);
      stepDisplacement += (-1 * stepperStepIndividual);
    }
    stepperRadiusCount += stepperStepIndividual;
  }
  else{
    stepperRadiusCount = 0;
    stepperTurnDirection = !stepperTurnDirection;
  }
 // Serial.print("stepperRadiusCount: "); Serial.print(stepperRadiusCount); Serial.print("\n");
}

void moveDirection(int turnDegree, int speedIntensity, int duration, boolean backwards) { // runs for specific time duration regardless of pathClear
  unsigned long previousMillis = millis();
  while ((previousMillis + duration) >= millis()) {
    moveDirection(turnDegree, speedIntensity, backwards);
  }
}

void moveDirection(int turnDegree, int speedIntensity, boolean pathClear, int duration, boolean backwards) { // runs for specific time duration if pathClear
  unsigned long previousMillis = millis();
  while ((previousMillis + duration) >= millis()) {
    moveDirection(turnDegree, speedIntensity, pathClear, backwards);
  }
}

void checkSurroundings(void) {
  halt(WAITTIME); // stop motor rotation
  stepDegree( -1 * stepDisplacement); // allign stepper to forward facing before recording data
  stepDisplacement = 0;
  int cmDistanceNorth = ping(echoUS);
  int cmDistanceMax = cmDistanceNorth;
  String dirMaxDis = "NORTH"; // max distance direction stored in string
 // Serial.print("Dis no: "); Serial.print(cmDistanceNorth); Serial.print("\n");
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion); // rotate 1/4 of a circle
  halt(WAITTIME);   // temporarily stop to ensure precise measurement
  int cmDistanceWest = ping(echoUS);
 // Serial.print("Dis we: "); Serial.print(cmDistanceWest); Serial.print("\n");
  if (cmDistanceWest > cmDistanceMax) {
    cmDistanceMax = cmDistanceWest;
    dirMaxDis = "WEST"; // dirMaxDis replaced if needed
  }
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceSouth = ping(echoUS);
 // Serial.print("Dis so: "); Serial.print(cmDistanceSouth); Serial.print("\n");
  if (cmDistanceSouth > cmDistanceMax) {
    cmDistanceMax = cmDistanceSouth;
    dirMaxDis = "SOUTH";
  }
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceEast = ping(echoUS);
 // Serial.print("Dis ea: "); Serial.print(cmDistanceEast); Serial.print("\n");
  if (cmDistanceEast > cmDistanceMax) {
    cmDistanceMax = cmDistanceEast;
    dirMaxDis = "EAST";
  }
  moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  halt(WAITTIME);   // back to facing north
  Serial.print("Dis max: "); Serial.print(cmDistanceMax); Serial.print(" in "); Serial.print(dirMaxDis); Serial.print("\n");
  if (dirMaxDis.equals("NORTH")) { // returns to distance with obstacle farthest in sight by associating dirMaxDis with direction
    halt(WAITTIME);
  }
  else if (dirMaxDis.equals("WEST")) {
    moveDirection(180, motorSpeedPot, turnTime, backwardsMotion);
  }
  else if (dirMaxDis.equals("SOUTH")) {
    moveDirection(180, motorSpeedPot, 2 * turnTime, backwardsMotion);
  }
  else if (dirMaxDis.equals("EAST")) {
    moveDirection(180, motorSpeedPot, 3 * turnTime, backwardsMotion);
  }
}

void checkNearby(double checkDegreeRadius) { // takes three measurements, straight, + checkDegreeRadius, - checkDegreeRadius 
  halt(WAITTIME);
  int rotationTime = (int)(((double)turnTime / 90.0) * checkDegreeRadius); // turnTime / 90 = time to turn 1 degree, multiply by input degree to find total rotation time
  stepDegree( -1 * stepDisplacement); 
  stepDisplacement = 0;
  int cmDistanceStraight = ping(echoUS);
  int cmSideDistanceMax = cmDistanceStraight;
  String sideMaxDis = "STRAIGHT";
 // Serial.print("Dis str: "); Serial.print(cmDistanceStraight); Serial.print("\n");
  moveDirection(180, motorSpeedPot, rotationTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceLeft = ping(echoUS);
 // Serial.print("Dis left: "); Serial.print(cmDistanceLeft); Serial.print("\n");
  if (cmDistanceLeft > cmSideDistanceMax) {
    cmSideDistanceMax = cmDistanceLeft;
    sideMaxDis = "LEFT";
  }
  moveDirection(0, motorSpeedPot, 2 * rotationTime, backwardsMotion);
  halt(WAITTIME);
  int cmDistanceRight = ping(echoUS);
 // Serial.print("Dis right: "); Serial.print(cmDistanceRight); Serial.print("\n");
  if (cmDistanceRight > cmSideDistanceMax) {
    cmSideDistanceMax = cmDistanceRight;
    sideMaxDis = "RIGHT";
  }
  moveDirection(180, motorSpeedPot, rotationTime, backwardsMotion);
  halt(WAITTIME);
  Serial.print("Dis max: "); Serial.print(cmSideDistanceMax); Serial.print(" in "); Serial.print(sideMaxDis); Serial.print("\n");
  if (sideMaxDis.equals("STRAIGHT")) { // goes either straight, or left or right degree input depending on where the farthest distance was measured
    halt(WAITTIME);
  }
  else if (sideMaxDis.equals("LEFT")) {
    moveDirection(180, motorSpeedPot, rotationTime, backwardsMotion);
  }
  else if (sideMaxDis.equals("RIGHT")) {
    moveDirection(0, motorSpeedPot, rotationTime, backwardsMotion);
  }
}

void buttonDriver(void){ // auto control using only buttons
  if (digitalRead(BUTTONFORWARDONE) == LOW || digitalRead(BUTTONFORWARDTWO) == LOW){ // checks if any push button pushed
    buttonDriveForward = false;
    ++buttonCycleCount;
    if (buttonCycleCount % 4 == 0){ // turn every 2 button pushes
      moveDirection(180, 255, 2 * turnTime, false); 
      buttonCycleCount = 0;
    }
  }
  else if (digitalRead(BUTTONBACKWARDONE) == LOW || digitalRead(BUTTONBACKWARDTWO) == LOW){
    buttonDriveForward = true;
    ++buttonCycleCount;
    if (buttonCycleCount % 2 == 0){
      moveDirection(0, 255, turnTime, false); 
      buttonCycleCount = 0;
    }
  }
  if (buttonDriveForward){ // move in opposite direction of push after pushed impulse
    moveDirection(90, 255, false);
  }
  else if (!buttonDriveForward){
    moveDirection(90, 255, true);
  }
}

void autonomous(void){
  unsigned long currentMillis = millis();
  ++numberOfLoops; // used to make checkSurroundings and checkNearby which run periodically

  /*valPot = analogRead(POTPIN);
    motorSpeedPot = (int)(valPot / 4.01); // ratio to convert pot reading to analog
    if (motorSpeedPot > 255){ // ensures max speed is 255 and min speed is 0
    motorSpeedPot = 255;
    }
    else if (motorSpeedPot < 0){
    motorSpeedPot = 0;
    }*/
  motorSpeedPot = 255; // valPot changed and varied around 255, sometimes going to zero and stopping autonomous car functions
  fractionOfFullSpeed = ((double)motorSpeedPot) / 255.0;
  turnTime = (int)(1350.0 / fractionOfFullSpeed); // for carpet 1350

  backwardsMotion = false;
  cmDistance = ping(echoUS);
  currentCm = cmDistance;
  double randomTurnDegreeRadius = (double)(random(12, 37));
  
  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
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");    
    kick(false);
    backwardsMotion = true;
    moveDirection(90, motorSpeedPot, 2 * turnTime, backwardsMotion); // back up robot
    halt(WAITTIME);
    stepperCircleDegreeCheck();
    kick(false);
    int turnTimeFarthest = (turnTime / 90) * degreeMaxDistanceStepper;
   // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\n");
    backwardsMotion = false;
    moveDirection(180, motorSpeedPot, turnTimeFarthest, backwardsMotion); // turn to degree where saw farthest with U.S. circle rotation from stepper
  }
  else if (currentCm <= 16) { // if object within 8 to 16 cm
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");    
    kick(false);
    backwardsMotion = true;
    moveDirection(90, motorSpeedPot, 4 * turnTime / 3, backwardsMotion); 
    halt(WAITTIME);
    stepDegree(-1 * STEPPERDEGREERADIUS); 
    stepperDegreeCheck(0, 2 * STEPPERDEGREERADIUS, -1 * STEPPERDEGREERADIUS, 600);
    int turnTimeInitial = (turnTime / 90) * (STEPPERDEGREERADIUS); // time for motors to turn depending on which angle stepper says max distance is
    backwardsMotion = false;
    // turn to degree where saw farthest with U.S. circle rotation from stepper  
    if (degreeMaxDistanceStepper < STEPPERDEGREERADIUS){ // for east side of measurements 
      turnTimeFarthest = (turnTime / 90) * (STEPPERDEGREERADIUS - degreeMaxDistanceStepper); // calculate turnTimeFarthest (stepper only uses positive degreees), and drive motors to either direction      
     // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\n");
      moveDirection(0, motorSpeedPot, turnTimeFarthest, backwardsMotion); 
    }
    else if (degreeMaxDistanceStepper > STEPPERDEGREERADIUS){ // for west side of measurements
      turnTimeFarthest = (turnTime / 90) * (degreeMaxDistanceStepper - STEPPERDEGREERADIUS);   
     // Serial.print("Step turn time: "); Serial.print(turnTimeFarthest); Serial.print("\n");
      moveDirection(180, motorSpeedPot, turnTimeFarthest, backwardsMotion);
    }
  }
  else if (currentCm <= 28) { // checkSurroundings if object within 16 to 28 
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
    checkSurroundings();
  }
  else if (currentCm <= 44) { // checkNearby if object within 28 to 44 cm to find a slight deviation
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
    checkNearby(randomTurnDegreeRadius);
  }
  else if (currentCm <= 53) { // pathClear false between 44 to 53 cm so robot rotates counterclockwise
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
    pathClear = false;
  }
  else {
    Serial.print("Dis: "); Serial.print(currentCm); Serial.print("\n");
   // Serial.print("Speed: "); Serial.print(motorSpeedPot); Serial.print("\n");
    pathClear = true; // goes straight if no object within 52 cm is detected
  }
  
  // interval of repeat is random within boundaries
  if (numberOfLoops % random(70, 141) == 0) { // checkSurroundings once every random or 140 loops
    checkSurroundings(); // sometimes doesn't run
  }
  else if (numberOfLoops % random(35, 71) == 0) { // once every random or 35 loops, checks +-15 degrees or random setting
    randomTurnDegreeRadius = (double)(random(18, 37)); // average of 27 degrees turn radius for three distance checks
    checkNearby(randomTurnDegreeRadius);
  }
  else {
    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
  }
}

void volumeUpIRDirControl(void){
  if (controlDirVolume <= (180 - controlDirVolIncrement)){ // increment 5 degrees, or specified increment, every time volume up pressed until 180 degrees
    controlDirVolume += controlDirVolIncrement;
    controlDir = controlDirVolume;
  }
  else{
    controlDir = 180;
  }
}

void volumeDownIRDirControl(void){ // not using volume down key, called when channel up key pressed
  if (controlDirVolume >= controlDirVolIncrement){ // decrease specific degree increment
    controlDirVolume -= controlDirVolIncrement;
    controlDir = controlDirVolume;  
    }
  else{
    controlDir = 0;
  }
}

void remoteInput(void) { // data of ir reciever from ir transmitor remote from sony tv remote
  manualStepper = false; // only true when their specific button pushed
  buttonDriverOverride = false; 
  switch (results.value) // robot actions for pushing buttons 
  { 
    case 0xA90: controlSpeed = 0;                     controlBackwards = false;             stepDegree(-1 * stepDisplacement); stepDisplacement = 0; autoOff = !(autoOff); break; // power button: change between autopilot and manual modes
    case 0x910: controlSpeed = 0;                                                                                                                                          break; // 0 button: stop nxt motion
    case 0x710: controlSpeed = 0;                                                                                                                                          break; // guide button: stop nxt motion (same as above)
    case 0x110: controlSpeed = 255; controlDir = 0;                                                                                                                        break; // 9 button: 0 degrees
    case 0xA10: controlSpeed = 255; controlDir = 30;                                                                                                                       break; // 6 button: 30 degrees
    case 0x410: controlSpeed = 255; controlDir = 60;                                                                                                                       break; // 3 button: 60 degrees
    case 0x810: controlSpeed = 255; controlDir = 90;                                                                                                                       break; // 2 button: 90 degrees (straight)
    case 0x10:  controlSpeed = 255; controlDir = 120;                                                                                                                      break; // 1 button: 120 degrees
    case 0xC10: controlSpeed = 255; controlDir = 150;                                                                                                                      break; // 4 button: 150 degrees
    case 0x610: controlSpeed = 255; controlDir = 180;                                                                                                                      break; // 7 button: 180 degrees
    case 0x210: controlSpeed = 255; controlDir = 90;  controlBackwards = false;                                                                                            break; // 5 button: north (forward)
    case 0xE10: controlSpeed = 255; controlDir = 90;  controlBackwards = true;                                                                                             break; // 8 button: south (backward) 
    case 0x2D0: controlSpeed = 255;                                                         volumeUpIRDirControl();                                                        break; // left arrow button: increase left turn angle
    case 0xCD0:  controlSpeed = 255;                                                        volumeDownIRDirControl();                                                      break; // vol down button: kick servo forward
    case 0x2F0: controlSpeed = 255; controlDir = 90;                                        controlDirVolume = 90;                                                         break; // up arrow button: reset volume turn degree to forward (90 degrees)    
    case 0xC90: controlSpeed = 0;                                                           kick(false);                                                                   break; // right arrow button: increase right turn angle
    case 0x890: controlSpeed = 0;                                                           kick(true);                                                                    break; // ch down button: kick servo backward
    case 0x490: controlSpeed = 0;                                                           manualStepper = true; stepperClockwise = false;                                break; // volume up button: turn stepper counterclockwise
    case 0x90: controlSpeed = 0;                                                            manualStepper = true; stepperClockwise = true;                                 break; // channel up button: turn stepper clockwise
    case 0x3B0: controlSpeed = 0;                                                           stepDegree(-1 * stepDisplacement); stepDisplacement = 0;                       break; // freeze button: reset ultrasonic sensor to facing forward by moving stepper
    case 0xA70: controlSpeed = 0;                     controlBackwards = !controlBackwards;                                                                                break; // center of arrows button: change directions from backwards to forwards and vise versa
    case 0x270: controlSpeed = 0;                                                           buttonDriverOverride = true;                                                   break; // picture button: autonomous movemment dependent only on push buttons on bumpers  
  }
  irrecv.resume();
}

void loop(void) {
  stepper.setSpeed(450); // max speed around 700, various functions manipulate stepper speed
  if (digitalRead(BUTTONFORWARDONE) == LOW || digitalRead(BUTTONFORWARDTWO) == LOW){ // read button state (pushed or not)
    forwardButtonControl = true;
  }
  else if (digitalRead(BUTTONBACKWARDONE) == LOW || digitalRead(BUTTONBACKWARDTWO) == LOW){
    backwardButtonControl = true;
  }
  if (!autoOff) { // if autonomous, check for remoteInput or run autonomous mode
    if (irrecv.decode(&results)) {
      remoteInput();
    }
    else {
      if (forwardButtonControl){ // if push button pushed, autonomous reacts quickly, else runs autonomous 
        forwardObstacleButton();
        forwardButtonControl = false;
      }
      else if (backwardButtonControl){  
        backwardObstacleButton();
        backwardButtonControl = false;
      }
      else{
        autonomous();        
      }
    }
  }
  else if (autoOff) { // if autonomous off, check for another remoteInput or run based on previous remoteInput
    if (irrecv.decode(&results)) {
      remoteInput();
    }
    else {
      if (!buttonDriverOverride){
        if (!manualStepper) { // rotate stepper if stated, or run remoteInput task
          moveDirection(controlDir, controlSpeed, controlBackwards);
        }
        else if (manualStepper) { 
          if (stepperClockwise) {
            stepDegree(-1.0);
            --stepDisplacement;
          }
          else if (!stepperClockwise) { 
            stepDegree(1.0);
            ++stepDisplacement;
          }
        }
      }
      else if (buttonDriverOverride){
        buttonDriver();
      }
    }
  }
}

int ping(byte echoUS)
{
  long duration;
  long cm;
  pinMode(TRIGUS, OUTPUT);
  digitalWrite(TRIGUS, LOW); // give short low pulse to ensure clean high pulse
  delayMicroseconds(2);
  digitalWrite(TRIGUS, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGUS, LOW);
  pinMode(echoUS, INPUT);
  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)
  cm = microsecondsToCentimeters(duration);
  if (cm == 0) {
    cm = 200; // returns high distance of 200 if nothing found, actual zero cm is never read, lowest reading seems to be around 3 cm
  }
  return (int)cm ; // int type used with ping to determine cmDistance
}

long microsecondsToCentimeters(long microseconds)
{
  return microseconds / 58; // convert time to distance
}

// TO DO/ FIX:
// function which allows robot to go back to starting place (integrator) constantly calculating 2d position
// fix potentiometer analogIn: not consistent values, leads to one motor off, only controls speed of one motor when used, try interrupt
// l9110 motor only drives one motor with analog values and one motor with digital values (even when analog in code)
// make millis non blocking: run stepper at same time as nxt motors, not rapid alteration
// 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
// find a way to know if motors hung up and not rotating- find power consumption
// in remote mode, doesn't stop moving backwards when stepper pressed
// recenter robot to degree found when moving staight and sees object, then run avoidance task, backDir = false
// use bluetooth module to tranfer data

Credits

achyuta_yadunandan

achyuta_yadunandan

1 project • 0 followers

Comments