Autonomous Follower

\ [date]
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''ROBO-RALPH 833
''This code gives Eddie three modes of opperation:
''Manual-mode(Yellow light):
'Moves forward when joystick is moved forward
'Rotates 90CW when joystick is moved right and 90CCW when moved to the left.
''Auto-mode(White light):
'Code moves into AutoMode.
'A new cog that runs the ping sensors is launched in background.
'Eddie roams around avoiding obsticles until the button is pushed again.
''Kinect-mode(Green light)
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''INFO'''''''''''''''''''''''''''
'The range that the IR sensors yield is from about 2500(close) and about 200(far). 1500 = about
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''TO DO''''''''''''''''''''''''''
'Joystick control should be "continuous" instead of only forward, spin left, and spin right.
'Impliment encoders for AUTO-MODE
'Should always have start and stop methods
'Channel 7 of the ADC reads the current voltage of the battery
'All encompasing method for all movements. Forward, reverse, rotate etc
'Can use pin 13 with Simple_Serial object for recieving data from processing.
'Run pings and IR's in Track-mode.
'Implement speech in response to picture being taken and various other functions.
{{''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''MOTOR-PIN KEY'''''''''''''''''''''
pin 19 = Left reverse
pin 20 = PWM left
pin 21 = Left forward
pin 22 = Right reverse
pin 23 = PWM right
pin 24 = Right forward
}}
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
OBJ
XB : "XBee_Object"
pst : "Parallax_Serial_Terminal"
ping : "Ping"
encoder: "Quadrature Encoder"
ADC : "MCP3208"
Serial : "Extended_FDSerial"
CON
_clkmode = xtal1 + pll16x
_xinfreq = 5_000_000
XB_Baud = 9600'
'_______________________________________________________________________________________________
'________________________________________________________THESE ARE ALL OF THE PIN DESIGNATIONS__
ping_pin0 = 0 'Right front(From it's point of view)
ping_pin1 = 1 'Center front ''This is the one pointing down. (Doesn't work because sound bo
ping_pin2 = 2 'Left front (From it's point of view)
ping_pin3 = 3 'Rear
front_servo4 = 4 '
rear_servo5 = 5 '
'Pin 6 is for the XBee to send data to the board.
ping_pin7 = 7 'Rear/Top/It's left
ping_pin8 = 8 'Front/Top/It's left
page \ [page] of \ [pages]
\ [date]
ping_pin9 = 9 'Rear/Top/It's right

ping_pin10 = 10' 'Front/Top/It's right
Speaker_Pin = 11'
'Pins 12-15 are spare GPIO from the center of the board.
Battery_GreenLED = 13 '
Battery_BlueLED = 14
Battery_RedLED = 15'
'Pins 16-18 are the auxilary pins that put out 12V(Pin 16 is used for the EL wire).
EL_Pin = 16'
K_Track_led = 17' |
'Pins 19-24 are for the H-bridges.
'Pins 25-27 are for the ADC shown below.
chipSel = 25 'Pin 25 is connected to the chip select pin of the MCP3208
chipDIO = chipSel+1 'Pin 26 is connected to both the in and out pin of the MCP3
chipClk = chipSel+2 'Pin 27 is connected to the clock pin of the MCP3208
'Pin 28 is the Pin that would send data to the XBee, but we're not using it.
'I'm not sure what pin 29 is for.
'Pin 30 = tx 'Via Serial
'Pin 31 = rx
'Pins and Baud rate for XBee comms.

XB_Rx = 6 ' XBee DOUT 'This is the only one used. We are only recieving with the on-
XB_Tx = 28 ' XBee DIN 'This is set to pin 12 because pin 12 is not an I/O pin(We are
'_______________________________________________________________________________________________
'__________________________________________________________________ADC PIN KEY__________________
'
'0 = Center, front, downward-facing IR
'1 = Top, right IR.
'2 = Top, left IR.
'3 = Top, rear IR.
'Pins 4-7 are unused.
'8 = battery gauge.
'_______________________________________________________________________________________________
'_________________________________________________________THESE ARE THE CHANGEABLE PARAMETERS___
Ping_dist = 10 'Ping_dist at which things are sensed by Ping sensors (Inches)
speed = 4 'Smaller is faster. This controls the speed of the wheels. It the time with
IR_dist = 1500 'Higher is closer. 1500 is about 5 inches
IR_dist_ground = 1500 '
K_Horiz_dist = 200 'This is for kinect mode. It's the amount of pixels from the middle of the
K_Far_dist = 1_200 'This is for kinect mode. It's the distance in mm that a user is from th
K_Near_dist = 600 'This is for kinect mode. It's the distance in mm that a user is from the
K_speed = 6
rotate_time = 10_000
'
'_______________________________________________________________________________________________
VAR 'This section can be optimized. ie: Do they all need

long range0
long range1
long range2
long range3
long range4
long range5
long range7
long range8
long range9
long range10
long IR_range0
long IR_range1
\ [date]
long IR_range2
long IR_range3
long DataUD
long DataLR
word DataIn
long pos[3]
long stack[20]
long stack1[20]
long stack2[20]
long stack3[20]
long stack4[20]
long stack5[20]
long stack6[20]
long stack7[20]
long stack8[20]
long PingsCog
long PingsCog_k
long JoyStickValueUDCog
long JoyStickValueLRCog
long ServoCog
long XBCog
long EL_tapeCOG
long BeepCOG
long P_checkCOG
long K_LEDCOG
long Battery_CheckCOG
long K_DataIn_z
long K_DataIn_x
word K_Character
long K_CharIn
long Battery_Voltage
Pub Init
XB.start(XB_Rx, XB_Tx, 0, XB_Baud) 'Initialize comms for XBee
'pst.start(9600) 'Using this to print to serial monitor
ADC.start(chipDIO, chipCLK, chipSel, 255)
Serial.start(31, 30, 0, 9600) ' Either pst OR Serial, not both!! 'This
Battery_CheckCOG := cognew(Batt_RGB_LED, @Stack8)
Start
Pub Start
repeat
DataIn := XB.Rx 'IMPORTANT: XB.Rx can only receive one digit a
if DataIn == "k" 'Kinect

PingsCog_k := cognew(Pings_k, @stack5) 'Still want to avoid obsticles in track mo
waitcnt(clkfreq + cnt)
Track_Mode
waitcnt(clkfreq/100 + cnt)
if DataIn == "y" 'ASCII decimal equivalent for the character y

PingsCog := cognew(Pings, @stack) 'PingsCog returns the ID of the cog that was l
waitcnt(clkfreq + cnt) 'This pause must be here in order to give the
Auto_Mode
if DataIn == "@" 'LR 'Can use 64 or "@" here.

DataLR := XB.RxDec 'DataIn sits at about 516ish 'M
\ [date]
if DataLR < 200

CCWmanual 'Go left
elseif DataLR > 800
CWmanual'Go right
else
if DataIn == "!" 'UD 'Can use 33 or "!" here.

DataUD := XB.RxDec 'DataIn sits at about 516ish 'Mu
if DataUD > 800
Manualforward
ctra[30..26] := %00000 'This is here to get rid of the sudden burs

ctrb[30..26] := %00000
Pub Track_Mode 'pst.dec(123) Either pst OR Serial, not both!!!

Servo_Straight(Front_servo4)
Servo_Straight(Rear_Servo5)
P_CheckCOG := cognew(P_Check, @Stack)
'This tells the motors what to do based on the tracked user's position.
repeat 'FIRST ADJUST X-POS THEN Z-POS.
if (K_DataIn_x < -K_Horiz_dist) AND (range0 > Ping_dist) AND (IR_range1 < IR_dist) AND (rang
K_CW
if (K_DataIn_x > K_Horiz_dist) AND (range2 > Ping_dist) AND (IR_range2 < IR_dist) AND (range
K_CCW
if (K_DataIn_z > K_Far_dist) AND (K_DataIn_z <> 0) AND (range0 > Ping_dist) AND (range1 > Pi
K_forward
if (K_DataIn_z < K_Near_dist) AND (K_DataIn_z <> 0) AND (range3 > Ping_dist) AND (IR_range3
K_reverse
DataIn := XB.Rx
if DataIn <> "k"
Start
PUB Pings_k 'Reads DataIn and Knct_dist so that
repeat
range0 := ping.Inches(ping_pin0) 'Front right
range1 := ping.Inches(ping_pin1) 'Front on servo
range2 := ping.Inches(ping_pin2) 'Front left
range3 := ping.Inches(ping_pin3) 'Back
range7 := ping.Inches(ping_pin7) 'Top/Rear/Left
range8 := ping.Inches(ping_pin8) 'Top/Front/Left
range9 := ping.Inches(ping_pin9) 'Top/Rear/Right
range10:= ping.Inches(ping_pin10) 'Top/Front/Right
IR_range0 := adc.in(0) 'front facing down

\ [date]
' IR_range1 := adc.in(1) 'front right(It's right)

IR_range2 := adc.in(2) 'front left
IR_range3 := adc.in(3) 'back
DataIn := XB.Rx 'Having this cog check DataIn slows down the ping sen
if (DataIn <> "k")

cogstop(P_CheckCOG)
cogstop(k_LEDCOG)
OUTA[k_track_led] := 0
cogstop(PingsCog_k)
PUB P_Check | PicIn
K_CharIn := Serial.Rxcheck 'This must be outside of the repeat loop.

k_LEDCOG := cognew(k_LED, @Stack7)
repeat
Serial.RxFlush 'This needed to clear out the rx buffer. Will give inac
K_CharIn := Serial.Rx
PicIn := K_CharIn
if PicIn == "p"
Speaker_4_Pic
if K_CharIn == "x" '1700 is approximately 4' from kinect. Larger = farthe
K_DataIn_x := Serial.RxDec
repeat until K_CharIn == "z" '1700 is approximately 4' from kinect. Lar
Serial.RxFlush
K_CharIn := Serial.Rx
K_DataIn_z := Serial.RxDec
PUB k_LED | Check
'If not tracking set off.

'If tracking set on.
repeat
if K_CharIn == "x" OR K_CharIn == "z" AND K_DataIn_x <> 0 AND K_DataIn_z <> 0
DIRA[K_track_led] := OUTA[K_track_led] := 1
waitcnt(clkfreq/10+ cnt)
OUTA[K_track_led] := 0
{
repeat
Check := Serial.Rxcheck
if Check <> -1
DIRA[K_track_led] := OUTA[K_track_led] := 1
\ [date]
OUTA[K_track_led] := 0
waitcnt(clkfreq/10 +cnt)
}
PUB Batt_RGB_LED 'Could also do a mixture of rgb to create a finer resolution o
DIRA[Battery_GreenLED] := 1
DIRA[Battery_BlueLED] := 0
DIRA[Battery_RedLED] := 0
repeat
Battery_Voltage := adc.in(7)
' pst.dec(Battery_Voltage)
' pst.Str(String(pst#NL))
' waitcnt(clkfreq/10 + cnt)
if Battery_Voltage > 3_000

OUTA[Battery_GreenLED] := 1
OUTA[Battery_BlueLED] := 0
OUTA[Battery_RedLED] := 0
if Battery_Voltage < 3_000 and Battery_Voltage > 2_500
if Battery_Voltage < 2_500
PUB Speaker_4_Pic | i, a
DIRA[Speaker_Pin] := OUTA[Speaker_Pin] := 1
repeat 2
repeat 300
!OUTA[Speaker_Pin]
waitcnt(clkfreq/(500) + cnt)
repeat 1
repeat 750
!OUTA[Speaker_Pin]
waitcnt(clkfreq/(900) + cnt)
PUB Servo_Straight(Servo_Pin) | Frequency, Pulse_length, t, i
ctra[30..26] := %00100 'Setting up counter modules for PWM

ctra[5..0] := Servo_Pin
frqa := frqb := 1 'The frq register adds one to the current value of the ph
dira[Servo_Pin]~~
Frequency := clkfreq/50 'Freq = 50Hz
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''PULSE LENGTH RANGE (and initial pos

\ [date]
'Pulse_length := clkfreq/1667 'For 0 degrees, pulse width =

' Pulse_length := clkfreq/1111 'For 30 degreees
Pulse_length := clkfreq/690 'For Servos to face directly
'Pulse_length := clkfreq/476 'For 150 degrees
''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
t := cnt
repeat 30
phsa := phsb := -(pulse_length) 'Why is this negative?
t += Frequency 'phsa is the length of the pulse duration
waitcnt(t)
PUB Auto_Mode 'IR_range(x) gets larger as objects closer. Range(x) ge

repeat
If (DataIn == "y") AND (range0 > Ping_dist) AND (range1 > Ping_dist) AND (range2 > Ping_dis
EL_tapeCOG := cognew(EL_tape, @Stack2)
ServoCog := cognew(Servo(front_servo4), @stack1)
Forward("y")
If (DataIn == "y") AND ((range1 < Ping_dist) OR (IR_range0 > IR_dist_ground)) 'Center
BeepCog := cognew(Beep, @Stack3)
ServoCog := cognew(Servo(rear_servo5), @stack4)
Reverse(60)
If (DataIn == "y") AND ((range0 < Ping_dist) OR (range10 < Ping_dist) OR (IR_range1 > IR_dis
Rotate90CCW
If (DataIn == "y") AND ((range2 < Ping_dist) OR (range8 < Ping_dist) OR (IR_range2 > IR_dis
Rotate90cw
if DataIn <> "y"

Start
PUB Pings 'Reads DataIn and Knct_dist so that o
repeat
range0 := ping.Inches(ping_pin0) 'Front right
range1 := ping.Inches(ping_pin1) 'Front on servo
range2 := ping.Inches(ping_pin2) 'Front left
range3 := ping.Inches(ping_pin3) 'Back
range7 := ping.Inches(ping_pin7) 'Top/Rear/Left
range8 := ping.Inches(ping_pin8) 'Top/Front/Left
range9 := ping.Inches(ping_pin9) 'Top/Rear/Right
range10:= ping.Inches(ping_pin10) 'Top/Front/Right
IR_range0 := adc.in(0) 'front facing down

IR_range1 := adc.in(1) 'front right(It's right)
IR_range2 := adc.in(2) 'front left
IR_range3 := adc.in(3) 'back
DataIn := XB.Rx 'Having this cog check DataIn slows down the ping sen
if (DataIn <> "y") 'Can't stop a cog that has never started.
cogstop(El_tapeCog)
\ [date]
cogstop(PingsCog) 'This must be last or else any commands that follow will not be exe
cogstop(ServoCog)
PUB Forward(mode) | Frequency, Pulse_length, t 'AUTO_kinect 'Repeats

ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 0

dira [21] := outa [21] := 1
dira [22] := outa [22] := 0
dira [24] := outa [24] := 1
frqa := frqb := 1 'The frq register adds one to the current value of the p
dira[20]~~
dira[23]~~
Frequency := clkfreq/20_000 'This controls the frequency that the repeat loops cycle
Pulse_length := Frequency/Speed 'This controls the duration within a single cycle of the
'Would like to exit the loop when transmitter power is shut off, but I can't figure out how(N
repeat until (dataIn <> mode) or (range0 < Ping_dist) or (range1 < Ping_dist) or (range2 < P
t := cnt 'The reading of the current clock position mu
repeat 1_000 'Cannot check DataIn := XB.Rx while in the re
phsa := phsb:= -Pulse_length 'Why is this negative?
t += Frequency 'phsa is the length of the pulse duration.
waitcnt(t)
ctra[30..26] := %00000 'This is here to get rid of the sudden burst

ctrb[30..26] := %00000
if EL_tapeCOG <> 0
cogstop(EL_tapeCOG)
if ServoCog <> 0
cogstop(ServoCog)
if dataIn == "y"
Auto_Mode
if DataIn <> "y"

Start
PUB Servo(Servo_Pin) | Frequency, Pulse_length, t, i

ctra[5..0] := Servo_Pin
frqa := frqb := 1 'The frq register adds one to the current value of the ph
dira[Servo_Pin]~~
Frequency := clkfreq/50 'Freq = 50Hz
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''PULSE LENGTH RANGE'''''''''''''''''

'Pulse_length := clkfreq/1667 'For 0 degrees, pulse width =
Pulse_length := clkfreq/1111 'For 30 degreees
\ [date]
'Pulse_length := clkfreq/476 'For 150 degrees

''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
repeat
t := cnt
repeat while i < 100000
i += 3000
phsa := phsb := -(pulse_length + i) 'Why is this negative?
waitcnt(t)
t := cnt
repeat while i > -15000 '27_000 seems to be the farthest right that the servo can pan.
i -= 3000
phsa := phsb := -(pulse_length + i) 'Why is this negative?
waitcnt(t)
PUB EL_tape
dira[EL_Pin]~~
repeat
!outa[EL_Pin]
PUB Encoder_Read
PUB Reverse(iterations) | Frequency, Pulse_length, t ''For AUTO MODE. Remember: 'Cann
ctra[30..26] := %00100
ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 1

dira [21] := outa [21] := 0
dira [22] := outa [22] := 1
dira [24] := outa [24] := 0
frqa := frqb := 1 'The frq register adds one to the curren
dira[20]~~
dira[23]~~
Frequency := clkfreq/20_000 'This controls the frequency that the rep

Pulse_length := Frequency/Speed 'This controls the duration within a sing
if (DataIn == "y") and (range3 > Ping_dist) and (IR_range3 < IR_dist) AND (range7 > Ping_dist)
repeat iterations
if (range3 > Ping_dist) AND (IR_range3 < IR_dist) AND (range7 > Ping_dist) AND (range9 > Pin
t := cnt 'The reading
repeat 1_000 'The number
phsa := phsb:= -Pulse_length
t += Frequency 'phsa is the
waitcnt(t)
cogstop(BeepCOG)
cogstop(servoCog)

\ [date]
waitcnt(clkfreq/5 + cnt) 'This is th
if (DataIn == "y") 'or (DataIn == "k")

Rotate90CCW
PUB Beep
repeat
repeat 350
!OUTA[Speaker_Pin]
PUB Rotate90CCW | Frequency, Pulse_length, t 'Used for AUTO MODE.
ctra[30..26] := %00100
ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 1

dira [21] := outa [21] := 0
dira [22] := outa [22] := 0
dira [24] := outa [24] := 1
frqa := frqb := 1 'The frq register adds one to the current v
dira[20]~~
dira[23]~~
Frequency := clkfreq/20_000 'This controls the frequency that the repeat

Pulse_length := Frequency/Speed 'This controls the duration within a single
if DataIn == "y" ' or DataIn == "k"

t := cnt 'The reading o
repeat rotate_time 'The numb
t += Frequency 'phsa is the l
waitcnt(t)
ctra[30..26] := %00000
ctrb[30..26] := %00000
if (DataIn == "y")
Auto_Mode
' if(DataIn == "k")
' Track_Mode
PUB Rotate90CW | Frequency, Pulse_length, t 'Used for AUTO MODE.
ctra[30..26] := %00100
ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
\ [date]
dira [19] := outa [19] := 0

dira [21] := outa [21] := 1
dira [22] := outa [22] := 1
dira [24] := outa [24] := 0
frqa := frqb := 1 'The frq register adds one to the current value of the phs reg
dira[20]~~
dira[23]~~
Frequency := clkfreq/20_000 'This controls the frequency that the repeat loops cycles
Pulse_length := Frequency/Speed 'This controls the duration within a single cycle of the a
if DataIn == "y" 'or DataIn == "k"

t := cnt 'The reading o
repeat rotate_time 'The numbe
t += Frequency 'phsa is the l
waitcnt(t)
ctra[30..26] := %00000
ctrb[30..26] := %00000
if (DataIn == "y")
Auto_Mode
' if(DataIn == "k")
' Track_Mode
Pub Manualforward | Frequency, Pulse_length, t 'This method needs some work. Somehow it
ctra[30..26] := %00100
ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 0

dira [21] := outa [21] := 1
dira [22] := outa [22] := 0
dira [24] := outa [24] := 1
frqa := frqb := 1 'The frq register adds o
dira[20]~~
dira[23]~~
Frequency := clkfreq/20_000 'This controls the frequ

Pulse_length := Frequency/Speed 'This controls the durat
JoyStickValueUDCog := cognew(JoyStickValueUD, @stack) 'JoyStickValueCog return

waitcnt(clkfreq/10 + cnt) 'Have to use another cog
t := cnt
repeat until DataUD < 800 'Speed holds here until

phsa := phsb:= -Pulse_length 'The reading of the curr
t += Frequency 'Cannot check DataIn :=
waitcnt(t)
\ [date]
cogstop(JoyStickValueUDCog)
ctra[30..26] := %00000 ''Put this here to
ctrb[30..26] := %00000
Start
PUb JoyStickValueUD 'This is used by ManualForward
repeat
DataIn := XB.Rx 'Here we read the value being sent from the controller to th
if DataIn == "!"
DataUD := XB.RxDec
Pub CCWmanual | Frequency, Pulse_length, t
ctra[30..26] := %00100 'Configures counters for CTRMODE(the 30..26 part) and single ended NC
ctrb[30..26] := %00100
ctra[5..0] := 20 'These pins are where the signal is sent.

ctrb[5..0] := 23
dira [19] := outa [19] := 1 'Selects the direction of the motors.

dira [21] := outa [21] := 0
dira [22] := outa [22] := 0
dira [24] := outa [24] := 1
dira[20]~~
dira[23]~~ 'Sets PWM pins as outputs(the ~~ part means they are out
JoyStickValueLRCog := cognew(JoyStickValueLR, @stack) 'JoyStickValueCog return

t := cnt
repeat until DataLR > 200 'Speed holds here until cond
phsa := phsb:= -Pulse_length 'The reading of the current
t += Frequency 'Cannot check DataIn := XB.R
waitcnt(t)
cogstop(JoyStickValueLRCog)
ctra[30..26] := %00000
ctrb[30..26] := %00000
PUb JoyStickValueLR 'This is used by CCWmanual
repeat
DataIn := XB.Rx 'Here we read the value being sent from the controller to th
if DataIn == "@"
DataLR := XB.RxDec
PUB CWmanual | Frequency, Pulse_length, t
ctra[30..26] := %00100
ctrb[30..26] := %00100
\ [date]
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 0

dira [21] := outa [21] := 1
dira [22] := outa [22] := 1
dira [24] := outa [24] := 0
dira[20]~~
JoyStickValueLRCog := cognew(JoyStickValueLR, @stack) 'JoyStickValueCog return

t := cnt
repeat until DataLR < 800 'Speed holds here until cond
waitcnt(t)
cogstop(JoyStickValueLRCog)
ctra[30..26] := %00000
ctrb[30..26] := %00000
PUB K_Forward | Frequency, Pulse_length, t 'AUTO_kinect 'Repeats unt

ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 0

dira [21] := outa [21] := 1
dira [22] := outa [22] := 0
dira [24] := outa [24] := 1
dira[20]~~
dira[23]~~
\ [date]
Frequency := clkfreq/20_000 'This controls the frequency that the repeat loops cycle
Pulse_length := Frequency/(K_Speed) 'This controls the duration within a single cycle of
repeat until (K_DataIn_z < K_Far_dist) OR (DataIn <> "k") OR (K_CharIn == "l") OR (K_DataIn_x
t := cnt 'The reading of the current clock position mu
repeat 1_000 'Cannot check DataIn := XB.Rx while in the re
phsa := phsb:= -Pulse_length 'Why is this negative?
t += Frequency 'phsa is the length of the pulse duration.
waitcnt(t)

ctrb[30..26] := %00000
PUB K_Reverse | Frequency, Pulse_length, t ''For AUTO MODE. Remember: 'Cannot check D
ctra[30..26] := %00100
ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 1

dira [21] := outa [21] := 0
dira [22] := outa [22] := 1
dira [24] := outa [24] := 0
frqa := frqb := 1 'The frq register adds one to the curren
dira[20]~~
dira[23]~~
Frequency := clkfreq/20_000 'This controls the frequency that the rep

Pulse_length := Frequency/(K_Speed) 'This controls the duration within a
repeat until (K_DataIn_z > K_Near_dist) OR (DataIn <> "k") OR (K_CharIn == "l") OR (range3 < P
t := cnt
repeat 1_000 'The readin
t += Frequency 'phsa is the
waitcnt(t)

ctrb[30..26] := %00000
waitcnt(clkfreq/5 + cnt) 'This is th
PUB K_CW | Frequency, Pulse_length, t
ctra[30..26] := %00100
ctrb[30..26] := %00100
ctra[5..0] := 20
ctrb[5..0] := 23
dira [19] := outa [19] := 0

dira [21] := outa [21] := 1
\ [date]
dira [22] := outa [22] := 1

dira [24] := outa [24] := 0
dira[20]~~
Pulse_length := Frequency/(K_Speed) 'This controls the duration within a single cycle of t
repeat until (K_DataIn_x > -K_Horiz_dist) OR (DataIn <> "k") OR (K_CharIn == "l") OR (range0 < P
t := cnt
repeat 100
waitcnt(t)
ctra[30..26] := %00000
ctrb[30..26] := %00000
Pub K_CCW | Frequency, Pulse_length, t
ctra[30..26] := %00100 'Configures counters for CTRMODE(the 30..26 part) and single ended NC
ctrb[30..26] := %00100
ctra[5..0] := 20 'These pins are where the signal is sent.

ctrb[5..0] := 23
dira [19] := outa [19] := 1 'Selects the direction of the motors.

dira [21] := outa [21] := 0
dira [22] := outa [22] := 0
dira [24] := outa [24] := 1
dira[20]~~
Pulse_length := Frequency/(K_Speed) 'This controls the duration within a single cycle of t
repeat until (K_DataIn_x < K_Horiz_dist) OR (DataIn <> "k") OR (K_CharIn == "l") OR (range2 < Pi
t := cnt
repeat 100
waitcnt(t)
ctra[30..26] := %00000
ctrb[30..26] := %00000

Autonomous Follower

Caricato da

Informazioni sul documento

Titolo originale

Copyright

Formati disponibili

Condividi questo documento

Condividi o incorpora il documento

Opzioni di condivisione

Hai trovato utile questo documento?

Questo contenuto è inappropriato?

Copyright:

Formati disponibili

Autonomous Follower

Caricato da

Copyright:

Formati disponibili

\ [date]

''This code gives Eddie three modes of opperation:

ping_pin9 = 9 'Rear/Top/It's right

'Pins and Baud rate for XBee comms.

VAR 'This section can be optimized. ie: Do they all need

if DataIn == "k" 'Kinect

if DataIn == "y" 'ASCII decimal equivalent for the character y

if DataIn == "@" 'LR 'Can use 64 or "@" here.

if DataLR < 200

if DataIn == "!" 'UD 'Can use 33 or "!" here.

ctra[30..26] := %00000 'This is here to get rid of the sudden burs

Pub Track_Mode 'pst.dec(123) Either pst OR Serial, not both!!!

PUB Pings_k 'Reads DataIn and Knct_dist so that

IR_range0 := adc.in(0) 'front facing down

' IR_range1 := adc.in(1) 'front right(It's right)

if (DataIn <> "k")

PUB P_Check | PicIn

K_CharIn := Serial.Rxcheck 'This must be outside of the repeat loop.

if K_CharIn == "x" '1700 is approximately 4' from kinect. Larger = farthe

PUB k_LED | Check

'If not tracking set off.

PUB Batt_RGB_LED 'Could also do a mixture of rgb to create a finer resolution o

if Battery_Voltage > 3_000

PUB Servo_Straight(Servo_Pin) | Frequency, Pulse_length, t, i

ctra[30..26] := %00100 'Setting up counter modules for PWM

Frequency := clkfreq/50 'Freq = 50Hz

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''PULSE LENGTH RANGE (and initial pos

'Pulse_length := clkfreq/1667 'For 0 degrees, pulse width =

PUB Auto_Mode 'IR_range(x) gets larger as objects closer. Range(x) ge

if DataIn <> "y"

PUB Pings 'Reads DataIn and Knct_dist so that o

IR_range0 := adc.in(0) 'front facing down

PUB Forward(mode) | Frequency, Pulse_length, t 'AUTO_kinect 'Repeats

ctra[30..26] := %00100 'Setting up counter modules for PWM

dira [19] := outa [19] := 0

ctra[30..26] := %00000 'This is here to get rid of the sudden burst

if DataIn <> "y"

PUB Servo(Servo_Pin) | Frequency, Pulse_length, t, i

ctra[30..26] := %00100 'Setting up counter modules for PWM

Frequency := clkfreq/50 'Freq = 50Hz

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''PULSE LENGTH RANGE'''''''''''''''''

'Pulse_length := clkfreq/476 'For 150 degrees

PUB Reverse(iterations) | Frequency, Pulse_length, t ''For AUTO MODE. Remember: 'Cann

dira [19] := outa [19] := 1

frqa := frqb := 1 'The frq register adds one to the curren

Frequency := clkfreq/20_000 'This controls the frequency that the rep

page \ [page] of \ [pages]

waitcnt(clkfreq/5 + cnt) 'This is th

if (DataIn == "y") 'or (DataIn == "k")

PUB Rotate90CCW | Frequency, Pulse_length, t 'Used for AUTO MODE.

dira [19] := outa [19] := 1

frqa := frqb := 1 'The frq register adds one to the current v

Frequency := clkfreq/20_000 'This controls the frequency that the repeat

if DataIn == "y" ' or DataIn == "k"

dira [19] := outa [19] := 0

if DataIn == "y" 'or DataIn == "k"

dira [19] := outa [19] := 0

frqa := frqb := 1 'The frq register adds o

Frequency := clkfreq/20_000 'This controls the frequ

JoyStickValueUDCog := cognew(JoyStickValueUD, @stack) 'JoyStickValueCog return

repeat until DataUD < 800 'Speed holds here until

PUb JoyStickValueUD 'This is used by ManualForward

Pub CCWmanual | Frequency, Pulse_length, t