Page 1 of 1

### Very simple tracking algorithm

Posted: Sun Oct 04, 2015 8:23 pm
Apologies in advance, I am rubbish at programming and I think I'm being a bit thick here, so I'm appealing to this forum for inspiration.

I have an fixed array of eight PIR sensors arranged around the compass points, and a turret with various things on it rotated with a stepper motor. The turret should follow people as they walk past.

At the moment, I have a simple series of eight 'If' statements, i.e. If North-East PIR is active, goto 90 degrees. It kindof works and has the advantage that it always heads to the latest PIR to go active, but it's not very clever, and obviously its only precise to 45 degrees. Ideally I would be able to find an average position based on multiple sensor inputs, but with my dumb programmings techniques that would involve an exponentially large series of 'If' statements.

I briefly tried using callbacks functions, one for each sensor, but I think I just ended up with eight different threads fighting each other.

Can anyone help me with the 'proper' way of doing this?

### Re: Very simple tracking algorithm

Posted: Sun Oct 04, 2015 8:28 pm
Post your current code and we'll see if/how it can be improved.

### Re: Very simple tracking algorithm

Posted: Tue Oct 06, 2015 8:20 pm
OK, here we go. This first bit is the simple if/then program. It works, but as I said it's not very precise and it needs to complete each move before it can react to the next input, which is not ideal. 'Goto()' is just the function that drives the stepper motor to a particular bearing (this works fine).

Code: Select all

``````import RPi.GPIO as GPIO, sys, threading, time, os

#use physical pin numbering
GPIO.setmode(GPIO.BOARD)

IR_N = 7    #GPIO 4 GPCLK0
IR_NE = 11  #GPIO 17
IR_E = 13   #GPIO 27
IR_SE = 15  #GPIO 22
IR_S = 16   #GPIO 23
IR_SW = 18  #GPIO 24
IR_W= 19    #GPIO 10 MOSI
IR_NW= 26   #GPIO 7 CE1

# Set stepper pins as output
for pin in StepPins:
GPIO.setup(pin,GPIO.OUT)
GPIO.output(pin, False)

#set up IR pins as inputs
#GPIO 4 pin 7 is IR north
GPIO.setup(IR_N, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(IR_NE, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(IR_E, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(IR_SE, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(IR_S, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(IR_SW, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(IR_W, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
GPIO.setup(IR_NW, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)

def track():
if GPIO.input(IR_N):
print ("North")
goto(0)
if GPIO.input(IR_NE):
print ("North East")
goto(45)
if GPIO.input(IR_E):
print ("East")
goto(90)
if GPIO.input(IR_SE):
print ("South East")
goto(135)
if GPIO.input(IR_S):
print ("South")
goto(180)
if GPIO.input(IR_SW):
print ("South West")
goto(225)
if GPIO.input(IR_W):
print ("West")
goto(270)
if GPIO.input(IR_NW):
print ("North West")
goto(315)
else:
stop(0)

# Start main loop
while True:
track()
``````
This next bit was abortive attempt to use callbacks. It resulted in random behaviour, I suspect because I had several functions all trying to drive the motor to different places.

Code: Select all

``````def IRdetect(bearing):
print("Target bearing %i" %bearing)
goto(bearing)

# Set IR detect events.  Don't really understand what lambda does, but it allows me to pass an argument (bearing) to a callback function
GPIO.add_event_detect(IR_N, GPIO.FALLING, callback=lambda x: IRdetect(0), bouncetime =300)
GPIO.add_event_detect(IR_NE, GPIO.FALLING, callback=lambda x: IRdetect(45), bouncetime =300)
GPIO.add_event_detect(IR_E, GPIO.FALLING, callback=lambda x: IRdetect(90), bouncetime =300)
GPIO.add_event_detect(IR_SE, GPIO.FALLING, callback=lambda x: IRdetect(135), bouncetime =300)
GPIO.add_event_detect(IR_S, GPIO.FALLING, callback=lambda x: IRdetect(180), bouncetime =300)
GPIO.add_event_detect(IR_SW, GPIO.FALLING, callback=lambda x: IRdetect(225), bouncetime =300)
GPIO.add_event_detect(IR_W, GPIO.FALLING, callback=lambda x: IRdetect(270), bouncetime =300)
GPIO.add_event_detect(IR_NW, GPIO.FALLING, callback=lambda x: IRdetect(315), bouncetime =300)
``````

### Re: Very simple tracking algorithm

Posted: Sun Oct 11, 2015 10:36 am
OK, maybe I can be more specific with my question.

I think what I am doing wrong is having a separate callback function for each potential input, each of which call a 'move' function to drive the motor to a particular position. If more than one input triggers, then I have multiple instances of 'move', all trying to drive in different directions.

So what I need to do is have multiple callbacks to a single instance of a function, so each callback calls the same function but interrupts its current trajectory and sends it in a new direction.

Or have a single callback function triggered by any one of many inputs.

Any ideas how I can do this please?

### Re: Very simple tracking algorithm

Posted: Sun Oct 11, 2015 5:41 pm
Could you calculate the position first and then signal the motor?

If the PIR just signals active/inactive a solution like this could be possible:
*Generate a number by adding the active sensors together with the sensors being numbered N 1, NE 2, E 4, SE 8, ... NW 128.
*Use the generated number to find the direction and turn the motor.
Something like this:

Code: Select all

``````directions = [
(0, -1),
(1, 0),
(2, 45),
(3, 22.5),
(4, 90),
(6, 67.5),
(8, 135),
#    ...
(128, 315),
(129, 337.5),
(192, 292.5)
]

flag=0
if GPIO.input(IR_N):
flag=flag+1
if GPIO.input(IR_NE):
flag=flag+2
#...
if GPIO.input(IR_NW):
flag=flag+128

move_to=-1
for dir in directions:
if dir[0] == flag :
move_to=dir[1]
break

if move_to >= 0 :
#Signal motor to move towards move_to
elif flag==0:
print ("No target")
else:
print ("Multiple targets. Confused")
``````
You can of course turn the flag variable into a byte and make other enhancements.
This is just a sketch (and not tested!)...

### Re: Very simple tracking algorithm

Posted: Mon Oct 12, 2015 8:31 am
Thought a little more.
If two active sensors means that the target is in between the two sensors you can us the fact that Python uses True=1 and False=0 to calculate the target angle:

Code: Select all

``````angle=0
while True:
inN=GPIO.input(IR_N)
inNE=GPIO.input(IR_NE)
...
inNW=GPIO.input(IR_NW)

try:
angle= ((360*(inN and inNW)+45*inNE+ ... +315*inNW)/(inN+inNE+...+inNW)) % 360
except ZeroDivisionError:
print("No target")

#Set motor direction to angle
``````
Edit: Korrigerat koden

### Re: Very simple tracking algorithm

Posted: Wed Oct 21, 2015 10:06 pm
Anders you are a genius thank you for taking the time to think about it. I've adopted your second idea and it works ...

with one small flaw. When it works out the average position it gets it wrong when the difference is greater than 180 degrees, e.g. when one is 315 and another is 45, I want a zero but it gives me 180.

I will try to work that one out.

### Re: Very simple tracking algorithm

Posted: Wed Oct 21, 2015 10:59 pm
The simplest method to get the average angle will be to convert the angle in cartesian coordonate and sum the vectors.

Code: Select all

``````import math

def vectorFromAngle(degree):
return ( math.cos(math.pi * degree / 180.0) , math.sin(math.pi * degree / 180.0))

def  vectorADD( v1 , v2 ):
return ( v1[0]+v2[0] , v1[1]+v2[1])

def   vectorToAngle( v1):
newangle = 180.0 * math.atan2(v1[1],v1[0]) / math.pi
if newangle < 0.0:
newangle = newangle + 360.0
return newangle

#define Angles

Angle1= 45
Angle2= 315

v1 = vectorFromAngle(Angle1)
v2 = vectorFromAngle(Angle2)

#sum them

#get the new angle

print("Average of angle1={:.1f} and angle2={:.1f}  is {:.1f}  degree".format(Angle1,Angle2,vectorToAngle(sum)))
``````
Average of angle1=45.0 and angle2=315.0 is 360.0 degree

Because of rounding error it is not 0 but 359.999999999.... This explain the 360 degree instead of 0.

### Re: Very simple tracking algorithm

Posted: Wed Oct 21, 2015 11:14 pm
Too bad that you don't have an amplitude on each PIR because adding vector with amplitude level will give you a better angle.

### Re: Very simple tracking algorithm

Posted: Wed Oct 21, 2015 11:25 pm
you could do something along the lines of

Code: Select all

``````rval = (a + b) / 2
if abs(a - b) > 180:
rval -= 180``````

### Re: Very simple tracking algorithm

Posted: Sat Oct 24, 2015 8:16 pm
Moe wrote:Anders you are a genius thank you for taking the time to think about it. I've adopted your second idea and it works ...
Thanks
Moe wrote:with one small flaw. When it works out the average position it gets it wrong when the difference is greater than 180 degrees, e.g. when one is 315 and another is 45, I want a zero but it gives me 180.
Yes, the second idea can not handle this situation. It is based on the assumption that at most two consecutive sensors are activated (it works when not straddling N for many other combinations).
Where is the target in this case since N sensor is inactive?
You can add more logical and clauses, similar to the one for N, to get it working in cases like the above but the summation can become cumbersome.

danjperron's solution is a good one assuming that the target will be at the mean direction from the active sensors regardless of which combination is triggered..

The first suggestion I made is probably the most versatile as it can handle all combinations of sensor triggering by testing possible combinations in practice and set the best target angle for each one.

### Re: Very simple tracking algorithm

Posted: Sat Oct 24, 2015 11:00 pm
My solution has only one problem.

Is when two opposites directions are set. The two vectors annihilate and the residual doesn't mean anything.

But in reality it doesn't mean anything anyway!

Maybe you should check the final amplitude. If it is near zero, nothing append!

### Re: Very simple tracking algorithm

Posted: Sun Oct 25, 2015 8:14 pm
The sensors are cheap HC-SR501s. They have a 120 viewing angle and are just as likely to trigger 'on the edge' as face on.

The averaging solution actually works quite well, and copes well with any number of sensors. The averaged target angle isn't very accurate but the turret itself is quite slow and follows the target pretty well.

I'm not too worried about two opposites being set - it's possible for this to happen, but it's likely that one in between would also be triggered at some point so the average works out on the correct side.

Project has been delayed slightly as I've been working on a skull that lights up and follows people round the room for daughter's Halloween party (this only needs 180 degrees of freedom) but will try out the vector addition idea when I have more time.