|
This is a simple but weird robot build entirely with veroboards (stripboards).
The robot is standing on two free running wheels and it has two legs that are pushing it like a small cart. Characteristics
|
Programming this weird robot to move is 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 make the required walking procedures complicated. 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. A better approach is to write a procedure that calculates the angles of each servo by solving the inverse kinematic problem for both legs. However this requires some sophisticated math.
Thinking all that, I was searching a way to make the motion routine more generic, to cover all of these 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 the total frames in the motion scenario. By addressing to the start of each scenario, it is possible to write a generic motion procedure.
For 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 to 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
GOSUB Move
where "Move" is the generic motion subroutine. Now, if we want to turn left will write something like that
address = GoRight0
GOSUB Move
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
Move:
' Generic motion subroutine for the weird legged robot.
'
' First read total frames.
READ address,bdata
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
NEXT
' Go to next frame
address = address + 12
NEXT
RETURN
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.
Coding
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 ]---------------------------------
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
ENDIF
RETURN
We must also define the following constants and variables
SoundFilterWindow CON 50
iCountSound VAR Byte
SoundLeftSensor VAR Byte
SoundRightSensor VAR Byte
LeftMIC VAR Bit
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