VCR Robot - Programming

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

return

The 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…