class MotorDriver:
def __init__(self, address=0x0f):
self.motor_driver = GroveMotorDriver(address=0x0f)
#"0b1001" defines the output polarity of both motors
# The first two least significant bits "01" are for the right motor
# and the following bits "10" specify the polarity of the left motor,
# "10" means the M+ is "positive
self.motor = {
'left': {
'dir': 0b10,
'val': 0,
},
'right': {
'dir': 0b01,
'val': 0,
},
}
def left_motor_callback(self, msg):
#"0b1010" defines the output polarity, "10" means the M+ is "positive
if (msg.data > 0):
self.motor['left']['dir'] = 0b10
self.motor['left']['val'] = min(msg.data, 100)
rospy.logdebug(f"Left forward: {self.motor['left']['val']}")
elif (msg.data < 0):
self.motor['left']['dir'] = 0b01
self.motor['left']['val'] = min(-msg.data, 100)
rospy.logdebug(f"Left backward: {self.motor['left']['val']}")
else:
self.motor['left']['val'] = 0
rospy.logdebug(f"Left stop: {self.motor['left']['val']}")
self.update_motors()
def right_motor_callback(self, msg):
#"0b1010" defines the output polarity, "10" means the M+ is "positive
if (msg.data > 0):
self.motor['right']['dir'] = 0b01
self.motor['right']['val'] = min(msg.data, 100)
rospy.logdebug(f"Right forward: {self.motor['right']['val']}")
elif (msg.data < 0):
self.motor['right']['dir'] = 0b10
self.motor['right']['val'] = min(-msg.data, 100)
rospy.logdebug(f"Right backward: {self.motor['right']['val']}")
else:
self.motor['right']['val'] = 0
rospy.logdebug(f"Right stop: {self.motor['right']['val']}")
self.update_motors()
def update_motors(self):
direction = self.motor['left']['dir']<<2 | self.motor['right']['dir']
self.motor_driver.MotorDirectionSet(direction)
self.motor_driver.MotorSpeedSetAB(self.motor['right']['val'], self.motor['left']['val'])
def stop_motors(self):
self.motor_driver.MotorSpeedSetAB(0, 0)
def __init__(self, address=0x0f):
self.motor_driver = GroveMotorDriver(address=0x0f)
#"0b1001" defines the output polarity of both motors
# The first two least significant bits "01" are for the right motor
# and the following bits "10" specify the polarity of the left motor,
# "10" means the M+ is "positive
self.motor = {
'left': {
'dir': 0b10,
'val': 0,
},
'right': {
'dir': 0b01,
'val': 0,
},
}
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 NONINFRINGEMENT. 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.
'''
from grove_i2c_motor_driver import GroveMotorDriver
import time
try:
# You can initialize with a different address too: grove_i2c_motor_driver.motor_driver(address=0x0a)
m = GroveMotorDriver(address=0x0f)
#FORWARD
print("Forward")
m.MotorSpeedSetAB(100, 100) #defines the speed of motor 1 and motor 2;
m.MotorDirectionSet(
0b1010
) #"0b1010" defines the output polarity, "10" means the M+ is "positive" while the M- is "negtive"
time.sleep(2)
#BACK
print("Back")
m.MotorSpeedSetAB(100, 100)
m.MotorDirectionSet(0b0101) #0b0101 Rotating in the opposite direction
time.sleep(2)