Nuisance V1 - Software

R/C

  • Receiver read by Axon using external interrupts
  • Interrupts capture timestamp of rising and falling edges of signal on each channel
  • Value calculated by the difference in time between edges. To keep the interrupt routine short, the calculation is done asynchronously by a scheduled function (see Scheduler below)
  • Since there can be interference with older FM radios, I used a state machine and counter to determine when I have seen enough good signals to consider the radio on. I also will ignore up to 3 bad signals before declaring the signal to be lost.
  • When radio is on, the right joystick (ch 1 & 2) controls speed and turn. Left joystick (ch 3 & 4) controls pan and tilt of the head. Aux (ch 6) is used for various debugging functions. Gear (ch 5) is currently unused.
  • If radio is off, robot functions in autonomous mode
PWM - For more info, see my PWM tutorial
  • Sabertooth and precision servos require 50Hz base frequency pulses to stay synchronized. Software PWM was not consistent enough to work properly.
  • Used built-in PWM mode on timers 4 and 5 of the atMega640 to drive 6 PWM outputs
  • Sensor servos work fine with software servo mode as long as the R/C interrupts are turned off for the duration of the pulse output. Otherwise, the length of the pulse can be changed enough by the interrupt to alter the desired value. Used the following macro instead of the one provided...

    #define myServo(port,number,position)   PCMSK2=0;PORT_ON(port,number); delay_cycles(position); PORT_OFF(port,number);PCMSK2=rcPinMask
Scheduler - For more info, see my scheduler tutorial
  • I used a scheduler routine to allow many functions to run at appropriate intervals in an asynchronous manner. Functions can be added and removed from the scheduler stack in an ad hoc fashion and each with its own period
  • Since the scheduler is done without interrupts and multiple functions can fire in the same pass, it is possible to have starvation of some functions. I added debugging code to help determine if and how often starvation occurs. The scheduler is only used for functions that can tolerate occasional delays in running.
  • All inputs and sensor servos are handled by the scheduler as are some mapping functions
Map
  • A map is created in memory using a 32x32 grid
  • Each grid represents a 3 inch square
  • The map is always relative to the robot, so it is more like a radar map with the robot always in the center.
  • There are actually two maps, one to store actual sensor data, and one to keep track of which grid locations are visible by sensors. This second map is a mask that helps the software know where there are blind spots
  • When an object is seen by a sensor, a fixed number is stored in the grid location corresponding to the location. At the same time, all the points from the sensor to the object are marked as visible in the mask map.
  • For sensors with a wide field of view (such as the PING) 3 points are plotted corresponding to the left, center, and right angles of the field of view.
  • There is a scheduled process that decrements all of the numbers in the map on a regular basis. This allows older plots to fade out and for more current ones to have a higher value. This is similar to a radar image fading from the display over time unless updated with a new hit.
  • Each sensor has knowledge of its location on the robot with X and Y offsets from center as well as the angular offset of the sensor from the servo's center point. This data is used when plotting the points on the map
  • The map is also marked with the outside edges of the robot itself so it knows its own dimensions
  • A buffer zone is defined that is used by the state machine to know if something is too close and must be handled by specialized navigation routines.
  • The fade process also updates several global variable based on the contents of the map. These are used by other functions to navigate.
    • closeLeft, closeRight, closeFront, closeBack are used to indicate something in the buffer zone. The first 3 are also indicated on the red LEDs for visual debugging to know when the robot thinks there is something in the buffer zone.
    • unitsClearFront, unitsClearBack are used to set the speed under normal mode. The more distance that is clear ahead, the faster you can go.
    • sumLeft, sumRight are weighted sums of all the points in left and right front quadrants. The closer or newer the point the higher the weight.
  • The difference between sumLeft and sumRight is used to determine which way to turn and how much. Always turn away from the side with the highest sum since that is where the closest and largest obstacles are.
  • When in R/C mode, the Aux switch can be used to take a snapshot of the maps and global variable for output to the USB port when connected back to the computer. This turned out to be a very valuable debugging tool. Here is an example of a snapshot of the internal map combined with the mask map when traveling down a hallway...

? ? ? ? ? ? ? ? ? ? ? . . . . . . ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? . . . . . . ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? . . . . . . . ? . ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? . ? . . . . . . . ? ? ? . ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? . ? . ? . ? . . . . . ? # ? o ? . ? ? ? ? ? ? ?
? ? ? ? ? . ? . . ? . . . . . . . . . . . ? . . ? . ? ? ? ? ?
? ? ? ? ? ? . ? o . . ? . . . . . . . ? # . . ? . ? ? ? ? ? ?
? ? ? ? . . ? . ? . . . . . . . . . . ? . o ? . ? . . ? ? ? ?
? ? ? ? ? . . ? # . . . . . . . . . . . # . . ? . . ? ? ? ? ?
? ? ? . . ? . . . . . . . . . . . . . . . o . . . ? . . ? ? ?
? ? ? ? ? . . . # o . . . . . . . . . . # . . . . . ? ? ? ? ?
? ? . . . . ? . ? # . . . . . . . . . . # o . . ? . . . . ? ?
? ? ? ? ? . . . . # . . . . . . . . . . . # . . . . ? ? ? ? ?
? ? . . . . . . . # . . . R R R R R . . . # # . . . . . . ? ?
? ? ? . . . . . . o . . . R R R R R . . . . # . . . . . ? ? ?
? ? . ? ? ? ? ? ? ? ? ? ? R R X R R ? ? ? ? ? ? ? ? ? ? . ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? R R R R R ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? # . . R R R R R . . . ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? . . . . # . . . . . . . . . ? . . ? ? ? ? ? ? ?
? ? ? ? ? . . . . . # # . . . . . . . . . o ? . . . ? ? ? ? ?
? ? ? ? . ? ? . . . # # . . . . . . . . . . . . ? ? . ? ? ? ?
? ? ? ? ? . . ? . . # . . . . . . . . . . . . ? . . ? ? ? ? ?
? ? ? ? ? . ? . . . . . # . . . . . . . . . . . ? . ? ? ? ? ?
? ? ? ? ? ? . ? ? . ? # # . . . . . . . ? . ? ? o ? ? ? ? ? ?
? ? ? ? ? ? ? ? . ? . ? . ? . . . ? . ? . ? . ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? . ? . . ? . . . . . . . ? . . ? . ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? . ? . . . ? . ? . . . ? . ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? . ? . ? . ? . ? . ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? . ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

FSM
  • In order to handle many different situations, the main loop uses a Finite State Machine
  • Each main state can have many sub steps
  • Current state is display in binary on four LEDs for easy visual indication during debugging
  • Transitions to another state or step can be based on any of the global variables or on a counter/timer set by previous states
  • Whenever a state causes a significant change in location or direction, it will go to the Learning state when it is done. The Learning state waits for a sufficient period of time to allow the map to catch up to any changes and for the globals variable to contain valid data.
  • Currently, there are only a few states defined:

     
    #define stateInitialize         0x00
    #define stateLearn              0x01
    #define stateRcControl          0x02
    #define stateAutoGoForward      0x03
    #define stateAutoBackUp         0x04
    #define stateAutoStopAndTurn    0x05

    #define stateTrapped            0x0E
    #define stateDebug              0x0F
[Next] [Back to index]