{{ Mission.spin }}

' ==============================================================================
'
'   File...... Mission.spin
'   Purpose... Mission application for N23 CoolerBot
'   Author.... (C) 2009 Steven R. Norris -- All Rights Reserved
'   E-mail.... steve@norrislabs.com
'   Started... 09/11/2009
'   Updated... 12/02/2009
'
' ==============================================================================

' ------------------------------------------------------------------------------
' Program Description
' ------------------------------------------------------------------------------
{{
  CoolerBot is an outdoor telepresence robot which is be used for nature photography and surveillance.
  It uses a tail-dragger design with 15" wheels in front and a 10" castering wheel in the rear.
  CoolerBot is capable of moving in either direction using two NPC-41250 motors powered by a 10 amp hour 12 volt SLA battery.
  The logic is powered by a separate 10 amp hour 12 volt SLA battery. Both batteries are recharged by the two onboard solar cells.
  The motors are driven by two Parallax HB-25s. CoolerBot is remote controlled through a 912 MHz Transceiver.
}}


' ------------------------------------------------------------------------------
' Revision History
' ------------------------------------------------------------------------------
{{
  0949a - This is the first version
}}


CON
' ------------------------------------------------------------------------------
' Constants
' ------------------------------------------------------------------------------

  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000

  ' Pins
  Pin_MotorRight = 0
  Pin_MotorLeft  = 1
  Pin_Lcd        = 2
  Pin_PIR        = 5
  Pin_DataIn     = 6
  Pin_DataOut    = 7
  Pin_Reserved   = 16
  Pin_Shutter    = 17
  Pin_VidPwr     = 18
  Pin_AuxPwr     = 19
  Pin_Motors     = 20
  Pin_Btn1       = 22
  Pin_Btn2       = 23
  Pin_Spare      = 26
  Pin_PSC        = 27

  ' Move Directions
  Mov_Halt      = 0
  Mov_Fwd       = 1
  Mov_Rev       = 2
  Mov_SpinCW    = 3
  Mov_SpinCCW   = 4

  ' Speeds
  Speed1 = 20
  Speed2 = 50
  Speed3 = 70

  ' PSC Channels
  PSCC_VidHorz  = 12
  PSCC_VidVert  = 14

  ' Camera modes
  CM_Off        = 0
  CM_PIR        = 1
  CM_TimeLapse  = 2


DAT
        Title     byte "N23-CoolerBot",0
        Version   byte "0949a",0


VAR
' ------------------------------------------------------------------------------
' Variables
' ------------------------------------------------------------------------------

  long m_1us
  long m_Center

  long m_MotorsOn
  long m_VideoOn
  long m_AuxOn
  
  long m_CurrentSpeed
  long m_CurrentDir

  long m_VidHorz
  long m_VidVert

  long m_LastPIR

  long m_CameraMode
  long m_ShutterCtr
    
  ' RF command buffer
  byte m_Buffer[32]

  ' Stacks
  long m_StackPIR[50]
  long m_StackMotion[50]
  long m_StackTL[50]
  
  
OBJ

  Lcd           : "debug_lcd"
  RfLink        : "FullDuplexSerial128"
  Psc           : "FullDuplexSerial128"

  
PUB Init
' ------------------------------------------------------------------------------
' Initialize
' ------------------------------------------------------------------------------

  ' Setup for HB-25 servo pulses
  m_1us   := clkfreq / 1_000_000   ' 1 microsecond
  m_Center := clkfreq / 667        ' Center pulse = 1.5 ms

  ' Initialize Relays
  dira[Pin_Reserved..Pin_Motors]~~
  outa[Pin_Reserved..Pin_Motors]~

  ' Initialize motor HB25s
  dira[Pin_MotorLeft..Pin_MotorRight]~~
  outa[Pin_MotorLeft..Pin_MotorRight]~

  dira[Pin_Shutter]~~
  
  Pause_ms(1000)

  ' Initialize PSC (uses 1 cog)
  Psc.Start(Pin_Spare, Pin_PSC, 0, 2400)
  SetPscBaudRate(0)
  ResetVideo
      
  ' Initialize RF serial link (uses 1 cog)
  RfLink.Start(Pin_DataIn, Pin_DataOut, 0, 9600)

  ' Initialize the LCD
  Lcd.init(Pin_Lcd, 19200, 2)                          
  Lcd.cls
  Lcd.home
  Lcd.cursor(0)                                     
  Lcd.backLight(false)
  
  SetPos(0,0)
  Lcd.str(@Title)
  SetPos(1,0)
  Lcd.str(@Version)

  ' Start PIR sensor and Motion Shutter cogs
  cognew(ProcessPIR, @m_StackPIR)
  cognew(ProcessMotion, @m_StackMotion)
  
  ' Start Time Lapse cog
  cognew(ProcessTimeLapse, @m_StackTL)
  
  m_CurrentSpeed := Speed1
  repeat
    ProcessRF
    ProcessButtons
    
    
' ------------------------------------------------------------------------------
' Process PIR Sensor
' ------------------------------------------------------------------------------
PRI ProcessPIR | pir

  repeat
    if m_CurrentDir == Mov_Halt
      ' Set PIR flag
      pir := ina[Pin_PIR]
      if m_LastPIR <> pir
        m_LastPIR := pir
    else
      m_LastPIR := 0
        
  
' ------------------------------------------------------------------------------
' Process Motion Shutter
' ------------------------------------------------------------------------------
PRI ProcessMotion

  dira[Pin_Shutter]~~
  repeat
    if(m_CameraMode == CM_PIR)
      if(m_LastPIR == 1)
        FireShutter(1000)
        Pause_ms(1000)

      
' ------------------------------------------------------------------------------
' Process Time Lapse Shutter
' ------------------------------------------------------------------------------
PRI ProcessTimeLapse

  dira[Pin_Shutter]~~
  repeat
    if(m_CameraMode == CM_TimeLapse)
      FireShutter(1000)
      Pause_ms(4000)


' ------------------------------------------------------------------------------
' Process Buttons
' ------------------------------------------------------------------------------
PRI ProcessButtons | btn

  btn := TestButton
  if btn == Pin_Btn1
    if m_CurrentDir == Mov_Halt
      if(!m_MotorsOn)
          MotorsOn
      m_CurrentSpeed := Speed1
      Direct(Speed1, Speed1)
      m_CurrentDir := Mov_Fwd
    else
      Halt
      m_CurrentDir := Mov_Halt
      RfLink.rxflush
      Pause_ms(500)
      
  elseif btn == Pin_Btn2
    if m_CurrentDir == Mov_Halt
      if(!m_MotorsOn)
          MotorsOn
      m_CurrentSpeed := Speed1
      Direct(-Speed1, -Speed1)
      m_CurrentDir := Mov_Rev
    else
      Halt
      m_CurrentDir := Mov_Halt
      RfLink.rxflush
      Pause_ms(500)
      
      
PRI TestButton : btn

  if ina[Pin_Btn1] == 0
    Pause_ms(30)
    if ina[Pin_Btn1] == 0
      repeat while ina[Pin_Btn1] == 0
      return Pin_Btn1
      
  if ina[Pin_Btn2] == 0
    Pause_ms(30)
    if ina[Pin_Btn2] == 0
      repeat while ina[Pin_Btn2] == 0
      return Pin_Btn2

  return -1
  

' ------------------------------------------------------------------------------
' Process RF Commands
' ------------------------------------------------------------------------------
PRI ProcessRF

  ' Check for and process remote commands
   if CheckRF
     if GetCmd
      ' Dump Telemetry
        if strcomp(@m_Buffer, string("DT"))
          DumpTelem
          
      ' Motors on
        if strcomp(@m_Buffer, string("M1"))
          MotorsOn
          
      ' Motors off
        if strcomp(@m_Buffer, string("M0"))
          MotorsOff
          
      ' Aux on
        if strcomp(@m_Buffer, string("A1"))
          AuxOn
          
      ' Aux off
        if strcomp(@m_Buffer, string("A0"))
          AuxOff
          
      ' Video on
        if strcomp(@m_Buffer, string("V1"))
          VideoOn
          
      ' Video off
        if strcomp(@m_Buffer, string("V0"))
          VideoOff

        ' Video Pan Left          
        if strcomp(@m_Buffer, string("VL"))
          PanLeft
          
        ' Video Pan Right         
        if strcomp(@m_Buffer, string("VR"))
          PanRight
          
        ' Video Tilt Up         
        if strcomp(@m_Buffer, string("VU"))
          TiltUp
          
        ' Video Tilt Down         
        if strcomp(@m_Buffer, string("VD"))
          TiltDown
          
        ' Scan Video          
        if strcomp(@m_Buffer, string("VS"))
          ScanVideo
          
        ' Center Video          
        if strcomp(@m_Buffer, string("VC"))
          ResetVideo
          
      ' Fire Shutter
        if strcomp(@m_Buffer, string("SH"))
          FireShutter(1000)

      ' Camera mode off
        if strcomp(@m_Buffer, string("C0"))
          m_CameraMode := CM_Off
          m_ShutterCtr := 0

      ' Camera mode - PIR
        if strcomp(@m_Buffer, string("C1"))
          m_CameraMode := CM_PIR

      ' Camera mode - time lapse
        if strcomp(@m_Buffer, string("C2"))
          m_CameraMode := CM_TimeLapse

      ' Forward
        if strcomp(@m_Buffer, string("FW"))
          if(!m_MotorsOn)
            MotorsOn
          if m_CurrentDir <> Mov_Halt and m_CurrentDir <> Mov_Fwd
            Halt
            Pause_ms(1000)
          Direct(m_CurrentSpeed,m_CurrentSpeed)
          m_CurrentDir := Mov_Fwd

      ' Reverse
        if strcomp(@m_Buffer, string("BK"))
          if(!m_MotorsOn)
            MotorsOn
          if m_CurrentDir <> Mov_Halt and m_CurrentDir <> Mov_Rev
            Halt
            Pause_ms(1000)
          Direct(-m_CurrentSpeed, -m_CurrentSpeed)
          m_CurrentDir := Mov_Rev
          
      ' Stop
        if strcomp(@m_Buffer, string("ST"))
          Halt
          m_CurrentDir := Mov_Halt
          RfLink.rxflush
            
      ' Left
        if strcomp(@m_Buffer, string("LF"))
          if m_CurrentDir == Mov_Fwd
            VeerLeft
          elseif m_CurrentDir == Mov_Rev
            VeerLeftRev
          elseif m_CurrentDir == Mov_Halt
            Spin(Mov_SpinCCW, 2000)
                         
      ' Right
        if strcomp(@m_Buffer, string("RT"))
          if m_CurrentDir == Mov_Fwd
            VeerRight
          elseif m_CurrentDir == Mov_Rev
            VeerRightRev
          else
            Spin(Mov_SpinCW, 2000)

      ' Left More
        if strcomp(@m_Buffer, string("L2"))
          if m_CurrentDir == Mov_Fwd
            VeerLeftMore
          elseif m_CurrentDir == Mov_Rev
            VeerLeftRev
          else
            Spin(Mov_SpinCCW, 2000)
            
      ' Right More
        if strcomp(@m_Buffer, string("R2"))
          if m_CurrentDir == Mov_Fwd
            VeerRightMore
          elseif m_CurrentDir == Mov_Rev
            VeerRightRev
          else
            Spin(Mov_SpinCW, 2000)

      ' Speed 1
        if strcomp(@m_Buffer, string("S1"))
          m_CurrentSpeed := Speed1
          if(m_CurrentDir == Mov_Fwd)
            Direct(m_CurrentSpeed,m_CurrentSpeed)
          elseif(m_CurrentDir == Mov_Rev)
            Direct(-m_CurrentSpeed, -m_CurrentSpeed)
          
      ' Speed 2
        if strcomp(@m_Buffer, string("S2"))
          m_CurrentSpeed := Speed2
          if(m_CurrentDir == Mov_Fwd)
            Direct(m_CurrentSpeed,m_CurrentSpeed)
          elseif(m_CurrentDir == Mov_Rev)
            Direct(-m_CurrentSpeed, -m_CurrentSpeed)
          
      ' Speed 3
        if strcomp(@m_Buffer, string("S3"))
          m_CurrentSpeed := Speed3
          if(m_CurrentDir == Mov_Fwd)
            Direct(m_CurrentSpeed,m_CurrentSpeed)
          elseif(m_CurrentDir == Mov_Rev)
            Direct(-m_CurrentSpeed, -m_CurrentSpeed)
          

PRI CheckRF : yesno | data

    data := RfLink.rxcheck
    if data == "$"
      yesno := true
    else
      yesno := false
      

PRI GetCmd : status | data,i

  i := 0
  data := 0
  repeat while data <> -1
    data := RfLink.rxtime(5000)

    if data == 13
      m_Buffer[i] := 0
      status := true
      return

    m_Buffer[i] := data

    i++
    m_Buffer[i] := 0
    if i == 31
      quit

  status := false  


PRI VeerRight

  Direct(m_CurrentSpeed * 2 <# 100, m_CurrentSpeed)
  

PRI VeerRightMore

  Direct(m_CurrentSpeed * 3 <# 100, m_CurrentSpeed)


PRI VeerRightRev

  Direct(-m_CurrentSpeed, -(m_CurrentSpeed * 2 <# 100))


PRI VeerLeft

  Direct(m_CurrentSpeed, m_CurrentSpeed * 2 <# 100)


PRI VeerLeftMore

  Direct(m_CurrentSpeed, m_CurrentSpeed * 3 <# 100)


PRI VeerLeftRev

  Direct(-(m_CurrentSpeed * 2 <# 100), -m_CurrentSpeed)


' ------------------------------------------------------------------------------
' Telemetry
' ------------------------------------------------------------------------------
PRI DumpTelem

  RfLink.str(string("$TL"))

  ' Motor power status
  if(m_MotorsOn == true)
    RfLink.dec(1)
  else
    RfLink.dec(0)
  RfLink.tx(",")
  
  ' Video camera power status
  if(m_VideoOn == true)
    RfLink.dec(1)
  else
    RfLink.dec(0)
  RfLink.tx(",")

  ' Aux power status
  if(m_AuxOn == true)
    RfLink.dec(1)
  else
    RfLink.dec(0)
  RfLink.tx(",")

  ' Last PIR status
  RfLink.dec(m_LastPIR)
  RfLink.tx(",")

  ' Current speed
  RfLink.dec(m_CurrentSpeed)
  RfLink.tx(",")

  ' Current direction
  RfLink.dec(m_CurrentDir)
  RfLink.tx(",")

  ' Current camera shutter mode
  RfLink.dec(m_CameraMode)
  RfLink.tx(",")

  ' Current shutter fire count
  RfLink.dec(m_ShutterCtr)
  
  RfLink.tx(13)

  
' ------------------------------------------------------------------------------
' Motor Control
' ------------------------------------------------------------------------------
PRI Direct(LeftPercent, RightPercent) | lp,rp

  if LeftPercent => 0 and LeftPercent < 10
    lp := 0
  elseif LeftPercent < 0
    lp := (44 * ((LeftPercent / 10) + 1)) - 100
  else
    lp := 100 + (44 * ((LeftPercent / 10) - 1))

  if RightPercent => 0 and RightPercent < 10
    rp := 0
  elseif RightPercent < 0
    rp := (44 * ((RightPercent / 10) + 1)) - 100
  else
    rp := 100 + (44 * ((RightPercent / 10) - 1))

  SetDriveMotors(lp, rp)


PRI Spin(Dir,time)

  case Dir
    Mov_SpinCW:
      Direct(80, -80)
      Pause_ms(400)
      Direct(m_CurrentSpeed, -m_CurrentSpeed)
      m_CurrentDir := Mov_SpinCW

    Mov_SpinCCW:
      Direct(-80, 80)
      Pause_ms(400)
      Direct(-m_CurrentSpeed, m_CurrentSpeed)
      m_CurrentDir := Mov_SpinCCW

    
PRI Halt
    
  SetDriveMotors(0, 0)
  m_CurrentDir := Mov_Halt


PRI SetDriveMotors(Left, Right) | leftHi,rightHi

  leftHi := (Left * m_1us) + m_Center
  rightHi := (Right * m_1us) + m_Center

  outa[Pin_MotorLeft]~~
  waitcnt(leftHi + cnt)
  outa[Pin_MotorLeft]~
  
  outa[Pin_MotorRight]~~
  waitcnt(rightHi + cnt)
  outa[Pin_MotorRight]~

  Pause_ms(6)


PRI MotorsOn

  ' Energize the motors power relay
  outa[Pin_Motors]~~
  m_MotorsOn := true
  Pause_ms(1000)
  
  ' Initialize HB25s
  Halt


PRI MotorsOff

  Halt

  ' Tilt the video camera up and down
  ' It's a visual indication that the motors are off
  TiltDown
  Pause_ms(500)
  TiltUp
  Pause_ms(750)
  
  ' De-energize the motor power relay
  outa[Pin_Motors]~
  m_MotorsOn := false
  Pause_ms(500)


' ------------------------------------------------------------------------------
' Video Camera Functions
' ------------------------------------------------------------------------------

PRI VideoOn

  ' Energize the video power relay
  outa[Pin_VidPwr]~~
  m_VideoOn := true


PRI VideoOff
  
  ' De-energize the video power relay
  outa[Pin_VidPwr]~
  m_VideoOn := false


PRI PanLeft

  m_VidHorz := m_VidHorz - 20 #> 290
  SetVideoPos(m_VidHorz, m_VidVert, 12)


PRI PanRight

  m_VidHorz := m_VidHorz + 20 <# 1170
  SetVideoPos(m_VidHorz, m_VidVert, 12)


PRI TiltUp

  m_VidVert := m_VidVert + 20 <# 850
  SetVideoPos(m_VidHorz, m_VidVert, 12)


PRI TiltDown

  m_VidVert := m_VidVert - 20 #> 330
  SetVideoPos(m_VidHorz, m_VidVert, 12)


PRI ResetVideo

  SetPscPos(750, 690, 10)

  
PRI ScanVideo

  if(!m_AuxOn)
    AuxOn

  SetVideoPos(1170, m_VidVert, 12)
  Pause_ms(3500)
  SetVideoPos(290, m_VidVert, 12)
  Pause_ms(5500)
  SetVideoPos(750, m_VidVert, 12)

  
PRI SetVideoPos(Horz, Vert, Ramp)

  if(!m_AuxOn)
    AuxOn
  SetPscPos(Horz, Vert, Ramp)

  
' ------------------------------------------------------------------------------
' Parallax Servo Controller (PSC) Functions
' ------------------------------------------------------------------------------

PRI SetPscPos(Horz, Vert, Ramp)

  Psc.str(STRING("!SC"))
  Psc.tx(PSCC_VidHorz)
  Psc.tx(Ramp)
  Psc.tx(Horz.BYTE[0])       
  Psc.tx(Horz.BYTE[1])
  Psc.tx(13)
  m_VidHorz := Horz
  
  Psc.str(STRING("!SC"))
  Psc.tx(PSCC_VidVert)
  Psc.tx(Ramp)
  Psc.tx(Vert.BYTE[0])       
  Psc.tx(Vert.BYTE[1])
  Psc.tx(13)
  m_VidVert := Vert
  

PRI SetPscBaudRate(Baud)
  ' Select the baud rate for the PSC
  ' 0 = 2400
  ' 1 = 38400
   Psc.str(string("!SCSBR"))
   Psc.tx(Baud)
   Psc.tx($0D)
   Pause_ms(250)
     

' ------------------------------------------------------------------------------
' Aux Power Functions
' ------------------------------------------------------------------------------

PRI AuxOn

  ' Energize the aux power relay
  outa[Pin_AuxPwr]~~
  m_AuxOn := true


PRI AuxOff
  
  ' De-energize the video power relay
  outa[Pin_AuxPwr]~
  m_AuxOn := false


' ------------------------------------------------------------------------------
' Camera Shutter Functions
' ------------------------------------------------------------------------------

PRI FireShutter(delay)

  ' Toggle the shutter relay
  outa[Pin_Shutter]~~
  Pause_ms(delay)
  outa[Pin_Shutter]~
  m_ShutterCtr++
  

' ------------------------------------------------------------------------------
' Misc
' ------------------------------------------------------------------------------

PRI SetPos(row, col)
  Lcd.gotoxy(col, row)


PRI Pause_ms(msDelay)
  waitcnt(cnt + ((clkfreq / 1000) * msDelay))
      