Motion Tracking

Jadams · January 08, 2010, 06:15:27 PM

Everything works ok except for 1 small issue (Isn't there always 1 'small' issue?). I'll try to explain.

This is to work as a radar on a turntable, rotatiing about 180 degrees, then reverse. When it (ping) sees an object all movement stops and the servo moves (trigger).

I have 1 limit switch fixed at the edge of the turntable, this is on Pin 4. It is a simple NC lever switch. As the turn table rotates it trips the switch and reverses. The problem is it reverses after it has over runs the stop by about 10 degrees. Then it reverses again. So now it ocillates between about 20 degrees.

What I want is a 2nd stop at about 180 degrees and the table reverse between the 2 stops. Currently, the table never reaches the 2nd stop. How can I stop the ocillation? I have tried a pause in different places, but it doesn't work.

If this doens't make sense I'll try to send a photo of the prototype setup.

Thanks

' =========================================================================
'
' File...... Motion Tracking Program.bs1
' Purpose... Move turrent to motion source
' Author....
' E-mail....
' Started...
' Updated...
'
' {$STAMP BS1}
' {$PBASIC 1.0}
'
' =========================================================================

' -----[ Program Description ]---------------------------------------------

' -----[ Revision History ]------------------------------------------------

' -----[ I/O Definitions ]-------------------------------------------------

SYMBOL HB25 = 7 ' SETUP = DN I/O Pin For HB-25
SYMBOL Limit = PIN4 ' Limit switch for max CW rotation
SYMBOL Servo = 1 ' I/O Pin for Servo
SYMBOL Ping = PIN0 ' Ping Sensor on Pin 0

' -----[ Constants ]-------------------------------------------------------

SYMBOL IsOn = 1 ' for active-high in/out
SYMBOL IsOff = 0

SYMBOL Yes = 1
SYMBOL No = 0

SYMBOL Trigger = 1 ' 10 uS trigger pulse
SYMBOL Scale = 10 ' raw x 10.00 = uS

SYMBOL RawToIn = 889 ' 1 / 73.746 (with **)
SYMBOL RawToCm = 2257 ' 1 / 29.034 (with **)

SYMBOL IsHigh = 1 ' for PULSOUT
SYMBOL IsLow = 0

' -----[ Variables ]-------------------------------------------------------

SYMBOL rawDist = W1 ' raw measurement
SYMBOL inches = W2
SYMBOL Speed = W3
SYMBOL loop = W4
SYMBOL pos = B10 ' position, 100 to 200
SYMBOL pulls = B11 ' number of trigger pulls

' -----[ Initialization ]--------------------------------------------------

GOSUB Stop_Motor

PINS = %00000000 ' Clear all
DIRS = %10000010 ' Set Pin 7 and 1 to Output

Reset:

GOSUB Motor_CW

' -----[ Program Code ]----------------------------------------------------

Main:

Check_Limit:
IF Limit = IsOn THEN Speed_Check

Sense:
LOW Ping ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, rawDist ' measure echo pulse
rawDist = rawDist * Scale ' convert to uS
rawDist = rawDist / 2 ' remove return trip

inches = rawDist ** RawToIn
PAUSE 50
IF rawDist>1000 THEN Main

PAUSE 30

GOSUB Stop_Motor

PAUSE 30

GOTO Servo_Move

GOTO Main

END

' -----[ Subroutines ]-----------------------------------------------------
Speed_Check:

PAUSE 600
IF Speed >150 THEN Motor_CCW
GOSUB Motor_CW

' -------------------------------------------------------------------------
Stop_Motor:

PAUSE 6 ' command hold-off
PULSOUT HB25, 150 ' center = stop
PAUSE 1000 ' command hold-off
PULSOUT HB25, 150 ' insurance command
PAUSE 1000

RETURN

' -------------------------------------------------------------------------
Motor_CW:
Speed = 156
PAUSE 6 ' command hold-off
PULSOUT HB25, Speed ' set speed/direction
GOTO Main

' -------------------------------------------------------------------------
Motor_CCW:
Speed=143
PAUSE 6 ' command hold-off
PULSOUT HB25, Speed ' set speed/direction
GOTO Main

' -------------------------------------------------------------------------

Get_Sonar:
LOW Ping ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, rawDist ' measure echo pulse
rawDist = rawDist * Scale ' convert to uS
rawDist = rawDist / 2 ' remove return trip
RETURN
' -------------------------------------------------------------------------
Servo_Move:

FOR pulls = 1 TO 5 'trigger pulls
FOR pos = 100 TO 175 STEP 5
PULSOUT Servo, pos
PAUSE 19
NEXT

FOR pos = 175 TO 100 STEP -5 ' rotate back
PULSOUT Servo, pos
PAUSE 19
NEXT
NEXT
PAUSE 50
PULSOUT Servo, 100 'reset servo to 100
GOTO Speed_Check

' -------------------------------------------------------------------------
' -------------------------------------------------------------------------
' -----[ User Data ]-------------------------------------------------------

JonnyMac · January 08, 2010, 06:42:57 PM

You really need two limit inputs to do what you want. You'll have direction variable that will be flipped every time you reach a limit. By doing it this way you can stop when you reach a limit switch, set your speed to zero, reverse the direction variable, then ramp out -- this will give much better motion. In the loop, of course, you'd check the Ping and if detected, stop and do your thing. When that's done you'd start moving again in the direction you were traveling before.

Make sense?

Jadams · January 08, 2010, 06:58:29 PM

Yeah, I understand the concept of adding limit2. How do I ignor the input signal from limit1 on the return, with a pause or delay? Maybe once I install it and tinker, I'll get it.

Thanks for the quick response.

JonnyMac · January 08, 2010, 08:11:23 PM

Your program is a great example of what should be simple is actually somewhat sophisticated. You've got simultaneous processes in operation and a requirement to refresh the servo every 20 to 40ms.

Have a look at my rewrite -- it will take some time to get used to. It runs in a loop moving back and forth until a target is detected by the Ping. Note that I refined the code down to inches so that the target distance is easy to set. There is a bit variable called direction that is used for updating the HB-25. If either limit switch is tripped this bit gets flipped. Now, in order to prevent it from immediately re-flipping, that is, allowing the now-closed switch to re-open, there is a variable called holdoff that is reset (to 0) on the flip and then incremented with every motor update. As set now the table has to move for about a second or so before the direction can change. Also note then when direction does change the speed ramps up.

When a target is detected the table stops and the servo does its thing (I used your loop having no idea what that does). When that's done the program jumps back into the Main loop and the board will accelerate away from the stopped point in the direction it was last traveling.

My friends in the Dallas Personal Robotics Group (DPRG) have a saying: It's Harder Than It Looks.

Yep. Sometimes, it is.

' =========================================================================
'
' File...... Motion Tracking Program.bs1
' Purpose... Move turrent to motion source
' Author....
' E-mail....
' Started...
' Updated...
'
' {$STAMP BS1}
' {$PBASIC 1.0}
'
' =========================================================================

' -----[ Program Description ]---------------------------------------------

' -----[ Revision History ]------------------------------------------------

' -----[ I/O Definitions ]-------------------------------------------------

SYMBOL HB25Check = PIN7 ' P7 in - no SETUP jumper
SYMBOL HB25 = 7 ' P7 control
SYMBOL Limit2 = PIN5
SYMBOL Limit1 = PIN4
SYMBOL Servo = 1 ' I/O Pin for Servo
SYMBOL Ping = 0 ' Ping Sensor on Pin 0

' -----[ Constants ]-------------------------------------------------------

SYMBOL IsOn = 1 ' for active-high in/out
SYMBOL IsOff = 0

SYMBOL Yes = 1
SYMBOL No = 0

SYMBOL Trigger = 1 ' 10 uS trigger pulse
SYMBOL Scale = 10 ' raw x 10.00 = uS

SYMBOL RawToIn = 889 ' 1 / 73.746 (with **)
SYMBOL RawToCm = 2257 ' 1 / 29.034 (with **)

SYMBOL IsHigh = 1 ' for PULSOUT
SYMBOL IsLow = 0

' -----[ Variables ]-------------------------------------------------------

SYMBOL flags = B0
SYMBOL direction = BIT0 ' motor direction

SYMBOL pos = B1 ' servo position
SYMBOL temp = B2
SYMBOL holdoff = B3 ' holf-off for new direction
SYMBOL speed = B4 ' HB-25 speed
SYMBOL ramp = B5 ' for HB-25 speed ramping

SYMBOL inches = W5

' -----[ Initialization ]--------------------------------------------------

Power_Up:
IF HB25Check = 0 THEN Power_Up ' allow HB25 to initialize
DIRS = %10000010 ' HB-25 and servo outputs
GOSUB Motor_Stop

pos = 100 ' neutral servo position
direction = 0 ' clockwise
speed = 150 ' stopped

' -----[ Program Code ]----------------------------------------------------

Main:
PULSOUT Servo, pos ' update servo
PAUSE 15

Check_Limits:
IF holdoff < 50 THEN Check_Dir ' allow at least 1.0s
temp = PINS & %00110000 ' ether active?
IF temp = %00000000 THEN Check_Dir ' skip if neither active
direction = 1 - direction ' else flip direction bit
holdoff = 0 ' disallow immediate changes
speed = 150 ' stop movement
ramp = 0

Check_Dir:
BRANCH direction, (Motor_CW, Motor_CCW)

Motor_CW:
IF ramp > 0 THEN Set_Motor
speed = speed + 1 MAX 156 ' adjust CW speed
ramp = ramp + 1 // 5 ' ramp
GOTO Set_Motor

Motor_CCW:
IF ramp > 0 THEN Set_Motor
speed = speed - 1 MAX 144 ' adjust CCW speed
ramp = ramp + 1 // 5 ' ramp

Set_Motor:
holdoff = holdoff + 1 MAX 100
PULSOUT HB25, Speed ' set speed/direction

Get_Sonar: ' consumes up to 11ms
LOW Ping ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, inches ' measure echo pulse
inches = inches * Scale ** RawToIn / 2 ' convert to inches

Check_Distance:
IF inches < 36 THEN Main ' back if out-of-range
GOSUB Motor_Stop

Servo_Move:
FOR temp = 1 TO 5
FOR pos = 100 TO 175 STEP 5
PULSOUT Servo, pos
PAUSE 18
NEXT
FOR pos = 175 TO 100 STEP -5
PULSOUT Servo, pos
PAUSE 18
NEXT
NEXT

GOTO Main

' -----[ Subroutines ]-----------------------------------------------------

Motor_Stop:
PAUSE 6 ' command hold-off
PULSOUT HB25, 150 ' center = stop
PAUSE 6 ' command hold-off
PULSOUT HB25, 150 ' insurance command
speed = 150 ' reset for next move
RETURN

' -------------------------------------------------------------------------

' -----[ User Data ]-------------------------------------------------------

Jadams · January 09, 2010, 09:56:08 AM

First of all, thanks for taking time to rewrite this. It's over my head but I'm determined to learn it. There are parts that work and some that I can't get to respond.

1. After the initial start-up, the motor does not run.
2. If I momentarily depress either Limit1 or 2 the motor will run CCW.
3. Depressing either again will stop motor, depress again and CCW................
4. The Ping and servo section works great.
5 Would you explain this logic :
speed = speed + 1 MAX 156 ' adjust CW speed
ramp = ramp + 1 // 5 ' ramp

Thanks

Jadams · January 09, 2010, 10:20:50 AM

After I sent the last post, I think the light came on. I understand speed= speed +1 MAX 156. This motor won't start turning until about 154. I changed the +1 to +10 and it works. Now I just have to do some tweaking. But, I do not understand the ramp = 1+ / / 5

livinlowe · January 09, 2010, 02:36:50 PM

From the programming techniques forum:

Modulus

The // (modulus) operator returns the remainder of a division. Microcontrollers do integer division, so this is useful in general math operations. We can use it to limit a value to a specific range. As above, we'll use // to create a 0 to 7 value from a random number -- here's how:

result = lottery // 8

Why eight as the divisor? Remember, the modulus operator returns the remainder of a division, and that will always be between zero and the divisor-1. We can also use create a value from some desired x to some desired y. First, take the max value (y) and subtract the min value (x); this is the span. Add one to the span and use this general formula:

result = value // span + min

Let's look at a real example. Let's say we have a random value and we want to create a delay from it that can be 25 to 100 milliseconds. The span is 100 - 25 + 1. The code becomes:

delay = lottery // 76 + 25

The "// 76" part of the equation will return a value between zero and 75; this added to 25 (min) gives us the output range of 25 to 100.

JonnyMac · January 09, 2010, 04:01:38 PM

Quote from: Jadams on January 09, 2010, 09:56:08 AM
First of all, thanks for taking time to rewrite this. It's over my head but I'm determined to learn it. There are parts that work and some that I can't get to respond.

1. After the initial start-up, the motor does not run.
2. If I momentarily depress either Limit1 or 2 the motor will run CCW.
3. Depressing either again will stop motor, depress again and CCW................
4. The Ping and servo section works great.
5 Would you explain this logic :
speed = speed + 1 MAX 156 ' adjust CW speed
ramp = ramp + 1 // 5 ' ramp

Thanks

1) I may have erred a bit (had MAX where I should have use MIN), and if the motor is moving slowly it might need to ramp up faster than the code is presently allowing -- see update, below.

2) That seems odd, again, the new code *should* be better -- let me know.

5) The speed adjustment should be easy to follow; it adds one to speed setting the max limit to 156. The ramp variable slows down how often one is added (or subtracted for CCW) from speed. As Shawn points out // (modulus) returns the remainder of a division, in this case that will be 0 to 4. By checking for ramp being zero this code only updates speed every fifth time through the loop.

Now... the motor may not like the very slow speed settings so I've removed the ramping for the moment.

' =========================================================================
'
' File...... Motion Tracking Program.bs1
' Purpose... Move turrent to motion source
' Author....
' E-mail....
' Started...
' Updated...
'
' {$STAMP BS1}
' {$PBASIC 1.0}
'
' =========================================================================

' -----[ Program Description ]---------------------------------------------

' -----[ Revision History ]------------------------------------------------

' -----[ I/O Definitions ]-------------------------------------------------

SYMBOL HB25Check = PIN7 ' P7 in - no SETUP jumper
SYMBOL HB25 = 7 ' P7 control
SYMBOL Limit2 = PIN5 ' CCW limit switch
SYMBOL Limit1 = PIN4 ' CW limit switch
SYMBOL Servo = 1 ' I/O Pin for Servo
SYMBOL Ping = 0 ' Ping Sensor on Pin 0

' -----[ Constants ]-------------------------------------------------------

SYMBOL IsOn = 1 ' for active-high in/out
SYMBOL IsOff = 0

SYMBOL Yes = 1
SYMBOL No = 0

SYMBOL Trigger = 1 ' 10 uS trigger pulse
SYMBOL Scale = 10 ' raw x 10.00 = uS

SYMBOL RawToIn = 889 ' 1 / 73.746 (with **)
SYMBOL RawToCm = 2257 ' 1 / 29.034 (with **)

SYMBOL IsHigh = 1 ' for PULSOUT
SYMBOL IsLow = 0

' -----[ Variables ]-------------------------------------------------------

SYMBOL flags = B0
SYMBOL direction = BIT0 ' motor direction

SYMBOL pos = B1 ' servo position
SYMBOL temp = B2
SYMBOL holdoff = B3 ' holf-off for new direction
SYMBOL speed = B4 ' HB-25 speed
SYMBOL ramp = B5 ' for HB-25 speed ramping

SYMBOL inches = W5

' -----[ Initialization ]--------------------------------------------------

Power_Up:
IF HB25Check = 0 THEN Power_Up ' allow HB25 to initialize
DIRS = %10000010 ' HB-25 and servo outputs
GOSUB Motor_Stop

pos = 100 ' neutral servo position
direction = Limit1 ' assume clockwise
holdoff = 50 ' in case on limit at start
speed = 150 ' stopped

' -----[ Program Code ]----------------------------------------------------

Main:
PULSOUT Servo, pos ' update servo
PAUSE 15

Check_Limits:
IF holdoff < 50 THEN Check_Dir ' allow at least 1.0s
temp = PINS & %00110000 ' ether active?
IF temp = %00000000 THEN Check_Dir ' skip if neither active
GOSUB Motor_Stop
direction = 1 - direction ' else flip direction bit
holdoff = 0 ' disallow immediate changes
ramp = 0

Check_Dir:
BRANCH direction, (Motor_CW, Motor_CCW)

Motor_CW:
speed = speed + 1 MAX 156 ' adjust CW speed
GOTO Set_Motor

Motor_CCW:
speed = speed - 1 MIN 144 ' adjust CCW speed

Set_Motor:
holdoff = holdoff + 1 MAX 100
PULSOUT HB25, Speed ' set speed/direction

Get_Sonar: ' consumes up to 11ms
LOW Ping ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, inches ' measure echo pulse
inches = inches * Scale ** RawToIn / 2 ' convert to inches

Check_Distance:
IF inches < 36 THEN Main ' back if out-of-range
GOSUB Motor_Stop

Servo_Move:
FOR temp = 1 TO 5
FOR pos = 100 TO 175 STEP 5
PULSOUT Servo, pos
PAUSE 18
NEXT
FOR pos = 175 TO 100 STEP -5
PULSOUT Servo, pos
PAUSE 18
NEXT
NEXT

GOTO Main

' -----[ Subroutines ]-----------------------------------------------------

Motor_Stop:
PAUSE 6 ' command hold-off
PULSOUT HB25, 150 ' center = stop
PAUSE 6 ' command hold-off
PULSOUT HB25, 150 ' insurance command
speed = 150 ' reset for next move
RETURN

' -------------------------------------------------------------------------

' -----[ User Data ]-------------------------------------------------------

Jadams · January 09, 2010, 06:16:57 PM

It works perfectly. Thank you guys for all the help on this project.

I also took the ramping out of the first program and it worked pretty well. Not that I knew what I was doing, just expermenting. This version is much smoother, I just made a small adjustment to the speed and the ping sensor field of view limits and it works a planned.

I feel I am now better versed in BRANCH programing and how to use the // operator.

I do appreciate the support from EFX-TEK. I noticed 380 hits on this thread. That in itself is amazing. Now I plan to attach a paint-ball gun and look for varmits!!!

JonnyMac · January 09, 2010, 07:51:26 PM

We MUST have video of that!

livinlowe · January 10, 2010, 12:35:38 PM

Quote from: JonnyMac on January 09, 2010, 07:51:26 PM
We MUST have video of that!

SECONDED, MOVED and PASSED! Video Please!

Jadams · February 02, 2010, 07:47:51 AM

Can the servo move between positions 100 and 175 'snap' back and forth any faster? A quick trigger pull would be good. Do you think a solenoid would be a better device?

Servo_Move:
FOR temp = 1 TO 5
FOR pos = 100 TO 175 STEP 5
PULSOUT Servo, pos
PAUSE 18
NEXT
FOR pos = 175 TO 100 STEP -5
PULSOUT Servo, pos
PAUSE 18
NEXT
NEXT

Thanks

JonnyMac · February 02, 2010, 10:35:25 AM

Servos move at the speed they do. If you change the value from 100 to 175 the speed you get is what you're going to get. You could use a solenoid, but I don't know that you'll find one that will work with the output of the Prop-1; most solenoids that can actually move something (external of itself) take a LOT of current. You might check a place like www.servocity.com for a servo that moves faster and use this on your trigger mechanism.

JackMan · February 02, 2010, 11:06:01 AM

Yep, Jon is right about servo speed. Most servos can handle 6 volts which increases the speed but you would need a separate power supply for it and the speed gain probably wouldn't be worth the effort. A faster servo as Jon suggested is an option. You can also gain a lot of speed by simply using the proper linkage. A long servo horn will give you a lot more speed so long as you don't need tons of torque on your trigger. I don't know how you are connecting the linkage to the trigger but if you are using some type of horn at that end you need to keep it short.
The linkage is a trade off,
Long servo horn = More Speed and Less Torque
Short servo horn = Less Speed and More Torque.

News:

Motion Tracking

Jadams

January 08, 2010, 06:15:27 PM #15

JonnyMac

January 08, 2010, 06:42:57 PM #16

Jadams

January 08, 2010, 06:58:29 PM #17

JonnyMac

January 08, 2010, 08:11:23 PM #18 Last Edit: January 08, 2010, 08:21:49 PM by JonnyMac

Jadams

January 09, 2010, 09:56:08 AM #19

Jadams

January 09, 2010, 10:20:50 AM #20

livinlowe

January 09, 2010, 02:36:50 PM #21

JonnyMac

January 09, 2010, 04:01:38 PM #22 Last Edit: January 09, 2010, 04:15:02 PM by JonnyMac

Jadams

January 09, 2010, 06:16:57 PM #23

JonnyMac

January 09, 2010, 07:51:26 PM #24

livinlowe

January 10, 2010, 12:35:38 PM #25

Jadams

February 02, 2010, 07:47:51 AM #26

JonnyMac

February 02, 2010, 10:35:25 AM #27

JackMan

February 02, 2010, 11:06:01 AM #28