Autonomous Car with Arduino, in progress write-up

Discussion in 'The Projects Forum' started by poopscoop, Sep 28, 2014.

  1. poopscoop

    Thread Starter Member

    Dec 12, 2012
    139
    16
    Objective: Build an autonomous car which avoids obstacles by means of an ultrasonic sensor. Properly document the build.

    Specific Goals/Intent: The car, powered by Arduino, will map its surroundings using an ultrasonic sensor mounted to a stepper motor. If this were a tank (Which it mildly resembles), the turret would be the ultrasonic sensor. The Arduino will identify the best direction to travel by finding the 22 degree sweep with the longest distance to an object, and then find the particular 5 degree sweep within that set and turn towards that direction. The Arduino will process the correction on the fly by altering motor speeds to affect the turn.

    The car will include multiple IR sensor to ensure it doesn't drive into object the ultrasonic sensor misses, and to prevent it from driving off stairs. These will be wired to interrupt pins on the Arduino.

    Parts List:
    Tamiya Tracked Vehicle Chassis Kit
    Tamiya 70168 Double Gearbox L/R Independ 4-Speed
    SainSmart HC-SR04 Ranging Detector Mod Distance Sensor (Blue)
    RioRand Stepper Motor 5V DC 4-Phase 5-Wire with ULN2003 Driver Board

    RHX 10 Pcs TCRT5000L TCRT5000 Reflective Optical Sensor Infrared 950mm 5V 3A New
    Arduino Uno (Or Mini, we'll see how it works out)
    Arduino Motor Shield (Easy to use H-Bridge)

    Particular Implementation Details:
    This is a work in progress, and given my school schedule, it will take about 6 months to complete. I intend to place this project on my resume.

    Assembly is ongoing, pictures will be up later.
    Pin use is as follows:
    4 Digital pins for motor control (2 for direction, 2 for speed)
    2 Analog pins for current sensing of the drive motors
    2 interrupt pins for IR sensors
    2 Digital pins for Ultrasonic Sensor
    4 Digital pins for the Stepper Motor
    1 Digital pin for "start"
    1 Digital pin for home position on the 'turret'.

    I'm working on the code as able, none of it is tested yet. I haven't used C++ before, but I've had classes in Java and MATLAB, and know how to use Google. I write code like an engineer so I'm sure there are several places where the language could simplify things for me, and I appreciate any input.

    The operation is broken into 2 parts; Start-up and Run.

    When initially started the car will:
    1. Find home position on the turret.
    2. Sweep a full rotation counter clockwise and map surroundings.
    3. Choose the best direction.
    4. Turn to face that direction. ***(Need help here, details at end)

    When running the car will:
    1. Sweep the front 90 degrees continuously.
    2. Adjust drive path to follow the best direction.
    3. If IR sensor trips, stop. Reverse direction.


    Here's the code I slapped together, nothing complete, just a few functions I intend to call multiple times later.
    Code (Text):
    1. //Function that collects the inputs from the distance sensor sweep.
    2. //Just assigns the input distance value to a corresponding spot in the range array.
    3. //Outputs true when array is filled.
    4. boolean totalS(double dist){
    5.   if (arrayPos <= 64){ //arrayPos is a global var that keeps track of array position
    6.     if (dist == 0)//Sensor outputs 0 when it times out, such as at long range
    7.       dist = 400;//400 cm is about the max length it detects, so assume anything that returns 0 is equiv to 400
    8.     ranges[arrayPos] = dist;//Assign input distance to array
    9.     arrayPos++;
    10.     return (arrayPos < 64);
    11.   }
    12.   else
    13.     return true;
    14. }
    15.  
    16.  
    17.  
    18.  
    19.  
    20.  
    21.  
    22.  
    23.  
    24. //Given a 64 input array (When the initial scan is done), choose best direction
    25. int bestDirection(int arr[64]){
    26.   for (int i=0;i<64;i++){//Ensures no 0 values in array
    27.     if (arr[i] == 0);
    28.     arr[i] = 400;
    29.  
    30.   }
    31.   //The sensor doesn't do well with walls at angles, so as a way to aid it in making
    32.   //good decisions, it averages every group of 4 ranges together into an array with 16 entries.
    33.   int groups[16];
    34.   int indexer = 0;
    35.  
    36.   for (int i = 0; i< 16; i++){//iterate through groups array
    37.     double avg = 0;
    38.     int sum = 0;
    39.     //collect 4 entires in the 64 length array, average them
    40.     for (int k = 1; k<=4; k++){//This just sums the 4 vectors, indexer retains its last value outside of the loop.
    41.       sum += arr[indexer];
    42.       indexer++;
    43.     }
    44.     avg = sum/4; //average the sum that comes out of the loop
    45.     groups[i] = avg; //place it in groups.
    46.  
    47.   }//end for
    48.  
    49.   //Now find the greatest average distance, using the average values found in groups.
    50.   int maximum = 0;
    51.   int indexOfMax = 0;
    52.   for (int i=0;i<16;i++){ //Your basic max finding loop
    53.     if (maximum < groups[i]){
    54.       maximum = groups[i];
    55.       indexOfMax = i;//The index of the max is what I really care about
    56.     }
    57.   }//end for
    58.  
    59.   //Using the highest average, find that group of 4 in the original array.
    60.   int selectGroup[4];
    61.   int startPoint = indexOfMax * 4; //max index in groups corresponds to the starting index in the original array
    62.   for (int i = 0; i < 4; i++){
    63.     selectGroup[i] = arr[startPoint + i]; //populate selectGroup with the group of 4 values from the OG array
    64.  
    65.   }
    66.  
    67.   int specificMax = 0;
    68.   int specificMaxIndex = 0;
    69.  
    70.   //Find the specific max in the group of 4, and its position in the OG array
    71.   for (int i = 0;i<4; i++){
    72.     if (specificMax < selectGroup[i]){//max finding loop
    73.       specificMax = selectGroup[i] ;
    74.       specificMaxIndex = i + startPoint;//translate that index into an index in the OG array.
    75.     }
    76.  
    77.   }
    78.  
    79.   return specificMaxIndex; //Return the index of the max in the original array.
    80.  
    81. }
    82.  
    83.  
    84.  
    85.  
    86.  
    87. //Translate index into an angle in radians.
    88. //Must rotate counter clockwise
    89. double directionfindStart(int index){
    90.   double angle = 0;
    91.   //If the index is less than 32, its in the first or second quadrant,
    92.   // and to the left of the car.
    93.   if (index <32)
    94.     angle = PI*index/32; //Maps the index into an angle
    95.    
    96.   //I want negative angles if the value is on the right side, this ensures I get a negative
    97.   if (index >= 32)
    98.     angle =  -1*(64-index)*PI/32; //If index = 64, value is straight head, angle = 0. 32, angle is -PI.
    99.  
    100.   return angle;
    101.  
    102. }
    103.  
    104.  
    105.  
    106.  
    107.  
    108.  

    Code (Text):
    1. //Translates an angle into motor speeds, outputs a 1x2 double array, left motor and right motor speeds.
    2. ////This is intended for when the car is lready moving and its only scanning a 90 degree angle, thus a 16 entry array.
    3. double[2] motorSpeed(int angle, int frontArr[16]){
    4.   //Maxspeed is dependent on the distance to the object directly in front.
    5.   int maxSpeed = round(frontArr[7]/400 * 180);//Any closer than 400cm and the car slows.
    6.   int rightMotor = 0;
    7.   int leftMotor = 0;
    8.  
    9.   //****Might Need to swap right and left motor on all of these***
    10.   //If best direction if of to the left.
    11.   if (angle >0 && angle < PI/2){
    12.     rightMotor = maxSpeed - angle*2; //Right motor moves at max speed minus how extreme the turn needs to be.
    13.     leftMotor = cos(angle) * maxSpeed//Left motor slows down corresponding the the cos of the angle.
    14.    
    15.   }
    16.   //When the best direction is behind the car, the negative cos will tell the motor to spin backwards.
    17.   if (angle >= PI/2){
    18.     rightMotor = maxSpeed - angle;
    19.     leftMotor = cos(angle) * maxspeed;
    20.    
    21.   }
    22.   //The angle is negative for the collowing inputs, so a few things must be changed.
    23.   if (angle <0 && angle > (-1)*PI/2){
    24.     leftMotor = maxSpeed + angle*2;
    25.     rightMotor = cos(angle) * maxSpeed
    26.    
    27.   }
    28.   //Same as above.
    29.   if (angle <= -PI/2){
    30.     leftMotor = maxSpeed + angle;
    31.     rightMotor = cos(angle)*maxspeed;
    32.    
    33.   }
    34.  
    35.  
    36. }
    37.  
    38.  
    39.  
    40.  
    41.  
    42.  
    43.  
    44.  
    45.  
    46.  
    47.  
    48. //Finds the best direction when moving, when the sweep is only 90*
    49. int bestDirectionMoving(ranges[16]){
    50. for (int i=0;i<16;i++){
    51.    if (arr[i] == 0);
    52.      arr[i] = 400;
    53. }
    54. int groups[4];
    55. int indexer = 0;
    56.   for (int i = 0; i< 4; i++){
    57.     double avg = 0;
    58.    
    59.     int sum = 0;
    60.     for (int k = 1; k<=4; k++){
    61.       sum += arr[indexer];
    62.       indexer++;
    63.     }
    64.     avg = sum/4;
    65.     groups[i] = avg;
    66.    
    67.   }
    68.  
    69. int maximum = 0;
    70. int indexOfMax = 0;
    71. for (int i=0;i<4;i++){
    72.    if (maximum < groups[i]){
    73.      maximum = groups[i];
    74.      indexOfMax = i;
    75.    }
    76.  
    77. }
    78.  
    79. int selectGroup[4];
    80. int startPoint = indexOfMax * 4;
    81. for (int i = 0; i < 4; i++){
    82. selectGroup[i] = arr[startPoint + i];
    83.  
    84. }
    85.  
    86. int specificMax = 0;
    87. int specificMaxIndex = 0;
    88.   for (int i = 0;i<4; i++){
    89.    if (specificMax < selectGroup[i]){
    90.     specificMax = selectGroup[i] ;
    91.     specificMaxIndex = i + startPoint;
    92.    }
    93.    
    94.   }
    95.  
    96. return specificMaxIndex;
    97.  
    98. }
    99.  
    Where I need help: How to turn the car to the proper direction upon startup? How do I create a feedback loop using the ultrasonic sensor?
     
  2. poopscoop

    Thread Starter Member

    Dec 12, 2012
    139
    16
    Update:

    That stepper motor is wired incorrectly, and after a few minutes fiddling a google search turned up the solution: Swap the center 2 wires. It takes about 2100 steps per revolution, at a max speed of 9 RPM. Not blazing, but it should work.

    Got the distance sensor up and running, its reasonably accurate but I'm using it in a 4 iteration loop to average the values for each direction.

    Settled on a magnetometer to get turning feedback. http://smile.amazon.com/365buying-G...8&qid=1412473977&sr=8-1&keywords=magnetometer

    I was originally going to use the Arduino Motorshield as my H-bridge for the drive motors. Unfortunately, it seems to be dead. I have 0 ohms between the Vin and Gnd pins, it doesn't turn the motors, and pulls 3 amps when connected to the battery pack. It's $25 to replace, so I intend use an L298N H-bridge (~$8) instead.
     
Loading...