' MOGOTUT2.BAS - BASICX version of Simple Motor Control
' USES SERIALPORT.BAS (a file included with the BasicX compiler files)
'      	You can eliminate the SerialPort.bas file if you wish, although
'  		you would lose the window updates. If you eliminate it, then you
'		will need to comment out (or remove) all debug window lines which
'		are marked with a 'DB
'
' The following program uses a lot of the PWM example available
' from the BasicX (www.basicx.com) website. It was modified to accept 
' feedback and it only controls one channel.  BE SURE TO REFER TO THE
' BASICX MANUAL, EXAMPLES, AND APP NOTES ON THEIR WEB SITE
'
' This program does not try to use code efficiently, merely to get the
' job done. Global variables are used to prevent
' confusion as to what is being used inside the methods.
'
'	This program is not for commercial use, is not a complete example of 
'    a motor controller and is presented here for learning purposes only.
'
'	Contact me at: dcreagan@scholars.bellevue.edu
'
'
	' *********************************************************************** '
	' *				             						 * '
	' *	Delarations and Port Setup:						 * '
	' *			locked anti-phase version 					 * '
	' *	This program expects a DC motor hooked up in locked anti phase  	 * '
	' *	configuration using an H bridge. There must be encoder feedback 	 * '
	' *	to PA7 (pin 13 on BX-24). PWM from the hardware PWM and is on 	 * '
	' *	Pin P26 (which is a pain since it doesn't have a factory soldered  * '
	' *	connection). This shows proportional gain - integral and derivative* '
	' *	are not addressed here (see main html for information on		 * '
	' *	these).								  	 * '
	' *											 * '
	' *	This is 8 bit anti-phase. The duty cycle can vary between		 * '
	' * 	0 and 255. That means a duty cycle of 128				 * '
	' *	turns the motors on for rotation in each direction exactly		 * '
	' *	half the time. To get a motor to rotate, you move the duty 	 	 * '
	' * 	cycle of the pulse train to either side of 128.	To get a 	 * '
	' * 	motor to stop, set the duty cycle to 128. It will then be		 * '
	' *	locked by opposing forces to turn. This technique discourages	 * '
	' *	the motor from free spinning after a stop is requested (which	 * '
	' *	is what you want for a robot.)						 * '
	' *											 * '
	' *********************************************************************** '




'-------------------------------------------------------------------------------

'
' BASICX is a multitasking system and I haven't used it that much, so when I 
' first started to program this, I didn't take advantage of the feature. My 
' single task programming style would probably have worked, but a much 
' more elegant way was to make use of the multi-tasking feature of BasicX 
' (in this case, BX-24)
'
' This program does that. Look at the separate stack, and call to the 
' task called 'WatchPin13'. These implement a constant watch of the
' pin that is reading the output of the motor encoder. 
'

Option Explicit


' BX-01 definitions.
'>>Public Const PinOC1A As Byte = 15
'>>Public Const PinOC1B As Byte = 29

' BX-24 definitions.
Public Const PinOC1A As Byte = 26  ' Also green LED (and the pin we will use to 
						 ' send PWM to the H-bridge)
Public Const PinOC1B As Byte = 27  ' not used, left in for clue as to what
						 ' pin to use for other PWM channel

Public Const DesiredSpeed As Integer = 20 'This is the desired speed of the motor
						     'and is compared to continually sampled
						     'encoder feedback

Public Const Pin13 As Byte = 13	' Encoder input pin

Public Const PGain as Single = 1.0     ' Proportional Gain of Loop (set to 1)

Dim ErrorC as Single		' Error value that is converted to a Duty Cycle

Dim DirLft as Byte 		' going backwards (0) or forwards (1)

Dim DutyCycle as Single

Dim LCurSpeed as Integer	' Current Speed as measured by encoder clicks

Private WatchPin13Stack(1 To 40) As Byte  ' stack for mult-tasked method

'-------------------------------------------------------------------------------
Sub Main()

' This program loops and counts the encoder pulse input through pin
' PA7 (pin 13). Depending on the count, the duty cycle is raised or
' lowered to get the motor to speed up or slow down
'
' Warning -- the Com2 serial port should not be used in this program because
' of conflicts with Timer1.

    Dim Tx As String*3			' DB line (eliminate if not using SerialPort.bas)
    Call PutPin(Pin13,bxInputPullup)	' set Pin13 to input and pull up

    ' If the BasicX Developer Board is used, be sure to set the serial port
    ' jumpers to Com1.

    Call OpenSerialPort(1, 19200)	' DB line (eliminate if not using SerialPort.bas)
						' DB - eliminate the following line if not using SerialPort.bas
    Tx = " DC"		' labels the dutycycle number on the output window
    DutyCycle=128.0		' init DutyCycle to 128 (motor is locked)
	DirLft=1			' going forwards=1 backwards=0
    Call InitializePWM(1) ' set PWM frequency to about 14 kHz

    LCurSpeed=0

    CallTask "WatchPin13", WatchPin13Stack  ' start watching the encoder output

    Do

    	' delay 1/2 second and then send value of LCurSpeed to
	' the processing subroutine called AdjLeft. LCurSpeed value is 
	' collected by the routine called WatchPin13
	'
	' The longer the delay, the more encoder clicks will be collected
	' Try moving the delay down to 1/4 second or so - the current
	' settings will drive the motor fairly slowly. You can infer from
	' all this that changing either loop time (the Delay) or the 
	' DesiredSpeed will affect the speed of the motor. Shorter delay
	' equates to faster actual wheel speed, higher DesiredSpeed equates
	' to faster actual wheel speed.

    	' Also, changing the delay will affect gain so always test your setup

	 	Call Delay(0.5)		    ' pause while the WatchPin13 sub collects data
        	Call AdjLeft(LCurSpeed)       ' adjust left speed of motor (you only have one motor
					           ' motor, name was used in case you want to add more)
  

    ' Display the duty cycle value.
	' Next two lines are DB lines (eliminate if not using SerialPort.bas)

	    	Call PutI(CInt(DutyCycle))
    		Call PutLine(Tx)
	   
    Loop

End Sub


Public Sub WatchPin13()	' Watches pin 13 and increments LCurSpeed counter when
					' Pin 13 changes. The counter is continually reset in the
					' AdjLeft subroutine

  	Dim x as byte

     x=GetPin(Pin13)

	Do
     	if GetPin(Pin13)<> x then 
      		LCurSpeed=LCurSpeed+1	 ' encoder output changed, so tally it
     	 	x=GetPin(Pin13)			 ' set x to current encoder output
		End if  
        Delay(0.0)		' give up time for other tasks
   Loop

End Sub


'-------------------------------------------------------------------------------

Public Sub AdjLeft(ByVal CurrentSpeed as Integer)

    ' 
    ' Proportional control area. Proportional control is derived from
    ' the difference between requested speed of the drive and actual
    ' speed. This is a very effective method and the use of proportional 
    ' control alone is probably sufficient for what we want to do.
    ' One disadvantage is that, under load, the system will settle into
    ' a slight offset from requested speed (especially if we are
    ' using fractional gains).  A later project could be to add in 
    ' Integral Feedback to stop the offset issue.
	'
	' Because BasicX has built in floating point and negative numbers, 
	' this whole area becomes much simpler to implement than with PicBasic
	' Pro. A real joy!
	'

    LCurSpeed=0		           ' reset current speed counter

    ErrorC=CSng(DesiredSpeed-CurrentSpeed)   ' this is the fundamental feedback calc (easy, eh?)

    ErrorC=ErrorC*PGain

	if dirLft=1 then			' we are going direction 1 (fwd, say)
		if (DutyCycle+ErrorC) > 255.0 then
			DutyCycle=255.0
              Call PutPinPWM(PinOC1A)
			Exit Sub
		End if

		if (DutyCycle+ErrorC) < 128.0 then
			DutyCycle=128.0
			Call PutPinPWM(PinOC1A)
			Exit Sub
		End if

		DutyCycle=DutyCycle+ErrorC
		Call PutPinPWM(PinOC1A)
		Exit Sub
	End if

'
'	if to here, then DirLft=0 and we are going the opposite direction
'

		if (DutyCycle-ErrorC) > 128.0 then
			DutyCycle=128.0
              Call PutPinPWM(PinOC1A)
			Exit Sub
		End if

		if DutyCycle-ErrorC < 0.0 then
			DutyCycle=0.0
			Call PutPinPWM(PinOC1A)
			Exit Sub
		End if

		DutyCycle=DutyCycle-ErrorC
		Call PutPinPWM(PinOC1A)

End Sub

'-------------------------------------------------------------------------------
Public Sub InitializePWM( _
    ByVal RateSetting As Byte)

    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)

' This procedure starts a PWM pulse train on the specified pin number. 

    Dim iDutyCycle As Byte

    iDutyCycle = FixB(DutyCycle) 

    ' 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
'-------------------------------------------------------------------------------