{{
Q-Bot 1.spin
Richard Lamb (cavelamb)
}}
CON
_CLKMODE = XTAL1 + PLL16X ' 80 Mhz clock
_XINFREQ = 5_000_000
OneSec = _XINFREQ
MSec = _XINFREQ / 1_000 * 8
USec = _XINFREQ / 1_000_000
'======================================
' bit level constants
out = %1
on = %1
off = %0
'======================================
' Pin Definitions:
' LSB MSB
' 000000000 01111111 11122222 2222233
' 012345678 90123456 78901234 5678901
PADpins = %11111111_00000000_00000000_00000000 ' 0..7 Touchpads
MOTPins = %00000000_11110000_00000000_00000000 ' 8..11 Motor Control
PWMpins = %00000000_00001100_00000000_00000000 ' 12..13 Motor PWM
LCDpins = %00000000_00000010_00000000_00000000 ' 14 LCD Display
IRCpins = %00000000_00000001_00000000_00000000 ' 15 Infra Red Controller
LEDpins = %00000000_00000000_11111111_00000000 ' 16..23 QS LEDs
OBXpins = %00000000_00000000_00000000_11110000 ' 24..27 Obsticle sensors
EPCpins = %00000000_00000000_00000000_00001000 ' 28 EEPROM Clock
EPDpins = %00000000_00000000_00000000_00000100 ' 29 EEPROM Data
USBpins = %00000000_00000000_00000000_00000011 ' 30..31 USB
OUTpins = MOTpins| PWMpins| LEDpins| LCDpins
'======================================
' Motor control pins for TB6612FNG
MotA1 = 8
MotA2 = 9
MotB1 = 10
MotB2 = 11
PWMA = 12
PWMB = 13
'======================================
' Motor Direction Masks for TB6612FNG
Mot_Fwd = %0110 ' Go fwd
Mot_Aft = %1001 ' Go Aft
Mot_Rit = %0010 ' Turn Right
Mot_Lft = %0100 ' Turn Left
Mot_RoR = %1010 ' Rotate Right
Mot_RoL = %0101 ' Rotate Left
Mot_Off = %0000 ' All Stop
'======================================
' Liquid Crystal Display - 2 line'
LCDpin = 14
LCDon1 = $16 ' LCD on; cursor off, blink off
LCDline1 = $80 ' move to line 1, column 0
LCDline2 = $94 ' move to line 2, column 0
LCD_TimeOut = 10 ' seconds
'======================================
IRCpin = 15
'======================================
' GP2Y0D810Z0F IR obsticle sensors input pins
DetFL = 24 ' Fwd Left
DetFR = 25 ' Fwd Right
DetAL = 26 ' Aft Left
DetAR = 27 ' Aft Right
'======================================
OBJ
ir : "IRC"
lcd : "serial_lcd"
num : "simple_numbers"
'======================================
VAR
long Stack1[6] ' LCD_Timeout cog = 6 longs
long IRcog, HeartCnt ' makes one long
Long Cmd_Addr, Heart_Addr ' two longs
byte X, Power, Channel, Volume ' makes one long
Byte V_Chan, V_Vol, V_Pwr, V_misc ' makes one long
Byte V_Turn, V_Speed, MOTcmd, LED ' makes one long
Byte LCD_Time, IRC_ret, x1 , x2 ' makes one long
Byte TurnType
'======================================
PUB Init | freq, index, cog, IRcode
' pin directions
outa[ 16..23 ] := $00 ' all LED pins off '
dira [0..31] := OUTpins ' make all output pins outputs
outa [12..13] := %11 ' PWMpins enable motors
TurnType := 1 ' 1 = turn 2 = spin
'======================================
' init LCD
if lcd.start(LCDpin, 9600, 2)
lcd.putc(lcd#Lcd_On1) ' no cursor
lcd.backlight(1)
lcd.cls
ProgramVersion
' waitMS(500)
' Start LCD_Timeout
LCD_Time := LCD_TimeOut ' reset LCD_Time each time a key is pressed
cognew (LCD_Timer, @stack1 )
'======================================
' Init IR remote
IRcog := ir.Start(IRCpin, @IRC_ret) ' Pin of IR receiver, address of variable
'===================================== '
' Top of IR Code input loop:
if IRcog > 0
repeat
If LCD_Time >0
LCD.backlight(1) ' turn it on
else ' timed out
LCD.backlight(0) ' turn if off
If IRC_ret <> ir#NoNewCode ' we have a key code
IRcode := IRC_ret
ir.Start(IRCpin, @IRC_ret) ' set up for next code
if LCD_Time := 0 ' if it was off,
LCD.backlight(1) ' turn it back on
LCD_Time := LCD_Timeout ' reset LCD_Timeout each time a key is pressed
lcd.gotoxy(0,1)
case IRcode ' Parse the key code
' control keys
ir#power :
LCD.CLS
lcd.str(string("Power "))
if V_Pwr == 1
V_Pwr := 0
lcd.str(string("OFF "))
LCD_Time := 1
outa [12..13] := %00 ' hit brakes
WaitMS(500) ' for this long
outa [16..23] := Mot_Off 'then turn off drive
outa [8..11] := Mot_Off
outa [12..13] := %11 're-enable motors !!
else
V_Pwr := 1
lcd.str(string("ON "))
ProgramVersion
ir#chUp :
LCD.CLS
lcd.str(string("Forward "))
outa [16..19] := Mot_Fwd
outa [8..11] := Mot_Fwd
ir#chDn :
LCD.CLS
lcd.str(string("Back "))
outa [16..19] := MOT_Aft
outa [8..11] := Mot_Aft
ir#volUp :
LCD.CLS
lcd.str(string("Left "))
GoLeft
' If TurnType == 1
' outa [16..19] := Mot_Lft ' show code on LEDs
' outa [ 8..11] := Mot_Lft ' make it go
' else
' outa [16..19] := Mot_RoL
' outa [8..11] := Mot_RoL
ir#volDn :
LCD.CLS
lcd.str(string("Right "))
GoRight
' If TurnType == 1
' outa [16..19 ] := MOT_Rit
' outa [ 8..11] := Mot_Rit
' else
' outa [16..19] := Mot_RoR
' outa [ 8..11] := Mot_RoR
ir#OK :
LCD.CLS
GoStop
' lcd.str(string(LcdLine1, "Stop "))
' outa [12..13] := %00 ' hit brakes
' WaitMS(500) ' for this long
'
' outa [16..23] := Mot_Off 'then turn off drive
' outa [8..11] := Mot_Off
' outa [12..13]:= %11 'PWM re-enable motors
' numeric keys
ir#zero :
LCD.CLS
lcd.str(string("<0> "))
ir#one :
LCD.CLS
lcd.str(string("<1> Turn "))
TurnType := 1
ir#two :
LCD.CLS
lcd.str(string("<2> Rotate "))
TurnType := 2
ir#three :
LCD.CLS
lcd.str(string("<3> "))
ir#four :
LCD.CLS
lcd.str(string("<4> Half Left and Back"))
outa [16..19] := Mot_RoR ' to LEDs
outa [ 8..11] := Mot_RoR
WaitMS (2000)
brakes(200)
outa [16..19] := Mot_RoL ' to LEDs
outa [8..11] := Mot_RoL
WaitMS (2000)
brakes(200)
outa [16..19] := %0000
outa [8..11] := %0000
brakes(200)
outa [ 8..11] := Mot_RoR
WaitMS (50)
brakes(200)
LCD.CLS
ir#five :
LCD.CLS
lcd.str(string("<5> "))
ir#six :
LCD.CLS
lcd.str(string("<6> Half Right and back"))
outa [16..19] := Mot_RoL
outa [ 8..11] := Mot_RoL
WaitMS (2000)
brakes(200)
outa [16..19] := Mot_RoR
outa [8..11] := Mot_RoR
WaitMS (2000)
brakes(200)
outa [16..19] := %0000
outa [8..11] := %0000
brakes(200)
outa [ 8..11] := Mot_RoL
WaitMS (50)
brakes(200)
LCD.CLS
ir#seven :
LCD.CLS
lcd.str(string("<7> Do the Twist"))
TurnType := 2
x1 := 2
Repeat While X1 > 0
X2 := 5
Repeat while X2 > 0
GoRight
WaitMS(300)
GoLeft
WaitMS(400)
X2 = X2 - 1
X2 := 5
Repeat while X2 > 0
GoLeft
WaitMS(300)
GoRight
WaitMS(400)
X2 = X2 - 1
GoLeft
WaitMS(4000)
GoStop
x1 -= 1
ir#eight :
LCD.CLS
lcd.str(string("<8> "))
ir#nine :
LCD.CLS
lcd.str(string("<9> "))
waitcnt((clkfreq / 1000) * 30 + cnt)
waitcnt((clkfreq / 1000) * 30 + cnt)
'======================================
PUB GoFwd
outa [16..19] := Mot_Fwd
outa [8..11] := Mot_Fwd
'======================================
PUB GoBack
outa [16..19] := MOT_Aft
outa [8..11] := Mot_Aft
'======================================
PUB GoRight
If TurnType == 1
outa [16..19] := MOT_Rit
outa [ 8..11] := Mot_Rit
else
outa [16..19] := Mot_RoR
outa [ 8..11] := Mot_RoR
'======================================
PUB GoLeft
If TurnType == 1
outa [16..19] := Mot_Lft ' show code on LEDs
outa [ 8..11] := Mot_Lft ' make it go
else
outa [16..19] := Mot_RoL
outa [ 8..11] := Mot_RoL
'======================================
PUB GoStop
outa [12..13] := %00 ' hit brakes
WaitMS(500) ' for this long
outa [16..23] := Mot_Off ' show LEDs
outa [ 8..11] := Mot_Off ' then turn off motors
outa [12..13] := %11 ' PWM re-enables motors
'======================================
PUB Brakes(y)
outa [12..13] := %00 ' PWMpins = 0 => hit brakes
WaitMS(y) ' for this long
outa [16..23] := Mot_Off ' LEDpins
outa [ 8..11] := Mot_Off ' then turn off drive
outa [12..13] := %11 ' PWMpins on re-enable motors
'======================================
PUB WaitMS(W) 'wait for W milliseconds
W := W * MSec
WaitCNT (W+cnt)
'======================================
PUB LCD_Timer
Repeat ' loop forever
waitcnt(clkfreq + cnt) ' wait one second
if byte[@LCD_Time] => 1 ' keep counting
byte[@LCD_Time] -- ' down
'======================================
PUB ProgramVersion
lcd.str(string(lcdline1, "Q-Bot 1A4"))
lcd.str(string(lcdline2, "08/16/2018"))
return
'======================================
_3u05Tpwasz.png?auto=compress%2Cformat&w=40&h=40&fit=fillmax&bg=fff&dpr=2)
Comments