Theremin Vision Robot Control Program V1.82 Jan. 06, 2004 Basic Stamp BS2p24 PBASIC 2.5 Theremin Vision Ver. 10a Not fully tested!! Fully released to the Public Domain with NO copyrights reserved. Features: Four control "personalities" to allow the robot to function in four different ways. They are test, sumo, rover, and follower. The personalities are selected by a small 4 position BCD rotary switch connected to Pin5 and Pin6. A push-button switch input to trigger the robot's operation such as for a mini-sumo match. This switch is not debounced. An LED indicator to signal waiting for button press. Also flashes during scan function. Automatic calibration to level the sensors at the start of operation. Simple sensing variable output to simplify programming functions. Automatic zeroing function to keep the sensors zeroed for long operation runs. Edge detection and motor control in the case of an edge. Object detection and motor control appropriate for the platform type. Fast array scanning and control allowing typically 20 scan/act loops per second. Plain English motor control commands. Comprehensive test output screen for testing and debugging. Program Overview: Variable Setup: This section defines the variables, constants, and Pins used in the program. platform = A four bit variable that defines the personality type. This input is normally controlled by a 4 position BCD rotary switch that is normally pulled low. calscale = The range the sensors can span. This range is typically -30000 to +30000. zerospeed = The auto zeroing function speed. When there is no object or edge detected, the auto zero function will shift the offsets toward zero by this value at every antenna array scan. incal = This flag indicates that the array is in calibration and scan will return valid information. edgedetect = This flag indicates that an edge is being detected. objectdetect = This flag indicates that an object is being detected. edgetrigger = The sensor value at which an edge is indicated. objecttrigger = The sensor value at which an object is indicated. objectclose = The sensor value at which an object is relatively close. antennaFL = The Front Left antenna enable Pin (negative logic). antennaFR = The Front Right antenna enable Pin (negative logic). antennaRL = The Rear Left antenna enable Pin (negative logic). antennaRR = The Rear Right antenna enable Pin (negative logic). led = LED Pin4 output light. pulseget = The input pulsewidth pin out of the 74CHT4040 binary counter. Pin 8 = / 64 Pin 9 = / 256 Pin 10 = / 1024 Pin 11 = / 1 The counter averages out the pulse width over many raw pulses and widens the width to get high resolution. It also allows the raw pulse width to be fairly short for higher stability. controlswitch = An input for a simple pushbutton switch. There is no debouncing. senseXX = The values from the sensors as calculated in the scan function. Zero is the normal value. An object will make a positive reading and an edge will give a negative reading. scaleXX = A scaling calibration constant that gives a sensor range of -calscale to +calscale. offsetXX = A scaling calibration constant that adjust the sensor offset value. motor = A 4 bit output that controls the two side motor directions. 3210 |||| ||||--Left Forward pin 0 |||---Left Reverse pin 1 ||----Right Forward pin 2 |-----Right Reverse PIN 3 0 0000 stop 1 0001 right turn outside wheel 2 0010 left turn inside wheel 3 0011 na 4 0100 left turn outside wheel 5 0101 forward 6 0110 spin left 7 0111 na 8 1000 right turn inside wheel 9 1001 spin right 10 1010 reverse 11 1011 na 12 1100 na 13 1101 na 14 1110 na 15 1110 na mXX = English motor control function set derived from above. mstop CON 0 mrightout CON 1 mleft CON 2 mleftout CON 4 mforward CON 5 mspinleft CON 6 mright CON 8 mspinright CON 9 mreverse CON 10 System Setup: This section setup up the basic initial variable values needed for operation. scaleXX is set to 1 to allow non-calibrated raw output to work. The motor ports are set to output and the motors are turned off. The flags are zeroed. The antenna array is turned off. The program jumps to the desired platform. Platform Control Functions: This section has each of the four platform control "personalities". The programming of these sections can vary considerably. However, there is a basic sequence that should probably be followed. The LED should be used to signal the operator of the start up state and indicate that the program is waiting for a controlswitch operation to begin. The system must go to the calibration subroutine when the robot is not near an edge or an object to set the sensor calibration constants. This will take ~100mS. A control loop cycles through a "sense and act" loop at the rate of about 20 cycles per second. This will typically use: Go to the scan routine to scan the antennas and return the senor values. Go to the edgefind routine to detect and back away from an edge. Display the results for debugging (normally commented out). Control the motors appropriate for the sensor readings and personality. Have a default function if there are no objects or edges. Subroutines: calibrate: Scans the array twice for a stable reading. Divides out the scale factors. Calculates the offsets. Sets the calibration flag. Scan: Turn on the LED so it flashes during a scan. Turn on and scan each of the antennas. The sensor enables are active low and it takes 2mS for the processor to give a stable reading. The program can be stopped here to lock a sensor on. Apply calibration if incal is valid. Perform an auto zero cycle if there are not objects or edges detected. edgefind: If the sensor reading falls below the edgetrigger value, then the motor is set to the opposite direction and the edgedetect flag is set. display: This sends a diagnostic screen to the serial port. Various important parameters are displayed along with a bar graph in the test personality.