def __init__(self, debug=False, bus_number=1, db="config"):
''' Init the direction channel and pwm channel '''
self.forward_A = True
self.forward_B = True
self.db = filedb.fileDB(db=db)
self.forward_A = int(self.db.get('forward_A', default_value=1))
self.forward_B = int(self.db.get('forward_B', default_value=1))
self.left_wheel = TB6612.Motor(self.Motor_A, offset=self.forward_A)
self.right_wheel = TB6612.Motor(self.Motor_B, offset=self.forward_B)
self.pwm = PCA9685.PWM(bus_number=bus_number)
def _set_a_pwm(value):
pulse_wide = self.pwm.map(value, 0, 100, 0, 4095)
self.pwm.write(self.PWM_A, 0, pulse_wide)
def _set_b_pwm(value):
pulse_wide = self.pwm.map(value, 0, 100, 0, 4095)
self.pwm.write(self.PWM_B, 0, pulse_wide)
self.left_wheel.pwm = _set_a_pwm
self.right_wheel.pwm = _set_b_pwm
self._speed = 0
self.debug = debug
if self._DEBUG:
print self._DEBUG_INFO, 'Set left wheel to #%d, PWM channel to %d' % (self.Motor_A, self.PWM_A)
print self._DEBUG_INFO, 'Set right wheel to #%d, PWM channel to %d' % (self.Motor_B, self.PWM_B)
def main():
import time
GPIO.setmode(GPIO.BCM)
GPIO.setup((27, 22), GPIO.OUT)
a = GPIO.PWM(27, 60)
b = GPIO.PWM(22, 60)
a.start(0)
b.start(0)
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(17)
motorB = TB6612.Motor(18)
motorA.debug = False
motorB.debug = False
motorA.pwm = a_speed
motorB.pwm = b_speed
motorA.speed = 30
motorB.speed = 30
# left
motorA.backward()
motorB.backward()
time.sleep(0.9)
motorA.stop()
motorB.stop()
def test():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.setup((12, 26), GPIO.OUT)
a = GPIO.PWM(12, 60)
b = GPIO.PWM(26, 60)
a.start(0)
b.start(0)
delay = 0.2
delay1 = 0.1
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(20)
motorB = TB6612.Motor(16)
# motorA.debug = True
# motorB.debug = True
motorA.pwm = a_speed
motorB.pwm = b_speed
inkey = _Getch()
if inkey == '\x1b[A':
print("forward")
motorA.forward()
motorA.speed = 100
motorB.forward()
motorB.speed = 100
time.sleep(delay)
elif inkey == '\x1b[B':
print("backward")
motorA.backward()
motorA.speed = 100
motorB.backward()
motorB.speed = 100
time.sleep(delay)
elif inkey == '\x1b[C':
print("right")
motorA.forward()
motorA.speed = 100
motorB.backward()
motorB.speed = 100
time.sleep(delay1)
elif inkey == '\x1b[D':
print("left")
motorA.backward()
motorA.speed = 100
motorB.forward()
motorB.speed = 100
time.sleep(delay1)
def main():
import time
#Connect MA to BCM20
#Connect MB to BCM16
#Connect PWMA to BCM12
#Connect PWMB to BCM26
GPIO.setmode(GPIO.BCM)
GPIO.setup((12, 26), GPIO.OUT)
a = GPIO.PWM(12, 60)
b = GPIO.PWM(26, 60)
a.start(0)
b.start(0)
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(20)
motorB = TB6612.Motor(16)
motorA.debug = True
motorB.debug = True
motorA.pwm = a_speed
motorB.pwm = b_speed
delay = 3
motorA.forward()
motorA.speed = 100
motorB.forward()
motorB.speed = 100
time.sleep(delay)
motorA.backward()
motorA.speed = 100
motorB.backward()
motorB.speed = 100
time.sleep(delay)
motorA.forward()
motorA.speed = 100
motorB.backward()
motorB.speed = 100
time.sleep(delay)
motorA.backward()
motorA.speed = 100
motorB.forward()
motorB.speed = 100
time.sleep(delay)
def main():
print("********************************************")
print("* *")
print("* SunFounder TB6612 *")
print("* *")
print("* Connect MA to BCM17 *")
print("* Connect MB to BCM18 *")
print("* Connect PWMA to BCM27 *")
print("* Connect PWMB to BCM12 *")
print("* *")
print("********************************************")
motorA = TB6612.Motor(17, 27)
motorB = TB6612.Motor(18, 22)
motorA.set_debug(True)
motorB.set_debug(True)
delay = 0.05
motorA.forward()
for i in range(0, 101):
motorA.set_speed(i)
time.sleep(delay)
for i in range(100, -1, -1):
motorA.set_speed(i)
time.sleep(delay)
motorA.backward()
for i in range(0, 101):
motorA.set_speed(i)
time.sleep(delay)
for i in range(100, -1, -1):
motorA.set_speed(i)
time.sleep(delay)
motorB.forward()
for i in range(0, 101):
motorB.set_speed(i)
time.sleep(delay)
for i in range(100, -1, -1):
motorB.set_speed(i)
time.sleep(delay)
motorB.backward()
for i in range(0, 101):
motorB.set_speed(i)
time.sleep(delay)
for i in range(100, -1, -1):
motorB.set_speed(i)
time.sleep(delay)
def main():
GPIO.setmode(GPIO.BCM)
GPIO.setup((12, 26), GPIO.OUT)
a = GPIO.PWM(12, 60)
b = GPIO.PWM(26, 60)
a.start(0)
b.start(0)
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(20)
motorB = TB6612.Motor(16)
motorA.debug = True
motorB.debug = True
motorA.pwm = a_speed
motorB.pwm = b_speed
def __init__(self, debug=False):
self._DEBUG = debug
rospy.init_node('Motor_Node')
rospy.loginfo(_DEBUG_INFO + " initializing node")
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
# set GPIO22 and GPIO27 (pins 13 and 15)
GPIO.setup((27, 22), GPIO.OUT)
# make the motor objects and assign the frequency.
self.a = GPIO.PWM(27, 60)
self.b = GPIO.PWM(22, 60)
self.a.start(0)
self.b.start(0)
def a_speed(value):
self.a.ChangeDutyCycle(value)
def b_speed(value):
self.b.ChangeDutyCycle(value)
self.motorA = TB6612.Motor(17)
self.motorB = TB6612.Motor(18)
self.motorA.debug = False
self.motorB.debug = False
self.motorA.pwm = a_speed
self.motorB.pwm = b_speed
self.delay = 0.05
# start the motor with a duty cycle of 0, basically it is off.
# self.startMotor(0)
# implement ROS subscribers
self.speed_sub = rospy.Subscriber('/angela/motor/setSpeed', motormsg,
self.move)
# stops the node from exiting
rospy.spin()
def main():
import time
print "********************************************"
print "* *"
print "* TEST FORWARD AND BACK *"
print "* *"
print "********************************************"
GPIO.setmode(GPIO.BCM)
GPIO.setup((27, 22), GPIO.OUT)
a = GPIO.PWM(27, 60)
b = GPIO.PWM(22, 60)
a.start(0)
b.start(0)
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(17)
motorB = TB6612.Motor(18)
motorA.debug = False
motorB.debug = False
motorA.pwm = a_speed
motorB.pwm = b_speed
motorA.speed = 29
motorB.speed = 29
# avanti
motorA.backward()
motorB.forward()
time.sleep(0.85)
motorA.stop()
motorB.stop()
def main():
import time
print "********************************************"
print "* *"
print "* SunFounder TB6612 *"
print "* *"
print "* Connect MA to BCM17 *"
print "* Connect MB to BCM18 *"
print "* Connect PWMA to BCM27 *"
print "* Connect PWMB to BCM22 *"
print "* *"
print "********************************************"
GPIO.setmode(GPIO.BCM)
GPIO.setup((27, 22), GPIO.OUT)
a = GPIO.PWM(27, 60)
b = GPIO.PWM(22, 60)
a.start(0)
b.start(0)
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(17)
motorB = TB6612.Motor(18)
motorA.debug = True
motorB.debug = True
motorA.pwm = a_speed
motorB.pwm = b_speed
delay = 0.05
motorA.forward()
for i in range(0, 101):
motorA.speed = i
time.sleep(delay)
for i in range(100, -1, -1):
motorA.speed = i
time.sleep(delay)
motorA.backward()
for i in range(0, 101):
motorA.speed = i
time.sleep(delay)
for i in range(100, -1, -1):
motorA.speed = i
time.sleep(delay)
motorB.forward()
for i in range(0, 101):
motorB.speed = i
time.sleep(delay)
for i in range(100, -1, -1):
motorB.speed = i
time.sleep(delay)
motorB.backward()
for i in range(0, 101):
motorB.speed = i
time.sleep(delay)
for i in range(100, -1, -1):
motorB.speed = i
time.sleep(delay)
print "********************************************" a = Servo.Servo(4) b = Servo.Servo(5) Servo.Servo(4).setup() Servo.Servo(5).setup() #GPIO.setmode(GPIO.BCM) #GPIO.setup((27, 22), GPIO.OUT) #a = GPIO.PWM(27, 60) #b = GPIO.PWM(22, 60) #a.start(0) #b.start(0)) def a_speed(value): a.write(value) def b_speed(value): b.write(value) motorB = TB6612.Motor(17) motorA = TB6612.Motor(18) motorA.debug = True motorB.debug = True motorA.pwm = a_speed motorB.pwm = b_speed delay = 0.05 motorA.forward() motorA.seped = 100 motorB.forward() motorB.speed = 100
def main():
import time
#Connect MA to BCM20
#Connect MB to BCM16
#Connect PWMA to BCM12
#Connect PWMB to BCM26
GPIO.setmode(GPIO.BCM)
GPIO.setup((12, 26), GPIO.OUT)
a = GPIO.PWM(12, 60)
b = GPIO.PWM(26, 60)
a.start(0)
b.start(0)
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(20)
motorB = TB6612.Motor(16)
motorA.debug = True
motorB.debug = True
motorA.pwm = a_speed
motorB.pwm = b_speed
delay = 0.1
state = 0
inkey = _Getch()
while 1:
k = inkey()
if k != '': break
# while 1:
if k == '\x1b[A':
state = 1
print(state)
elif k == '\x1b[B':
state = -1
print(state)
elif k == '\x1b[C':
state = 2
elif k == '\x1b[D':
state = 3
else:
state = 0
# break
# i = 0
while state == 1:
print("forward")
motorA.forward()
motorA.speed = 100
motorB.forward()
motorB.speed = 100
time.sleep(delay)
k1 = inkey()
if k1 != '': break
while state == -1:
print("backward")
motorA.backward()
motorA.speed = 100
motorB.backward()
motorB.speed = 100
time.sleep(delay)
k1 = inkey()
if k1 != '': break
while state == 2:
print("right")
motorA.forward()
motorA.speed = 30
motorB.backward()
motorB.speed = 30
time.sleep(delay)
k1 = inkey()
if k1 != '': break
while state == 3:
print("forward")
motorA.backward()
motorA.speed = 30
motorB.forward()
motorB.speed = 30
k1 = inkey()
if k1 != '': break
def main(): print "********************************************" print "* *" print "* TEST LEFT *" print "* *" print "********************************************" GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) GPIO.setup((27, 22), GPIO.OUT) a = GPIO.PWM(27, 60) b = GPIO.PWM(22, 60) a.start(0) b.start(0) GPIO.setup(20, GPIO.IN) stateLastA1 = GPIO.input(20) GPIO.setup(5, GPIO.IN) stateLastB1 = GPIO.input(5) stateCountA = 0 stateCountB = 0 def a_speed(value): a.ChangeDutyCycle(value) def b_speed(value): b.ChangeDutyCycle(value) motorA = TB6612.Motor(17) motorB = TB6612.Motor(18) motorA.pwm = a_speed motorB.pwm = b_speed motorA.speed = 29 motorB.speed = 28 #direction left motorA.forward() motorB.backward() while True: stateCurrentA1 = GPIO.input(20) stateCurrentB1 = GPIO.input(5) if stateCurrentA1 != stateLastA1: stateLastA1 = stateCurrentA1 stateCountA += 1 if stateCountA >= 355: motorA.stop() if stateCurrentB1 != stateLastB1: stateLastB1 = stateCurrentB1 stateCountB += 1 if stateCountB >= 355: motorB.stop() if stateCountA >= 355 and stateCountB >= 355: destroy()
def main():
# import time
#
# print "********************************************"
# print "* *"
# print "* SunFounder TB6612 *"
# print "* *"
# print "* Connect MA to BCM17 -> 20 *"
# print "* Connect MB to BCM18 -> 16 *"
# print "* Connect PWMA to BCM27 -> 12 *"
# print "* Connect PWMB to BCM22 -> 26 *"
# print "* *"
# print "********************************************"
GPIO.setmode(GPIO.BCM)
GPIO.setup((12, 26), GPIO.OUT)
a = GPIO.PWM(12, 60)
b = GPIO.PWM(26, 60)
a.start(0)
b.start(0)
def a_speed(value):
a.ChangeDutyCycle(value)
def b_speed(value):
b.ChangeDutyCycle(value)
motorA = TB6612.Motor(20)
motorB = TB6612.Motor(16)
# motorA.debug = True
# motorB.debug = True
motorA.pwm = a_speed
motorB.pwm = b_speed
inkey = _Getch()
while (1):
k = inkey()
if k != '': break
if k == '\x1b[A':
print("forward")
motorA.forward()
motorA.speed = 100
motorB.forward()
motorB.speed = 100
elif k == '\x1b[B':
print("backward")
motorA.backward()
motorA.speed = 100
motorB.backward()
motorB.speed = 100
elif k == '\x1b[C':
print("right")
motorA.backward()
motorA.speed = 100
motorB.forward()
motorB.speed = 100
elif k == '\x1b[D':
print("left")
motorA.forward()
motorA.speed = 100
motorB.backward()
motorB.speed = 100
# elif k==',':
# print("<")
# elif k=='.':
# print(">")
else:
print("not an arrow key!")
def main(): import time GPIO.setwarnings(False) GPIO.setmode(GPIO.BCM) GPIO.setup((12, 26), GPIO.OUT) a = GPIO.PWM(12, 60) b = GPIO.PWM(26, 60) a.start(0) b.start(0) def a_speed(value): a.ChangeDutyCycle(value) def b_speed(value): b.ChangeDutyCycle(value) motorA = TB6612.Motor(20) motorB = TB6612.Motor(16) # motorA.debug = True # motorB.debug = True motorA.pwm = a_speed motorB.pwm = b_speed delay = 0.05 while(1): s = get() if s == 0 #stop if nothing pressed# motorA.stop() motorB.stop() elif s == 1 #going forward# motorA.forward() motorA.speed = 100 motorB.forward() motorB.speed = 100 elif s == -1 #going backward# motorA.backward() motorA.speed = 100 motorB.backward() motorB.speed = 100 elif s == 2 #rotate right# motorA.forward() motorA.speed = 100 motorB.backward() motorB.speed = 100 elif s == 3 #rotate left# motorA.backward() motorA.speed = 100 motorB.forward() motorB.speed = 100 else: destroy()
