DebugTool.spin2

'' =================================================================================================
''
''   File....... DebugTool.spin2
''   Purpose.... Code for Kevinbot v3
''   Author..... Kevin Ahr
''               Copyright (c) 2023 Kevin Ahr
''               -- see below for terms of use
''
''   {$P2}
''
'' =================================================================================================


con { timing }

  CLK_FREQ = 200_000_000                                        ' system freq as a constant
  MS_001   = CLK_FREQ / 1_000                                   ' ticks in 1ms
  US_001   = CLK_FREQ / 1_000_000                               ' ticks in 1us

  _clkfreq = CLK_FREQ                                           ' set system clock


con { terminal }

  BR_TERM  = 230_400                                            ' terminal baud rate
  BR_RPI   = 230_400                                            ' raspberrypi / xbee baud rate

  #0, T_PST, T_ANSI                                             ' terminal types

  T_TYPE = T_PST


con { fixed io pins }

  RX1      = 63  { I }                                          ' programming / debug
  TX1      = 62  { O }

  SF_CS    = 61  { O }                                          ' serial flash
  SF_SCK   = 60  { O }
  SF_SDO   = 59  { O }
  SF_SDI   = 58  { I }

  SD_SCK   = 61  { O }                                          ' sd card
  SD_CS    = 60  { O }
  SD_SDO   = 59  { O }
  SD_SDI   = 58  { I }

  LED2     = 57  { O }                                          ' Eval and Edge LEDs
  LED1     = 56  { O }


con { app io pins }

  RPI_TX            = 6  { I }                                  ' Raspberry Pi
  RPI_RX            = 5  { O }

  CAMERA_LED        = 14  { O }                                 ' camera ring / lights
  LED               = 19  { O }                                 ' all robot leds (daisy-chained)

  SDA               = 15  { IO }                                ' i2c bus
  SCL               = 16  { IO }

  VOLT1_P           = 17  { I }                                 ' voltage sensors
  VOLT2_P           = 18  { I }

  LEFT_MOT          = 20  { O }                                 ' hb25 motor control
  RIGHT_MOT         = 21  { O }

  LEFT_ENC_A        = 22  { I }                                 ' left motor encoder
  LEFT_ENC_B        = 23  { I }

  RIGHT_ENC_A       = 24  { I }                                 ' right motor encoder
  RIGHT_ENC_B       = 25  { I }

  ONE_WIRE          = 26  { IO }                                ' one wire bus

  BUZZER_PIN        = 27  { O }                                 ' alert buzzer


con { settings }

  #true,  ON, OFF
  #false, NO, YES

  '''     Misc Settings      '''

  BUFFER_SIZE       = 48                                        ' command buffer size
  CMD_SIZE          = 48                                        ' max cmd and val name length

  HEAD_LEDS         = 8                                         ' number of leds in head
  BODY_LEDS         = 8                                         ' number of leds in body
  CAM_LEDS          = 8                                         ' number of leds in camera ring
  BASE_LEDS         = 8                                         ' number of leds in base
  TOTAL_LEDS        = HEAD_LEDS+BODY_LEDS+CAM_LEDS+BASE_LEDS    ' total leds in whole robot

  ''''''''''''''''''''''''''''''


con  { servo channels }

  US_MIN            = 500                                       ' min and max microseconds for pca
  US_MAX            = 2400


obj

' main                                                          ' * master Spin cog
  term    : "jm_fullduplexserial"                               ' * serial IO for terminal
  rpi     : "jm_fullduplexserial"                               ' * serial IO for RPi
  ansi    : "jm_ansi"                                           '   ANSI terminal control sequences

  strs    : "ka_strings"                                        '   string manipulation
  timer1  : "jm_ez_timer"                                       '   timer
  prng    : "jm_prng"                                           '   TEMPORARY RNG
  nstr    : "jm_nstr"                                           '   number-to-string

  leds    : "jm_rgbx_pixel"                                     ' * WS2812b leds

  pca     : "ka_pca9685"                                        '   pca9685 servo / pwm

  beeper  : "jm_pwm"                                            '   error buzzer pwm

  volt1   : "jm_ez_analog"                                      '   smart pin analog input for voltage meter
  volt2   : "jm_ez_analog"                                      '   smart pin analog input for voltage meter
  bme     : "bme280_I2C"

  left    : "jm_servo"                                          '   hb25 motor control
  right   : "jm_servo"                                          '   hb25 motor control
  lenc    : "jm_quadrature"                                     '   motor enc
  renc    : "jm_quadrature"                                     '   motor enc

  tsns    : "jm_ds18b20"                                        '   1-Wire temperature sensor

' * uses cog when loaded


dat

  Version       byte     "version=v0.1.0", 10, 0                ' version

'                        rr gg bb ww
  CameraColor   long    $FF_FF_FF_00                            ' color for camera ring

  ArmStartup    byte    0, 90, 45, 80, 180, 0, 0, 0, 0, 0       ' startup positions for arms
  HeadStartup   byte    80, 80                                  ' startup positions for head

  ' DS18b20 serial numbers

  SN0           byte    $28, $98, $C0, $23, $0C, $32, $20, $F0  ' left motor
  SN1           byte    $28, $BF, $08, $42, $0C, $32, $20, $C0  ' right motor
  SN2           byte    $28, $2E, $5C, $47, $0C, $32, $20, $4D  ' robot temp


dat { command tables }

  NumCmds       byte    1                                       ' number of commands in table

  Commands
                byte    "debugtool.connected" , 0               ' command table
                byte    0


var { globals }

  byte  running

  long  update_scstack[128]
  byte  upd_cog

  byte  serial_buf[BUFFER_SIZE]

  byte  cmdstr[CMD_SIZE]                                        ' command as string
  byte  valstr[CMD_SIZE]                                        ' value as string

  byte  eye_effect

  long  pixbuf[TOTAL_LEDS]                                      ' pixel buffer

  byte  error_state                                             ' current hardware error (0 for none)

  dispa_pix
  dispb_pix


pub main() | r, d, tc, tf, p, h, status, tc_left, tc_right, ta, tb, tc2, td, te

  error_state := 0
  running     := 1

  setup()

  d := bme.sensor_config(bme.CONFIG_1)

  status := tsns.reset()                                                    ' check for device
  ifnot (status == tsns.GOOD_RESET)
    case status
      tsns.BUS_SHORT : error_state := 5
      tsns.BAD_RESET : error_state := 6
      tsns.NO_DEVICE : error_state := 7

  upd_cog := cogspin(newcog, update_cog(), @update_scstack)                 ' cog

  repeat
    if running == 1
      check_serial()

    if error_state > 0
      term.fstr1(string("error=%d", 10), error_state)


pub check_serial() | index, i, v, vn, len, c

  get_msg(@serial_buf, BUFFER_SIZE)
  parse(@serial_buf, @cmdstr, @valstr)
  index := cmd_index(@cmdstr)

  if index == 1
    term.fstr1(string("error=%d", 10), error_state)
    term.fstr1(string("chip=%s", 10), string("Propeller 2"))
    term.str(string("root=P2 Kevinbot Board", 10))
    term.str(string("leds=WS1812B Leds", 10))
    fstr16(string("pins=DISP_A:%d..P%d,DISP_B:%d..P%d,RPI_RX:%d,RPI_TX:%d,CAMERA_LED:%d,LED:%d,SDA:%d,SCL:%d,VOLT1_P:%d,VOLT2_P:%d,LEFT_MOT:%d,RIGHT_MOT:%d,LEFT_ENC_A:%d,LEFT_ENC_B:%d,RIGHT_ENC_A:%d,RIGHT_ENC_B:%d,ONE_WIRE:%d,BUZZER_PIN:%d", 10), ...
                  RPI_RX, RPI_TX, CAMERA_LED, LED, SDA, SCL, VOLT1_P, VOLT2_P, LEFT_MOT, RIGHT_MOT, LEFT_ENC_A, LEFT_ENC_B, RIGHT_ENC_A, RIGHT_ENC_B, ONE_WIRE, BUZZER_PIN)
    term.fstr5(string("led_config=Total:%d=Head:%d, Body:%d, Base:%d, Cam:%d", 10), TOTAL_LEDS, HEAD_LEDS, BODY_LEDS, BASE_LEDS, CAM_LEDS)

    tone(50, 1000, 50)
    tone(60, 750, 100)
    tone(50, 1000, 50)
    tone(60, 750, 100)


pub tone(amp, freq, length)

  beeper.start(BUZZER_PIN, amp, freq, beeper.M_TRI)
  waitms(length)
  beeper.write(0)


pub update_cog()

  waitms(500)

  repeat
    if timer1.millis() >= 1000
      if error_state > 0
        term.fstr1(string("error=%d", 10), error_state)


pub parse(p_src, p_cstr, p_vstr) : result | len, sep, x, c

  bytefill(@cmdstr, 0, CMD_SIZE)                                ' reset buffers
  bytefill(@valstr, 0, CMD_SIZE)

  len := strsize(p_src)                                         ' get length of input
  sep := -1                                                     ' separator not found

  repeat x from 0 to len
    c := byte[p_src][x]
    if (c == "=")
      sep := x                                                  ' mark separator
    elseif (c == $0D)
      byte[p_src][x] := 0
      quit

  if (len > 0)
    if (sep >= 0)
      bytemove(p_cstr, p_src, sep)
      byte[p_cstr][sep] := 0
      bytemove(p_vstr, p_src+sep+1, strsize(p_src+sep+1))
      result := 2
    else
      bytemove(p_cstr, p_src, len)
      byte[p_cstr][len] := 0
      result := 1


pub str2dec(p_src) : result | df, sign, c

  df   := false
  sign := 1

  repeat
    c := byte[p_src++]
    case c
      " ", "=" :
        if (df)
          quit

      "-" :
        if (df)
          quit
        else
          sign := -1
          df := true

      "0".."9" :
        result := result * 10 + (c - "0")
        df := true

      0, $0D :                                                  ' end of string
        quit

  result *= sign


pub cmd_index(p_src) : idx | byte buf[CMD_SIZE], x, c, p_list

'' Convert command string into value
'' -- not case sensitive
'' -- returns 1..n if found in command table
'' -- returns 0 (false) if not found

  bytemove(@buf, p_src, strsize(p_src)+1)                       ' copy string + terminator

  x := 0
  repeat
    c := buf[x]
    if (c == 0)                                                 ' end of string
      quit
    elseif ((c >= "A") && (c <= "Z"))                           ' if uppercase
      buf[x] += 32                                              '  make lowercase
    x++

  p_list := @Commands                                           ' point to commands list

  repeat NumCmds
    if (strcomp(@buf, p_list))
      return idx+1
    else
      idx += 1                                                  ' update index
      p_list += strsize(p_list) + 1                             ' update list pointer

  return 0


pub get_msg(p_dest, maxlen) : len | k

  bytefill(p_dest, 0, maxlen)                                   ' clear buffer

  repeat
    k := term.rx()
    case k
      32..126 :
        if (len < maxlen-1)
          byte[p_dest][len++] := k

      term.BKSP :
        if (len > 0)
          byte[p_dest][--len] := 0

      term.CR :
        byte[p_dest][len] := 0                                  ' terminate string
        term.tx(13)
        return


pub fstr16(p_str, arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10, ...
           arg11, arg12, arg13, arg14, arg15, arg16)

'' Emit string with formatting characters and six arguments.

  term.format(p_str, @arg1)


pub setup() | r, ch
'' Configure IO and objects for application

  term.tstart(BR_TERM)                                                      ' start terminal io

  rpi.start(RPI_RX, RPI_TX, %0000, BR_RPI)                                    ' start serial for rpi

  leds.start_2812b(@pixbuf, TOTAL_LEDS, LED, 5)

  volt1.start(VOLT1_P, 0, 1000)                                               ' configure analog input for voltage
  volt2.start(VOLT2_P, 0, 1000)                                               ' configure analog input for voltage

  left.startx(LEFT_MOT, 1500)
  right.startx(RIGHT_MOT, 1500)

  lenc.start(LEFT_ENC_A, LEFT_ENC_B, -1, lenc.DETENT, 0, -1000, 1000)
  renc.start(RIGHT_ENC_A, RIGHT_ENC_B, -1, renc.DETENT, 0, -1000, 1000)

  tsns.start(ONE_WIRE, tsns.PU_1K5)                                            ' connect to 1W bus with internal pullups

  if r := bme.setup(SCL, SDA, 100, pca.PU_1K5, bme.BME280_I2C_ADDR)           ' setup bme280
    error_state := 3

  pca.start(SCL, SDA, 100, pca.PU_1K5)


pub wait_for_terminal(clear)

'' Wait for terminal to be open and key pressed

  term.rxflush()
  term.rx()
  if (clear)
    clear_screen()


pub clear_screen()

'' Move cursor home (upper left) and clear the terminal

  if (T_TYPE == T_PST)
    term.tx(term.HOME)
    term.tx(term.CLS)
  else
    term.str(ansi.hide_cursor())
    term.str(ansi.home())
    term.str(ansi.cls())


con { license }

{{

  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 NON-INFRINGEMENT. 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.

}}