194 lines
23 KiB
Plaintext
Executable File
194 lines
23 KiB
Plaintext
Executable File
{{
|
|
┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐
|
|
│ YAAARC Propeller Robot with VEX RC for control │
|
|
├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤
|
|
│ Project: PropVexRC-Bot.spin │
|
|
│ Author: James Ronald │
|
|
│ Credits: Base on work by Tim Moore's Propeller Robot Framework │
|
|
│ Copyright: James Ronald, November 2009. │
|
|
│ * See License at end of file for terms of use. │
|
|
└──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘
|
|
|
|
This program and its objects are all under construction.
|
|
|
|
INSTRUCTIONS
|
|
‣ Press F8 to compile this application.
|
|
‣ Double-click the PropVexRC-Bot Schematic object in the object browser window for the various schematics.
|
|
‣ Compare the schematic to the comments in this object's init code block (below).
|
|
‣ Adjust the schematic, constants section for your particular wiring.
|
|
‣ Load the code into the Propeller chip with F11, and let it go.
|
|
|
|
MORE INFO
|
|
‣ Open each of the objects and view them in documentation mode. Their instructions should match the commands in this
|
|
object's init section.
|
|
|
|
}}
|
|
|
|
CON
|
|
_clkmode = xtal1 + pll16x
|
|
_xinfreq = 5_000_000
|
|
|
|
LOFFSET = 0 'offset to stop left servo
|
|
ROFFSET = 0 'offset to stop right servo
|
|
|
|
OBJ
|
|
'2 Cog here (main and watchdog)
|
|
schematic : "PropVexRC-Bot Schematic" '0 Cog required
|
|
Joy : "JoyVex" '1 Cog - RC controller
|
|
motor : "motorsrv" '1 Cog - Motor driver
|
|
timer : "Timer" '0 Cog, ctra and ctrb on the calling COG
|
|
leds : "leds" '0 Cog - display leds
|
|
config : "config" '0 Cog required
|
|
'5 Cogs in total
|
|
|
|
VAR
|
|
long lastrcsync 'RC variables
|
|
long Forward, Turn 'Forward and turning robot speed
|
|
long Stack[60], cog, watchdogcounter 'watchdog variables
|
|
|
|
PUB Start | secs, testrcsync, NoControllerTime
|
|
|
|
'Init pin configuration tables
|
|
config.Init(@pininfo, 0)
|
|
|
|
'initialize leds and make sure they are off
|
|
leds.Init(config.GetPin(CONFIG#DEBUG_LED1), config.GetPin(CONFIG#DEBUG_LED3), LEDS#HIGHON)
|
|
leds.SetState(%111, %111)
|
|
|
|
waitcnt(clkfreq*1 + cnt) 'start-up delay
|
|
leds.SetState(%111, %000) 'Clear all the LEDs
|
|
leds.SetState(%001, %001) 'Set no RC Sync LED
|
|
|
|
cog := cognew(WatchDog,@Stack) + 1 'dont start watchdog until after startup debug delay
|
|
|
|
Joy.Start (config.GetPin(CONFIG#VEXDEMUX)) 'Start VEX RC receiver
|
|
|
|
'Start 2 Servos
|
|
motor.Init(config.GetPin(CONFIG#SERVO1), config.GetPin(CONFIG#SERVO2), {
|
|
} JOY#MIN_VALUE, JOY#CENTER_STICK, JOY#MAX_VALUE, ROFFSET, LOFFSET, %01)
|
|
|
|
NoControllerTime := timer.GetSeconds 'assume no RC controller
|
|
|
|
repeat 'Main loop
|
|
watchdogcounter++ 'change watchdogcounter so watchdog knows we are alive
|
|
|
|
secs := timer.GetSeconds 'current time, used to detect RC controller power down
|
|
|
|
testrcsync := Joy.RCFound 'need to get a local copy since RC cog is updating in another COG
|
|
if testrcsync == true 'RC controller is valid
|
|
leds.SetState(%001, %000) 'Clear no RC Sync LED
|
|
ProcessRCController 'We have a RC controller so check its state and do any processing required
|
|
|
|
elseif lastrcsync == true 'lost sync with RC Controller and have been in sync
|
|
leds.SetState(%001, %001) 'Set no RC Sync LED
|
|
motor.MotorStop 'brake motors, set robot speed to zero
|
|
NoControllerTime := secs 'note when we lost the RC controller
|
|
|
|
lastrcsync := testrcsync 'save sync value for lost sync detection
|
|
|
|
PRI ProcessRCController | change, FSpeed, TSpeed, Fire, FireButton
|
|
'All the RC controller processing
|
|
'
|
|
'Check if joystick has changed requested speed
|
|
change := false 'assume no speed change
|
|
|
|
'Move forwards/backwards
|
|
if (FSpeed := motor.ConvertSpeed(Joy.Axis(JOY#FORWARD_JOY))) <> Forward
|
|
change := true
|
|
|
|
'Turning
|
|
if (TSpeed := motor.ConvertSpeed(Joy.Axis(JOY#SIDEWAYS_JOY))) <> Turn
|
|
change := true
|
|
|
|
' Speed changed so update the motors
|
|
if change <> false
|
|
if ||FSpeed > 50
|
|
motor.MotorSpeed(FSpeed, TSpeed/2) ' soften steering if moving
|
|
else
|
|
motor.MotorSpeed(FSpeed, TSpeed)
|
|
|
|
Forward := FSpeed
|
|
Turn := TSpeed
|
|
|
|
|
|
' Check back buttons
|
|
if Joy.Axis(JOY#POWER_SWITCH) == JOY#MAX_VALUE 'back right bottom power's off
|
|
leds.SetState(%010, %010) 'turn 2nd LED on.
|
|
' else
|
|
' leds.SetState(%010, %000) 'turn 2nd LED off.
|
|
|
|
if Joy.Axis(JOY#POWER_SWITCH) == JOY#MIN_VALUE 'back right top button
|
|
leds.SetState(%010, %000) 'turn 2nd LED off.
|
|
|
|
|
|
|
|
PRI WatchDog | lastcounter, secs, lastsec
|
|
' Watchdog dog is also used to run other background tasks
|
|
' Anything run here must not hang/delay/etc
|
|
'
|
|
'Start timer - uses counters on watchdog cog
|
|
timer.Init(config.GetPin(CONFIG#TIME_CLK),TIMER#NOTUSED)'allocates ctra and ctrb
|
|
lastsec := timer.GetSeconds 'so time available to other cogs
|
|
|
|
repeat
|
|
secs := timer.GetSeconds 'so time available to other cogs
|
|
|
|
if secs <> lastsec '
|
|
if lastcounter <> watchdogcounter 'has main cog changed the watchdogcounter
|
|
lastcounter := watchdogcounter 'we are alive
|
|
else
|
|
waitcnt(clkfreq/20 + cnt) 'allow the debug string to be sent
|
|
reboot 'main cog is dead so reboot
|
|
lastsec := secs 'last time we checked and passed
|
|
|
|
|
|
DAT
|
|
pininfo word CONFIG#SERVO1 'pin 0 Left servo motor
|
|
word CONFIG#SERVO2 'pin 1 Right servo motor
|
|
word CONFIG#NOT_USED 'pin 2
|
|
word CONFIG#NOT_USED 'pin 3
|
|
word CONFIG#NOT_USED 'pin 4
|
|
word CONFIG#NOT_USED 'pin 5
|
|
word CONFIG#NOT_USED 'pin 6
|
|
word CONFIG#NOT_USED 'pin 7
|
|
word CONFIG#NOT_USED 'pin 8
|
|
word CONFIG#TIME_CLK 'pin 9 Used by timer
|
|
word CONFIG#NOT_USED 'pin 10
|
|
word CONFIG#NOT_USED 'pin 11
|
|
word CONFIG#NOT_USED 'pin 12
|
|
word CONFIG#NOT_USED 'pin 13
|
|
word CONFIG#NOT_USED 'pin 14
|
|
word CONFIG#NOT_USED 'pin 15
|
|
word CONFIG#NOT_USED 'pin 16
|
|
word CONFIG#NOT_USED 'pin 17
|
|
word CONFIG#NOT_USED 'pin 18
|
|
word CONFIG#NOT_USED 'pin 19
|
|
word CONFIG#NOT_USED 'pin 20
|
|
word CONFIG#NOT_USED 'pin 21
|
|
word CONFIG#NOT_USED 'pin 22
|
|
word CONFIG#NOT_USED 'pin 23
|
|
word CONFIG#DEBUG_LED1 'pin 24 Red LED (RC Sync)
|
|
word CONFIG#DEBUG_LED2 'pin 25 Button LED
|
|
word CONFIG#DEBUG_LED3 'pin 26
|
|
word CONFIG#VEXDEMUX 'pin 27 Vex RC controller
|
|
word CONFIG#I2C_SCL1 'pin 28 proto board I2C_SCL1
|
|
word CONFIG#I2C_SDA1 'pin 29 proto board I2C_SDA1
|
|
word CONFIG#DEBUG_TX 'pin 30 proto board USB serial
|
|
word CONFIG#DEBUG_RX 'pin 31 proto board USB serial
|
|
{{
|
|
┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐
|
|
│ TERMS OF USE: MIT License │
|
|
├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤
|
|
│Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation │
|
|
│files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, │
|
|
│modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software│
|
|
│is furnished to do so, subject to the following conditions: │
|
|
│ │
|
|
│The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.│
|
|
│ │
|
|
│THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE │
|
|
│WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR │
|
|
│COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, │
|
|
│ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. │
|
|
└──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘
|
|
}} |