Saturday, January 18, 2014

Weird legged PCB Robot

This is one of my weird robots build with Veroboards.
It has two legs that they push the entire robot, which is standing on two free running wheeles.
  • CPU: Basic Stamp 2. Board of Education
  • Motion: Two legs with 3 degrees of freedom each (Each leg looks like a PUMA manipulator, without the end effector tool). Two free running wheels are used at the frontend (no extra motors). The legs are pushing the robot like a cart.
  • Motors: Six small servomotors (1.5 Kg). Three motors on each leg.
  • Sensors:
    • Eyes: Two infrared transceivers at the front, for object avoidance.
    • Ears: Two microphones. Capability to detect sound direction.

Programming this weird robot to move in not an easy task. The required procedures are not so simple, like in the case of a wheeled robot (or even on a three servo robot). The number of servos is enough to complicate the procedures. Using a different procedure for going forward, turning left and right is not the best way, because you will soon overflow the microcontroller's rom (EEPROM), due to the complexity of the routines.

Thinking all that, I was searching a way to make the motion routine more generic, to cover all the cases (forward, left, right, backwards etc). The simplest way is to save all the necessary PWM values to memory. The trick here is to split each motion in frames. Each frame will have the PWM values of all the servos (in our case, six). The only thing you need, is to save the number showing total frames in the motion scenario. By addressing to the start of each scenario, it is possible to write a generic motion procedure.

For the example lets take a look on how we should save the PWM values for a going forward motion scenario.

Forward0 DATA 2
Forward1 DATA Word 450, Word 550, Word 450, Word 850, Word 880, Word 600
Forward2 DATA Word 450, Word 470, Word 600, Word 850, Word 800, Word 750

The PWM values are the result of trial and error and they will be different to your robot.
In this case we split the scenario in two frames (Forward1 and Forward2). Forward0 shows the total number of frames (in our case, two). All these numbers have a meaning. In my case this is the format of each frame

Format: Total Steps (byte)
        Right Shoulder (Word) -> Right Arm (Word) -> Right Elbow (Word) ->
        Left Shoulder (Word) -> Left Arm (Word) -> Left Elbow (Word)

The first byte is the total number of the frames. The six word numbers in each frame are for the right and left leg respectively.
So in order the robot execute the movement, the only thing we have to do is point the address (word) variable to the starting address of each movement. In our case:

address = Forward0

where "Move" is the generic motion subroutine. Now, if we want to turn left will write something like that

address = GoRight0

supposing we have saved in memory the PWM values of the specific movement. For example:

GoRight0 DATA 2
GoRight1 DATA Word 330, Word 550, Word 500, Word 700, Word 880, Word 600
GoRight2 DATA Word 330, Word 500, Word 600, Word 700, Word 800, Word 700

Once we have decided they way we will save the PWM values of the servos in robot's memory, we can go on writing the generic motion subroutine. Despite the complicity the motion subroutine is quite simple

  ' Generic motion subroutine for the weird legged robot.
  ' First read total frames.

  address = address + 1
  FOR j=1 TO bdata
    ' Send total 21 pulses for each servo.
    ' 21 pulses are enough to complete the motion (of one frame).
    ' You may need to change it for different type of servos

    FOR i=0 TO 20
      PAUSE 3
      ' Read PWM value for right shoulder and send the pulse.
      READ address,Word wdata
      PULSOUT right_shoulder,wdata
      ' Read PWM value for right arm and send the pulse.
      READ address + 2,Word wdata
      PULSOUT right_arm,wdata
      ' Read PWM value for right elbow and send the pulse.
      READ address + 4,Word wdata
      PULSOUT right_elbow,wdata

      ' Read PWM value for left shoulder and send the pulse.
      READ address + 6,Word wdata
      PULSOUT left_shoulder,wdata
      ' Read PWM value for left arm and send the pulse.
      READ address + 8,Word wdata
      PULSOUT left_arm,wdata
      ' Read PWM value for left elbow and send the pulse.
      READ address + 10,Word wdata
      PULSOUT left_elbow,wdata
 ' Go to next frame

    address = address + 12

First we go to the address stored in the variable address and we read the total number of frames (variable bdata). Each frame is consisted from 12 bytes total (6 word values). Next we read the six word PWM values with the order we discussed earlier and simultaneously we send the pulses to the servos. Each stored value is the duration of the pulse we must send to the appropriate servo. In reality each value is the duration in milliseconds divided by the factor two, because 2ms is the step of Basic Stamp 2.
For each frame we send the same PWM values to the servos for 21 times (meaning we send total 21 pulses to each servo). This is important or else the motion will not be able to complete (servos require a number of pulses to complete their movement to the desired position).
When sending of all these pulses is completed, we go to the address of the next frame (if this exists) and we repeat the process. The reason we increase the value of the variable address by 12 is because 12 bytes are stored in each frame, as we said earlier.

Detection of sound direction
This robot has the ability to detect sound direction. That means it can follow a sound source.
Unfortunatelly Basic Stamp 2 doesn't have analog inputs like arduino (remember that arduino is newer). This means that for Basic Stamp 2 we don't have the information of how much left or right the sound is coming from.
In the following picture you can see the schematic diagram of the circuit that is build in the weird robot

The circuit is simple enough. The signal of each microphone is amplified by the transistor based pre-amplifier and then passes through the peak detector. The next stage is the two subtraction amplifiers. The first one is amplifying the difference between the left and the right signal and the second one the difference between right and left. The outputs of the subtraction amplifiers are then compared with a threshold voltage (one for each calculated difference). The threshold voltages are also calibrating the sensitivity of the circuit

Why we are calculating these differences?
Generally we need three distinct states. i) The sound is coming from the left side ii) The sound is coming from the right side and iii) No sound detected. Clearly with arduino things would be much easier if we used two of the analog inputs.
There are totally three trimmers to adjust sensitivity. The trimmer of the one preamplifier is used to adjust the voltage differences to zero (or almost to zero) when the sound is coming in front of the robot.

Το πρόγραμμα
In order Basic Stamp 2 to decide about the direction of the sound, a small program must be written in PBasic. The complete detection process is typically one simple subroutine

' -----[ Subroutine - Get_Sound_Sensors_Status ]---------------------------------
   SoundRightSensor = SoundRightSensor + IN5
   SoundLeftSensor = SoundLeftSensor + IN6
   iCountSound = iCountSound + 1
   IF iCountSound = SoundFilterWindow THEN
     iCountSound = 0
     LeftMIC = 0
     RightMIC = 0
     IF SoundLeftSensor<SoundRightSensor THEN LeftMIC = 1
     IF SoundRightSensor<SoundLeftSensor THEN RightMIC = 1
     SoundRightSensor = 0
     SoundLeftSensor = 0

We must also define the following constants and variables
SoundFilterWindow CON 50
iCountSound VAR Byte
SoundLeftSensor VAR Byte
SoundRightSensor VAR Byte
RightMIC VAR Bit

The status of the left microphone is stored to LeftMIC (if is one, the sound is coming from the left). Variable SoundLeftSensor is actually showing how many times is Vleft > Vright + Vthres (Vthres comes from hardware. Meaning it depends on the position of the comparator's trimmer, which is adjusting the sensitivity).
Same things for the variable RightMIC, regarding the right microphone. Variable SoundRightSensor is showing how many times is Vright > Vleft + Vthres

The constant SoundFilterWindow is showing how many measurements the program must do, in order to make a reliable result. The value will come out from the test and will be deferent in your case.

In the following video you can see the robot in a simple test. In order to visually see which decision the robot made, regarding the direction of the sound, I connected two LEDs

No comments:

Post a Comment