Файл: Advanced Robotics with the Toddler (Paralax, student guide, v1.3, 2004).pdf

Скачать файл
Заказать решение

ВУЗ: Не указан

Категория: Не указан

Дисциплина: Не указана

Добавлен: 12.06.2025

Просмотров: 1933

Скачиваний: 0

ВНИМАНИЕ! Если данный файл нарушает Ваши авторские права, то обязательно сообщите нам.

Chapter #9: Acceleration-Based Motion · Page 193

” ” ©2002

Toddler

X8

Vdd

Vin Vs1 Vss

Vs2

X9

+ - P11 - P4

-P15 + - P14

X11

B R 13

X12

X7

P15

B R 12

P14

X6

X10

P13

P12

B 9 10

P11

P10

X5

P9

B 8 7

X4

B 2 3

P5

X3

B 1 0

P4

X2

P3

P2

+

P1

P0

Power

www.parallaxinc.com

Rev C

X1

Reset

+ 6Vdc -

Figure 9-2: Pictorial of Memsic 2125 Circuit. X2 and X3 are connected to the Twinkle Toes. X6 and X7 are connected to tilt and stride servos. Additionally, you may connect two LEDs to P5 and P6 for visual feedback of the infrared detectors.

Once the hardware is wired, enter and run Program Listing 9.1

Page 194 · Advanced Robotics with the Toddler

'-----[ Title ]-----------------------------------------------------------

'Toddler Program 9.1: Memsic Accelerometer Test Program

'Incline feedback determines tilt limitations of Toddler

'{$STAMP BS2}

'{$PBASIC 2.5}

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

Yout

PIN

10

' Yout from Memsic 2125

TiltServo

PIN

13

' Toddler tilt servo

StrideServo

PIN

12

' Toddler stride servo

' -----

[ Constants ]

-------------------------------------------------------

TiltCenter

CON

750

' TiltServo center position

StrideCenter

CON

750

' StrideServo center position

StepVal

CON

2

' Servo step value = 1 or 2

' Set StepVal = 3 if using 'DEBUGs

escVal

CON

4

' Indicator for excessive tilt

Delay

CON

8

' Pause between servo pulses -

' add 8 ms to get 20 total

RightLimit

CON

500

' Limit does not occur on flat

LeftLimit

CON

1000

' Limit does not occur on flat

' Right and left depend on

' how you mounted the Memsic

' sensor. Below assume pins 1 & 6

' are toward front of Toddler.

YAxisRtLimit

CON

2600

' Yout right limit *2us = 5.34 ms

YAxisLfLimit

CON

2400

' Yout left limit *2us = 4.500 ms

' -----

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

Incline

VAR

Word

' Pulses from Memsic 2125

ServoLoop

VAR

Word

' General purpose loop var

Counter

VAR

Nib

' Counter filters acceleration

tiltCount

VAR

Nib

' Temporary counter

'-----[ Initialize ]

ServoLoop = 750

'-----[ Main Code ]-------------------------------------------------------------------------------------------------------------

RightFromCenter:

Counter = 0

FOR ServoLoop = ServoLoop to RightLimit STEP StepVal

PULSOUT TiltServo, ServoLoop

PULSOUT StrideServo, StrideCenter

Chapter #9: Acceleration-Based Motion · Page 195

PAUSE Delay

PULSIN Yout,1,Incline

'DEBUG "Right from Center - ServoLoop = ", DEC ServoLoop 'DEBUG ": Incline = ", DEC Incline, CR

IF (Incline > YAxisRtLimit) THEN Counter = Counter + 1 'DEBUG "Counter = ", DEC Counter

'DEBUG ": tiltCount = ", DEC tiltCount, CR IF Counter > escVal THEN LeftFromRight

NEXT

'DEBUG "ServoLoop = ", DEC ServoLoop,": Incline = ", DEC Incline, cr

LeftFromRight:

Counter = 0

FOR ServoLoop = ServoLoop to LeftLimit STEP StepVal

PULSOUT TiltServo, ServoLoop

PULSOUT StrideServo, StrideCenter

PAUSE Delay

PULSIN Yout,1,Incline

'DEBUG "Left from Right - ServoLoop = ", DEC ServoLoop 'DEBUG ": Incline = ", DEC Incline, CR

IF (Incline < YAxisLfLimit) THEN Counter = Counter + 1

'DEBUG "Counter = ", DEC Counter,": tiltCounter = ", DEC tiltCount, cr IF Counter > escVal THEN RightFromLeft

NEXT

'DEBUG "ServoLoop = ", DEC ServoLoop,": Incline = ", DEC Incline, cr

RightFromLeft:

Counter = 0

FOR ServoLoop = ServoLoop to RightLimit STEP StepVal PULSOUT TiltServo, ServoLoop

PULSOUT StrideServo, StrideCenter PAUSE Delay

PULSIN Yout,1,Incline

'DEBUG "Right from Left - ServoLoop = ", DEC ServoLoop 'DEBIG ": Incline = ", DEC Incline, CR

IF (Incline > YAxisRtLimit) THEN Counter = Counter + 1

'DEBUG "Counter = ", DEC Counter,": tiltCount = ", DEC tiltCount, cr IF Counter > escVal THEN LeftFromRight

NEXT

'DEBUG "ServoLoop = ", DEC ServoLoop,": Incline = ", DEC Incline, CR

GOTO LeftFromRight

END

There are two ways to run program 9.1.

Page 196 · Advanced Robotics with the Toddler

The first way is as it is printed above. Toddler will tilt back and forth going between the YaxisRtLimit and YAxisLfLimit values. Be sure to support your Toddler while it is moving as it may fall over.

The second way is to globally replace ‘DEBUG with DEBUG first. Toddler will go much slower because the servo refresh rate is so slow but Toddler will indicate the measured tilt (actually just the pulse width/2) values on the DEBUG screen on your PC.

Watch the values as Toddler goes from side to side. Pick the robot up and tilt left and right it to get a feel for how the Memsic sensor works and what values correspond to what tilt.

Noise Filter

The counter in the Program 9.1 allows filtering the tilt values. If it weren’t there, the tilting would stop the first time a value over the limit was read. The sensor is looking at G forces. Anything touching the Toddler while it is sensing could cause an out of limit value to be read. The IF statements and counter require multiple measurements outside the limit before acting.

ACTIVITY #2: INTEGRATED SENSOR WALKING PROGRAM

Enter and run Toddler Program 9.2.

Chapter #9: Acceleration-Based Motion · Page 197

'-----[ Title ]-----------------------------------------------------------

'Toddler Program 9.2 Complete Sensor Management.bs2

'This program has real time servo programming, inclinometer sensing

'Toddler bumper toes detection and infrared object detection

'{$STAMP BS2}

'{$PBASIC 2.5}

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

TiltServo

PIN

13

'

tilt servo on X7

StrideServo

PIN

12

'

stride servo on X6

LeftIRLED

PIN

4

'

left IRLED

RightIRLED

PIN

15

'

right IRLED

LeftIR_Sensor

PIN

11

'

left IR sensor input

RightIR_Sensor

PIN

14

'

right IR sensor input

Spkr

PIN

8

'

beeper pin

Yout

PIN

10

'

Yout from Memsic 2125

LeftBumper

PIN

2

'

bumper switch on left foot

RightBumper

PIN

1

'

bumper switch on right foot

LeftBumpPU

PIN

3

'

left bumper pull up

RightBumpPU

PIN

0

'

right bumper pull up

' Indicator LEDs are optional, but useful

for seeing movements

LeftVisionLED

PIN

5

'

left vision display LED -

RightVisionLED

PIN

6

'

right vision display LED -

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

MoveDelay

CON

0

'

in milliseconds - normally 20

'

code is already using 30 ms

TiltStep

CON

5

'

experiment with this value

RightTilt

CON

500

'

tilt limits was 620

CenterTilt

CON

750

LeftTilt

CON

1000

'

was 880

StrideStep

CON

7

'

StrideServo step size

RightStride

CON

625

'

stride limits was 650

CenterStride

CON

750

LeftStride

CON

875

'

was 850

ctrlLEDs

CON

%1111

'

define DIRs as constants to

prepBumper

CON

%1001

'

simplify LED/bumper setups

Page 198 · Advanced Robotics with the Toddler

bothGreen

CON

%0110

' OUTa register sets red/green

bothRed

CON

%1001

' status on P0-P3 for Twinkle Toes

rightRed

CON

%0101

' object detection states

leftRed

CON

%1010

bothLEDsOff

CON

%1001

Fwd

CON

0

' codes to pick movement table

PivL

CON

1

' set newMove to one of these

PivR

CON

2

' values and New_Movemt will LOOKUP

BumpL

CON

3

' the right movement table

BumpR

CON

4

escVal

CON

4

' Indicator for excessive tilt

' Right and left depend on

' how you mounted the Memsic

' sensor. Below assume pins 1

& 6

' are toward front of Toddler.

' Use your values here

YAxisRtLimit

CON

2560

' Right limit - was 2570 = 5.14 ms

YAxisLfLimit

CON

2460

' left limit - was 2350 = 4.70

ms

' Yout center - is 2510 = 5.02

ms

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

'Bit variable flags set so 1 causes the action to be taken

doneTiltFlag

VAR

Bit

' Tilt servo reached new value

doneMoveFlag

VAR

Bit

' Servos have reached new values

rightIR_Flag

VAR

Bit

' See something on right side

leftIR_Flag

VAR

Bit

' See something left side

rightBumpFlag

VAR

Bit

' Bumper hit on right side

leftBumpFlag

VAR

Bit

' Bumper hit on left side

sensors

VAR

Nib

' Lower 2 bits of the sensors

' variable used to store IR values

currentMove

VAR

Nib

newMove

VAR

Nib

Counter

VAR

Nib

' Filters tilt sensor output

Mx

VAR

Word

' index for movement tables

MxOffset

VAR

Byte

' added to Mx for index

bMovmnt

VAR

Byte

' table value for movement lookup

currentTilt

VAR

Word

currentStride

VAR

Word

newTilt

VAR

Word

newStride

VAR

Word

Incline

VAR

Word

' Pulses from Memsic 2125

Смотрите также файлы