Spin Code for Spin Stamp on Hybrid Beam Walker
- October 13th, 2011
- Posted in BEAM robotics . Robot . code
- By skaterj10
- Write comment
0
Here is the code I used to test the Spin stamp on the Walker when the hardware was completed:
''Test Program for Spin stamp on Scoutwalker III
''Written for Jonathan Knil by Jeremie Boulianne
''January 31, 2011
''This program uses the Spin stamp to monitor and control
''the ScoutWalker III from Solarbotics
CON
_clkmode = xtal1 + pll8x ' p118x = constant for clkmode frequency times 8
' xtal1 = constant for low-speed crystal
_xinfreq = 10_000_000 ' 10Mhz Crystal on board Spin Stamp
PC_TX = 30
PC_RX = 31
PC_Baud = 57_600
PC_Mode = 1
'Special Characters
LF = 10
CR = 13
CLS = 16
VAR
long Pause, Tail, LitesensL, LitesensR ' Variable declarations
long Ltact, Rtact, RearM1, RearM2, FrntM1, FrntM2
byte ClkStart, DurationL, DurationR, Clkstop
obj
DEBUG: "fullduplexserialplus" ' Debug to terminal screen
PUB Scoutwalker
Pause := clkfreq/1_000
Init_DEBUG ' Starts up Cog 2 for serial communtications to computer
' For the DIRA Register '~' means input '~~' means output
dira[0..2]~ ' Configure Pins 0 (backup), 1 (right light sense)
' & 2 (left light sense) as inputs
dira[3..4]~~ ' Configure Pins 3 (left drive) & 4 (right drive)
' as outputs to activate hard turns
' For the OUTA Register '~' means low (0V) output
' '~~' means high (3.3V)
outa[3..4]~ ' Initialize pin 3, pin 4 low
dira[5..10]~ ' Configure Pins 5 (Rear Mtr input), 6 (Rear Mtr input),
' 7 (Direct left tact sensor), 8 (Direct right tact sensor),
' 9 (Front Mtr input), & 10 (Front Mtr input) as inputs
dira[11..15]~~ ' Configure Pins 11 - 15 LED lines as outputs
outa[11..15]~ ' Initialize LED Pins 11 - 15 low
repeat
outa[3..4]~~ ' Initialize P3 & P4 high so turning transistors are deactivated
Tail := ina[0] ' Get state of P0
LitesensR := ina[1] ' Get state of P1
LitesensL := ina[2] ' Get state of P2
Ltact := ina[7] ' Get state of P7
Rtact := ina[8] ' Get state of P8
{{ Condition that watches the Right tactile sensor for activation causes walker to turn left for ~6 seconds on activation }}
If Rtact == 0
outa[3]~ ' Activate Low enabled PNP transistor for a left turn
outa[12]~~ ' LED2 On
waitcnt(Pause * 6000 + cnt) ' ~ 6 second delay
outa[12]~ ' LED2 Off
{{ Condition that watches the Left tactile sensor for activation causes walker to turn right for ~6 seconds on activation }}
If Ltact == 0
outa[4]~ ' Activate Low enabled PNP transistor for a left turn
outa[13]~~ ' LED3 On
waitcnt(Pause * 6000 + cnt) ' ~ 6 second delay
outa[13]~ ' LED3 Off
{{ Block of code that calculates and stores the value of the Light level in your environment, read from the Left IR sensor }}
ClkStart := cnt ' Save counter for start time
waitpne(%0100, %0100, 0) ' Wait for opposite state to end
Clkstop := cnt ' Save stop time
DurationL := (Clkstop - ClkStart) ' calculate the Left IR sensor value in 1us resolution
debug.str(string("LightsensL = "))
debug.dec(DurationL) ' output the Left IR sensor value on the serial terminal
debug.tx(CR) ' Keep the value on screen for 100 mS
waitcnt(Pause * 100 + cnt)
{{ Block of code that calculates and stores the value of the Light level in your environment, read from the Right IR sensor }}
ClkStart := cnt ' Save counter for start time
waitpne(%0010, %0010, 0) ' Wait for opposite state to end
Clkstop := cnt ' Save stop time
DurationR := (Clkstop - ClkStart) ' calculate the Right IR sensor value in 1us resolution
debug.str(string("LightsensR = "))
debug.dec(DurationR)
debug.tx(CR) ' output the Right IR sensor value on the serial terminal
waitcnt(Pause * 100 + cnt) ' Keep the value on screen for 100 mS
{{ Condition that watches if the right IR sensor reaches a threshold of 230 the value 230 should be changed according to your lighting conditions}}
If DurationR > 230
outa[3]~ ' Activate Low enabled PNP transistor for a left turn
outa[14]~~ ' LED4 on
waitcnt(Pause * 1000 + cnt) ' ~ 1 second delay
outa[14]~ ' LED4 off
{{ Condition that watches if the right IR sensor reaches a threshold of 230 the value 230 should be changed according to your lighting conditions }}
If DurationL > 230
outa[4]~ ' Activate Low enabled PNP transistor for a left turn
outa[15]~~ ' LED5 on
waitcnt(Pause * 1000 + cnt) ' ~ 1 second delay
outa[15]~ ' LED5 off
pub Init_DEBUG
{{ Starts up serial communication with a computer and displays information to a terminal screen }}
debug.start(PC_RX, PC_TX, PC_Mode, PC_Baud)
debug.str(string(CR,"Starting Program",CR))
debug.str(string(CR,"Running Light Sens Value Debug",CR))
waitcnt(clkfreq + cnt)

No comments yet.