{{
┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐
│   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.                         │
└──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘
}}