'DriveMotor---------------------------------------------------------------------
'
' This module contains the functions for the differential drive motors
' and encoders.
'
'
'-------------------------------------------------------------------------------
PUBLIC Const PinOC1A As Byte = 26  ' Also green LED
PUBLIC Const PinOC1B As Byte = 27
PUBLIC RMotorSpeed as INTEGER
PUBLIC LMotorSpeed as INTEGER
PUBLIC REncoderCount AS INTEGER
PUBLIC LEncoderCount AS INTEGER
PUBLIC Direction as BYTE
Public Sub InitializePWM( _
    ByVal RateSetting As Byte)
    '=====================================
    ' Parm: RateSetting
    ' TCCR1B Tick      8-bit PWM
    ' Value  Freq      Pulse Rate
    '------- --------- -----------
    ' 1      7,372,800 14,546
    ' 2        921,600  1,807
    ' 3        115,200    225.9
    ' 4         28,800     56.47
    ' 5          7,200     14.12
    '=====================================

    Const PWMmode8bit  As Byte = bx0000_0001
    Const PWMmode9bit  As Byte = bx0000_0010
    Const PWMmode10bit As Byte = bx0000_0011
    Const PWMmodeOff   As Byte = bx0000_0000

    Const MaskOC1A As Byte = bx1000_0000
    Const MaskOC1B As Byte = bx0010_0000

    ' Turn off Timer1.
    Register.TCCR1B = 0

    ' Set Timer1 to 8-bit PWM mode.
    Register.TCCR1A = PWMmode8bit

    ' Initialize pin directions.
    Call PutPin(PinOC1A, bxOutputLow)
    Call PutPin(PinOC1B, bxOutputLow)

    ' Clear duty cycles on both OCR1A and OCR1B pins.
    Register.OCR1AH = 0
    Register.OCR1AL = 0

    Register.OCR1BH = 0
    Register.OCR1BL = 0

    ' Start Timer1 according to the specified rate setting.
    Register.TCCR1B = RateSetting

    ' Enable PWM for both pins.
    Register.TCCR1A = Register.TCCR1A Or MaskOC1A
    Register.TCCR1A = Register.TCCR1A Or MaskOC1B

End Sub
'-------------------------------------------------------------------------------
Public Sub PutPinPWM( _
    ByVal PinNumber As Byte, _
    ByVal DutyCycle As Single)

' This procedure starts a PWM pulse train on the specified pin number. The
' nondimensional DutyCycle should be in range 0.0 to 1.0.

    Dim iDutyCycle As Byte

    ' Scale and enforce range constraints.
    If (DutyCycle < 0.0) Then
        iDutyCycle = 0
    ElseIf (DutyCycle > 1.0) Then
        iDutyCycle = 255
    Else
        iDutyCycle = FixB((DutyCycle * 255.0) + 0.5) ' Round off.
    End If

    ' Set the proper pin.
    If (PinNumber = PinOC1A) Then
        Register.OCR1AH = 0
        Register.OCR1AL = iDutyCycle
    ElseIf (PinNumber = PinOC1B) Then
        Register.OCR1BH = 0
        Register.OCR1BL = iDutyCycle
    End If

End Sub
'-------------------------------------------------------------------------------
SUB SetDirection()
   IF (Direction AND Left = Left) THEN
      CALL PutPin(LMotorDirPinNum,1)
   ELSE
      Call PutPin(LMotorDirPinNum,0)
   END IF
   IF (Direction AND Right = Right) THEN
      Call PutPin(RMotorDirPinNum,1)
   ELSE
      Call PutPin(RMotorDirPinNum,0)
   END IF
END SUB

SUB SetMotors(ByVal pLeftSpd as Integer,ByVal pRightSpd As Integer,ByVal pDir as byte)
   LMotorSpeed = pLeftSpd
   RMotorSpeed = pRightSpd
   Direction = pDir
   Call SetDirection
   Call PutPinPWM(LMotorSpdPinNum, csng(pLeftSpd)/100.0)
   Call PutPinPWM(RMotorSpdPinNum, csng(pRightSpd)/100.0)
END SUB