' JP1-1.OSC
' (revised: 11/15/06).
' This program controls JP1 RAD-hack robot speed via 
'   PC keyboard interface, using OOPic IDE comm-window
'   or terminal emulator. 
' 1. uses OOBOT40-3 controller board.
' 2. accepts keypresses from PC via serial port to 
'    control left-right motors via L298N h-bridge. 
' 3. uses serial port to report status back to PC.
  
  Const DEF_SPEED = 150      ' default motor speed.
  
  Const ch_plus = 43
  Const ch_minus = 45
  Const ch_A = 65
  Const ch_B = 66
  Const ch_C = 67
  Const ch_D = 68
  Const ch_E = 69
  Const ch_F = 70
  Const ch_H = 72
  Const ch_K = 75
  Const ch_L = 76
  Const ch_R = 82
  Const ch_S = 83
  
  Dim com1 as New oSerialPort	' use buffered RS232
  Dim key as New oByte          ' incoming keypresses
  
  Dim x as New oByte
  Dim y as New oByte
  Dim speed as New oByte
  
  Dim wtemp1 as Word
  Dim btemp2 as Byte
  
  Dim mL as New oDCMotor2
  Dim mR as New oDCMotor2
  
  Dim batt1 as New oA2D		' battery 1 voltage
  
  Dim sw1 as New oDio1
  Dim Frq As New oFreq
  
  
  ' ****************************************
  '   MAIN LOOP
  ' ****************************************
  Sub Main() 
  
    call motor_setup
    call sensor_setup
    call misc_setup
    call play1
    oopic.delay = 100
  
    mL = 0              ' motor speed = 0
    mR = 0
    Do
      ' process incoming keypresses
      if (com1.received=1) then 
         key = com1.value
         ' convert small to capital
        if (key > 97) then key = key - 32
  
        if (key=ch_S) then 
           call stop                            ' "S"
           call play1
          end if
  
        if (key=ch_F) then call forward		' "F"
        if (key=ch_A) then call accel           ' "A"       
        if (key=ch_D) then call decel           ' "D"
        if (key=ch_B) then call backup          ' "B"
  
        if (key=ch_H) then call half            ' "H"
        if (key=ch_L) then call turn_left       ' "L"
        if (key=ch_R) then call turn_right      ' "R"
         if (key=ch_K) then call brake          ' "K"
         call report
      end if
  
    Loop
  
  End Sub
  
  
  ' ****************************************
  '  left turn
  ' ****************************************
  Sub turn_left()
    ' first slowdown, then reverse direction 
    if (mL>=50) then
      mL = 0
      oopic.delay=20       ' pause 200-msec
    end if
    mL.direction = 1
    mR.direction = 0
    mL = speed
    mR = speed
  end Sub
  
  ' ****************************************
  '  right turn
  ' ****************************************
  Sub turn_right()
    ' first slowdown, then reverse direction 
    if (mR>=50) then
      mR = 0
      oopic.delay=20       ' pause 200-msec
    end if
    mL.direction = 0
    mR.direction = 1
    mL = speed
    mR = speed
  end Sub
  
  ' ****************************************
  '  go forward 
  ' ****************************************
  Sub forward()
    ' first slowdown, then reverse direction 
    if (mL >= 50) OR (mR >= 50) then
      call stop
      oopic.delay=20       ' pause 200-msec
    end if
    mL.direction = 0
    mR.direction = 0
  
    mL = (2*speed)/3       ' ramp up speed
    mR = (2*speed)/3
    oopic.delay=30
    mL = speed
    mR = speed       
  end Sub
  
  ' ****************************************
  '  back up
  ' ****************************************
  Sub backup()
    ' first slowdown, then reverse direction 
    if (mL >= 50) OR (mR >= 50) then
      call stop
      oopic.delay=20       ' pause 200-msec
    end if
    mL.direction = 1
    mR.direction = 1
  
    mL = (2*speed)/3       ' ramp up speed
    mR = (2*speed)/3
    oopic.delay=30
    mL = speed
    mR = speed
  end Sub
  
  ' ****************************************
  '  half-speed
  ' ****************************************
  Sub half()
    call set_half
    mL = speed
    mR = speed
  end Sub
  
  ' ****************************************
  '  set half-speed
  ' ****************************************
  Sub set_half()
    speed = DEF_SPEED
  end Sub
  
  ' ****************************************
  '  report state info to host
  ' ****************************************
  Sub report()
    com1 = 13              ' CR
    com1 = 10              ' LF
  
    com1.string = "mL "
    if (mL.direction = 0) then
      com1 = ch_plus           	' send "+"
    else
      com1 = ch_minus         	' send "-"
    end if
    x = mL
    call send_dec
  
    com1.string = ", mR "
    if (mR.direction = 0) then
      com1 = ch_plus		' send "+"
    else
      com1 = ch_minus         	' send "-"
    end if
    x = mR
    call send_dec
  
    ' send battery voltage
    com1.string = ", b1 "
    wtemp1 = (75 * batt1) / 128
    x = wtemp1
    call send_dec
  end Sub
  
  ' ****************************************
  '   stop 
  ' ****************************************
  Sub stop()
    mL = 0
    mR = 0       
  end Sub
  
  ' ****************************************
  '   brake 
  ' ****************************************
  Sub brake()
    mL = 0
    mR = 0       
  end Sub
  
  ' ****************************************
  '   accelerate
  ' ****************************************
  Sub accel()
    if (speed >= 225) then 
      speed = 250
    else
      speed = speed + 25
    end if
    ' change to new speed, if motors=running
    call set_newspeed
  end Sub
  
  ' ****************************************
  '   decelerate 
  ' ****************************************
  Sub decel()
    if (speed <= 30) then 
      speed = 0
    else
      speed = speed - 25
    end if 
    ' change to new speed, if motors=running
    call set_newspeed
  end Sub
  
  ' ****************************************
  '   set new motor speed, if running
  ' ****************************************
  Sub set_newspeed()
    if (mL<>0) OR (mR<>0) then
      mL = speed
      mR = speed
    end if
  end Sub
  
  ' ****************************************
  '  miscellaneous setup subroutine
  ' ****************************************
  sub misc_setup()
    ' setup pushbutton switch in RC0
    sw1.ioline = 16                     ' RC0 
    sw1.direction = cvinput
  
    ' setup RS232 port
    com1.Baud = cv9600       	' stay at 9600 bps
    com1.Mode = 0               ' asynchronous ops
    com1.Operate = 1 
  end sub
  
  ' ****************************************
  '  motor setup subroutine
  ' ****************************************
  sub sensor_setup()
    ' setup battery monitor
    batt1.IOLine = 1
    batt1.Operate = 1
  end sub
  
  ' ****************************************
  '  motor setup subroutine
  ' ****************************************
  sub motor_setup()
    ' setup left-motor object
    mL.IOLineP = 18              ' PWM1
    mL.IOLine1 = 24              ' RD0 = L298-M1
    mL.IOLine2 = 25              ' RD1 = L298-M2
    mL.Prescale = 2              ' base freq = 312 Khz
    mL.Period = 255              ' PWM = 312-khz/256 = 1220-hz
  
    mL.Unsigned = 1              ' enable .Direction flag
    mL.Direction = 0
    mL.Operate = 1
    mL.Brake = cvOff
  
    ' setup right-motor object
    mR.IOLineP = 17         	' PWM2
    mR.IOLine1 = 26             ' RD2 = L298-M3
    mR.IOLine2 = 27             ' RD3 = L298-M4
    mR.Unsigned = 1
    mR.Direction = 0
    mR.Operate = 1
    mR.Brake = cvOff
  
    call set_half              ' default motor speed
    mL = 0                     ' turn-off motors
    mR = 0
  end sub
  
  
  ' ****************************************
  '  send byte as ASCII-decimal string 
  ' ****************************************
  Sub send_dec()
    y = x / 100              	' get 100s
    ' suppress leading "0" 
    if (y = 0) then 
      com1 = 32              	' space
    else
      com1 = y + 48             ' xlat to "0"-base
    end if
  
    y = (y * 100) 
    x = x - y                	' get 10s
    y = x / 10
    com1 = y + 48
    x = x - (y * 10)       	' get 1s       
    com1 = x + 48
  end Sub
  
  ' *****************************************
  '   play1 - play tune #1
  ' *****************************************
  Sub play1()
      Frq.Beep(64000,10)
      Frq.Beep(64500,10)
      Frq.Beep(65000,10)
      Frq.Beep(63500,5)
      Frq.Beep(63600,5)
      Frq.Beep(0,10)
      Frq.Beep(5500,10)
      Frq.Beep(0,10)
      Frq.Beep(5500,10)
      Frq.Beep(0,10)
  End Sub