Very simple tracking algorithm

[Moe]

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?

[DougieLawson]

Post your current code and we'll see if/how it can be improved.

[Moe]

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)

[Moe]

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?

[AndersM]

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!)...

[AndersM]

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

[Moe]

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.

[danjperron]

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

sum = vectorADD(v1,v2)

#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.

[danjperron]

Too bad that you don't have an amplitude on each PIR because adding vector with amplitude level will give you a better angle.

[paddyg]

you could do something along the lines of

Code: Select all

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

[AndersM]

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

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.

[danjperron]

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!

[Moe]

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.

Thanks all for your inputs.

[Moe]

Finally got round to implementing Dan's vector solution, slightly modified so the angles are relative to the the y-axis (north), and it works nicely. Thanks everyone for their input.