4. Programming.
Have a look at the program:
Nemesis
'===============================================================
'VCR Robot with 2 DC Motors and 1 dual IR proximity sensor
'Random driving with IR headlights'Compiled program size: 176 bits (out of 1024 bits)
'===============================================================
'--------------------------------------------------
'Variables declaration, labels, timer
'--------------------------------------------------
dim LeftIR ' variables to store the values of the
dim RightIR ' IR sensor for left and right directions
const Motor1a 8 ' right motor
const Motor1b 9 '
const Motor2a 10 ' left motor
const Motor2b 14 '
const MotorEn 13 ' enable motors
const IRsensor 5 ' IR sensor pin
const IRpulse 6 ' pin used to generate the IR pulse
const LeftIRLed 11 ' left proximity IR LED
const RightIRLed 7 ' right proximity IR LED
const RightLineS 1 ' right line sensor
const CenterLineS 2 ' center line sensor
const LeftLineS 3 ' left line sensor
hwpwm 0,50,15 ' pwm signal 40KHz 60% "on" time
'=================================================================
'Main program
'=================================================================
output Motor1a ' we declare the direction of the I/O pins
output Motor1b ' for the motors and IR LEDs to
output Motor2a ' "output" and set the value to
output Motor2b ' "high" or "low".
low Motor1a ' all undeclared pins will be set to
low Motor1b ' "input".
low Motor2a
low Motor2b
output LeftIRLed
output RightIRLed
high LeftIRLed
high RightIRLed
output MotorEn
high MotorEn ' turn on the power to the motors
Mainloop:
'-------------------------------------------------
'Read the sensors
'-------------------------------------------------
gosub ReadIRSensor
'-------------------------------------------------
'Print all sensor values in Debug window
'-------------------------------------------------
print "Left ",LeftIR," Right ",RightIR
'-------------------------------------------------
'Decide where to go
'-------------------------------------------------
if RightIR = 0 then ' obstacle to the right
gosub ReverseLeft ' reverse left motor
else ' no object
gosub ForwardLeft ' forward left motor
endif
if LeftIR = 0 then ' obstacle to the left
gosub ReverseRight ' reverse right motor
else ' no object
gosub ForwardRight ' forward right motor
endif
goto Mainloop ' start again
'=================================================================
'Subrutines
'=================================================================
'-------------------------------------------------
'Motor control subrutines
'-------------------------------------------------
ForwardRight:
low Motor1b
high Motor1a
return
ReverseRight:
low Motor1a
high Motor1b
return
ForwardLeft:
low Motor2b
high Motor2a
return
ReverseLeft:
low Motor2a
high Motor2b
return
'-------------------------------------------------
'Read the IR proximity sensor subrutine
'-------------------------------------------------
ReadIRSensor:
low RightIRLed ' set active the right IR LED
output IRpulse ' start the pwm pulse
pause 1 ' wait a milisecond
input IRpulse ' stop the pulse
RightIR = inp.IRsensor ' read the value
pause 1 ' wait a milisecond
high RightIRLed ' deactivate the right IR LED
low LeftIRLed ' set active the left IR LED
output IRpulse ' start the pwm pulse
pause 1 ' wait a milisecond
input IRpulse ' stop the pulse
LeftIR = inp.IRsensor ' read the value
pause 1 ' wait a milisecond
high LeftIRLed ' deactivate the left IR LED
returnThe program is easy. First there are the variable, ports and other stuff declared, so the microcontroller knows where the sensors and motors are connected. Then the main program starts a loop that reads the sensor, prints the values on the Debug window on the PC, decides how to drive then loops. Let’s take them one by one:
1. Declarations. We need to set 2 variables to hold the sensor values for later operations. Then we need to specify the pins where the sensors, motors and LEDs are connected. All the pins that will be used as outputs have to be specifically set so, otherwise they will be set as inputs by default. And the microcontroller needs to know what state they need to be (high or low). Most important, we need to set the hardware PWM generator for a signal of 40 kHz and 50% duty cycle.
2. IR sensor reading. Here I have used only one sensor and 2 LEDs to build a dual IR sensor. I turn on one LED at a time for a short while, read the sensor and store the value in a variable, then turn the other LED on and read the sensor and store the value in an other variable.
3. Print the values. This is a simple print to serial port to be able to adjust the sensitivity of the sensor. When you do the adjusting, take off the jumper so the motors don’t get power. You don’t want the robot to fly off the table with the serial adapter connected to it. Also take the jumper off every time you program the robot. To adjust the sensor sensitivity, take a sheet of paper and put it vertically in front of the robot, about at the distance you want the robot to start to see the obstacles in it’s path. Then adjust the resistor until you see the values on the screen change from 1 to 0. Keep in mind that different colors reflect IR differently, so the robot will not always turn away at the same distance.
4. Driving decision. Now imagine you are onboard of a tank. You have 2 joysticks to control the motors. Pull the sticks and you drive forward. Push the sticks and you go on reverse. To turn you have to push one stick and pull the other. But instead of windows you have 2 lights. If left light turns on, you have an obstacle on you left. If the right light is on, you have an obstacle to your right. And if both lights are on, the obstacle is in front of you. So, you drive forward (pull both sticks), if the left light suddenly turns on, what do you do? You just push the right stick until the light turns off, then pull it back. For the right light, you push the left stick. And for both lights, you push both sticks to drive in reverse for a while and then turn either left or right.
5. Loop. For a human the action described above seems continuous. But for a robot is a continuous loop that checks the sensor and decides how to drive for a little period of time. This time period depends on the time the robot takes to read the sensor and the time it drives after the driving decision (if needed). Depending on how little or how large this period of time is, the robot will react faster or slower to the changes in it’s path. This program is very simple and can be used for driving with bumper sensors, IR photo reflective sensors (for line following), or, with a few modifications, can be used for sumo combat.
To complete the robot, I have built a daughter board that mounts underneath the robot and holds the IR photo reflective sensors for line following. See the result on the Add-on page…