'************************************************
' BaxicX-24 Code for MD05 Motor Controller
' Some of this code may have been started by other.. my thanks
' Requires I2C.bas which is a module for I2C coms
' Not a lot of comment here should explain itself :)
' Suggestions, Improvements, Questions to damon@homepagesetc.com
' I have had a few issues with the polling of status and sending commands
' causing some instability.  Let me know if you see the prob or fix it


Option Explicit
Const MD05BaseAddress As Byte = &hB0
Const MD05CommandReg As Byte = 0
Const MD05StatusReg As Byte = 1
Const MD05SpeedReg As Byte = 2
Const MD05AccelerationReg As Byte = 3

Public Const MD05Stop As Byte = 0
Public Const MD05Forward As Byte = 1
Public Const MD05Reverse As Byte = 2


Sub MD05_Forward(ByVal Device As Byte)
	Call MD05_WriteRegister(Device, MD05CommandReg , MD05Forward )
End Sub

Sub MD05_Reverse(ByVal Device As Byte)
	Call MD05_WriteRegister(Device, MD05CommandReg , MD05Forward )
End Sub

Sub MD05_Stop(ByVal Device As Byte)
	Call MD05_SetSpeed(Device, MD05Stop )
	Call MD05_Forward(Device)
End Sub

Sub MD05_eStop(ByVal Device As Byte)
	Call MD05_WriteRegister(Device, MD05CommandReg , 0)
End Sub

Sub MD05_SetSpeed(ByVal Device As Byte, ByVal Speed As Byte)
	Call MD05_WriteRegister(Device, MD05SpeedReg, Speed)
End Sub

Sub MD05_ChangeSpeed(ByVal Device As Byte, _
					ByVal Speed As Byte, ByVal Dir As Byte)
	Call MD05_WriteRegister(Device, MD05SpeedReg, Speed)
	Call MD05_WriteRegister(Device, MD05CommandReg , Dir)
End Sub

Sub MD05_SetAcceleration(ByVal Device As Byte, ByVal Accel As Byte)
	Call MD05_WriteRegister(Device, MD05AccelerationReg, Accel)
End Sub

private Sub MD05_WriteRegister(ByVal Device As Byte, _
   ByVal Reg As Byte, ByVal Val As Byte)
	Call I2cByteWrite(MD05AddressByte(Device),Reg,Val)
	Delay (0.050)                  'Writing to EEprom, 50mS delay
End Sub

Public Function GetMD05Status(ByVal Device As Byte) As Byte 
	GetMD05Status = I2CByteRead(MD05AddressByte(Device), MD05StatusReg)
End Function

Public Function GetMD05StatusString(ByVal Device As Byte) As String 
	Dim s as String
	Dim stat as Byte
	s = ""
	If CBool(GetMD05Status(Device) AND bx00000001)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         Then
		s = "Accelerating "
	End If
	If CBool(GetMD05Status(Device) And  bx00000010) Then
		s = s & "OverCurrent "
	End If
	If CBool(GetMD05Status(Device) And bx00000100) Then
		s = s & "OverTemp "
	End If
	If CBool(GetMD05Status(Device) And bx10000000) Then
		s = "Busy "
	End If
	GetMD05StatusString = s
End Function

Public Function GetMD05Speed(ByVal Device As Byte) As Byte 
	GetMD05Speed = I2CByteRead(MD05AddressByte(Device), MD05SpeedReg)
End Function

Public Function GetMD05Dir(ByVal Device As Byte) As Byte 
	GetMD05Dir = I2CByteRead(MD05AddressByte(Device), MD05CommandReg)
End Function

Public Function GetMD05Acceleration(ByVal Device As Byte) As Byte 
	GetMD05Acceleration = I2CByteRead(MD05AddressByte(Device), MD05AccelerationReg)
End Function

public Function MD05AddressByte(ByVal DeviceNumber As Byte) As Byte
	MD05AddressByte = MD05BaseAddress + 2 * DeviceNumber
End Function