def motor_pwm_set(motor, freq, duty_cycle):
# Check if motor is forward or rear
print "motor pwm set", motor, " ", freq, " ", duty_cycle
if motor == FWL_MOTOR:
global pwm1
motor_enable(FWL_MOTOR, True)
pwm1 = GPIO.PWM(fwl_port_info[LEFT][ENABLE], freq)
pwm1.start(0)
# pwm1.ChangeDutyCycle(duty_cycle)
elif motor == FWR_MOTOR:
global pwm2
motor_enable(FWR_MOTOR, True)
pwm2 = GPIO.PWM(fwl_port_info[RIGHT][ENABLE], freq)
pwm2.start(0)
# pwm2.ChangeDutyCycle(duty_cycle)
elif motor == BWL_MOTOR:
global pwm3
motor_enable(BWL_MOTOR, True)
pwm3 = GPIO.PWM(rwl_port_info[LEFT][ENABLE], freq)
pwm3.start(0)
# pwm3.ChangeDutyCycle(duty_cycle)
elif motor == BWR_MOTOR:
global pwm4
motor_enable(BWR_MOTOR, True)
pwm4 = GPIO.PWM(rwl_port_info[RIGHT][ENABLE], freq)
pwm4.start(0)
# pwm4.ChangeDutyCycle(duty_cycle)
else:
print "Undefined Motors"
exit(1)
def servo_init():
global p
GPIO_NO = 6
GPIO.setmode(GPIO.BCM)
GPIO.setup(GPIO_NO,GPIO.OUT)
p = GPIO.PWM(GPIO_NO,50) #sets pin 21 to PWM and sends 50 signals per second
p.start(7.5) #starts by sending a pulse at 7.5% to center the servo
def wait_BCM_initalized(self):
if False == RMS.bcm_initialized_from_other:
print("BCM has not initialized yet")
time.sleep(1)
return
#GPIO.setwarnings(False)
#GPIO.setmode(GPIO.BCM)
#GPIO.cleanup()
print "Distance Measurement In Progress"
GPIO.setup(self.TRIG,GPIO.OUT)
GPIO.setup(self.ECHO,GPIO.IN)
def measure_location_speed(self):
self.wait_BCM_initalized()
try:
GPIO.output(self.TRIG, False)
#print "Waiting For Sensor To Settle"
time.sleep(0.5)
GPIO.output(self.TRIG, True)
time.sleep(0.00001)
GPIO.output(self.TRIG, False)
while GPIO.input(self.ECHO)==0:
pulse_start = time.time()
while GPIO.input(self.ECHO)==1:
pulse_end = time.time()
pulse_duration = pulse_end - pulse_start
# v = d /t if v is light speed and t is 2t, then
distance = pulse_duration * 17150
distance = round(distance, 2)
print "Distance:", distance, "cm"
speed = math.fabs(self.previous_distance - distance) / (pulse_duration * 1000)
self.previous_distance = distance
print("Speed = {} cm/second(s)".format(speed))
msg = {}
msg["speed"] = speed
self.mqtt.publish("AWS_IOT/SPEED",msg)
if distance < 35 and RMS.get_motor_state() == RMS.FORWARD_RUNNING:
print("Executing stopping motor with object {} cm".format(distance))
RMS.move_stop()
RMS.update_motor_state(RMS.COLLISON_DETECT)
except Exception as e:
print("Exception happens {}".format(e))
def test():
try:
while True: #starts an infinite loop
print '-90'
p.ChangeDutyCycle(4.5) #sends a 4.5% pulse to turn to -90
time.sleep(3) #continues for a half a second
print '+90'
p.ChangeDutyCycle(10.5) #sends a 10.5% pulse to turn to +90
time.sleep(3) #continues for a half a second
print 'center'
p.ChangeDutyCycle(7.5) #sends a 7.5% pulse to center the servo again
time.sleep(3) #continues for a half a second
except KeyboardInterrupt:
p.stop()
GPIO.cleanup()
def servo_test():
servo_init()
move_to_center()
time.sleep(1)
move_to_bottom()
try:
while True:
cmd = raw_input("command, u- up, d-down s-start o-stop:")
if cmd[0] == "u":
move_up(0.5)
elif cmd[0] == "d":
move_down(0.5)
elif cmd[0] == "o":
stop()
elif cmd[0] == "s":
p = GPIO.PWM(GPIO_NO, 50)
start()
except KeyboardInterrupt:
p.stop()
GPIO.cleanup()
def motor_enable(motor, enable):
# Check if motor is forward or rear
if True == enable or enable == False:
print "Motor :", motor, "is to set to ", enable
else:
print "Undefined enable type"
exit(1)
if motor == FWR_MOTOR:
GPIO.output(fwl_port_info[LEFT][ENABLE], enable)
elif motor == FWL_MOTOR:
GPIO.output(fwl_port_info[RIGHT][ENABLE], enable)
elif motor == BWL_MOTOR:
GPIO.output(rwl_port_info[LEFT][ENABLE], enable)
elif motor == BWR_MOTOR:
GPIO.output(rwl_port_info[RIGHT][ENABLE], enable)
else:
print "Undefined Motors"
exit(1)
def motor_enable(motor, enable):
# Check if motor is forward or rear
if True == enable or enable == False:
print "Motor :", motor, "is to set to ", enable
else:
print "Undefined enable type"
exit(1)
if motor == FWL_MOTOR:
GPIO.output(fwl_port_info[LEFT][ENABLE], enable)
elif motor == FWR_MOTOR:
GPIO.output(fwl_port_info[RIGHT][ENABLE], enable)
elif motor == BWL_MOTOR:
GPIO.output(rwl_port_info[LEFT][ENABLE], enable)
elif motor == BWR_MOTOR:
GPIO.output(rwl_port_info[RIGHT][ENABLE], enable)
else:
print "Undefined Motors"
exit(1)
def motor_setup(motor, direction):
if direction == FORWARD:
input1 = True
input2 = False
elif direction == BACKWARD:
input1 = False
input2 = True
else:
print "Undefined Direction"
exit(1)
# Check if motor is forward or rear
if motor == FWL_MOTOR:
GPIO.output(fwl_port_info[LEFT][INPUT1], input1)
GPIO.output(fwl_port_info[LEFT][INPUT2], input2)
elif motor == FWR_MOTOR:
GPIO.output(fwl_port_info[RIGHT][INPUT1], input1)
GPIO.output(fwl_port_info[RIGHT][INPUT2], input2)
elif motor == BWL_MOTOR:
GPIO.output(rwl_port_info[LEFT][INPUT1], input1)
GPIO.output(rwl_port_info[LEFT][INPUT2], input2)
elif motor == BWR_MOTOR:
GPIO.output(rwl_port_info[RIGHT][INPUT1], input1)
GPIO.output(rwl_port_info[RIGHT][INPUT2], input2)
else:
print "Undefined Motors"
exit(1)
def port_init():
# initialize GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.cleanup()
# front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
# pwm = GPIO.PWM(M1A_ENABLE_GPIO, 500)
# pwm.start(0)
# front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
# rear - left
GPIO.setup(M3A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT2_GPIO, GPIO.OUT)
# rear - right
GPIO.setup(M4A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT2_GPIO, GPIO.OUT)
def motor_gpio_init():
# initialize GPIO
# front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
# pwm = GPIO.PWM(M1A_ENABLE_GPIO, 500)
# pwm.start(0)
# front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
# rear - left
GPIO.setup(M3A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT2_GPIO, GPIO.OUT)
# rear - right
GPIO.setup(M4A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT2_GPIO, GPIO.OUT)
def port_init():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.cleanup()
global bcm_initialized_from_other
bcm_initialized_from_other = True
def test_left_wheel():
M1A_ENABLE_GPIO = 14
M1A_INPUT1_GPIO = 23
M1A_INPUT2_GPIO = 24
# initialize GPIO
#front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
GPIO.output(M1A_INPUT1_GPIO, True)
GPIO.output(M1A_INPUT2_GPIO, False)
GPIO.output(M1A_ENABLE_GPIO, True)
time.sleep(1)
GPIO.output(M1A_ENABLE_GPIO, False)
GPIO.output(M1A_INPUT1_GPIO, False)
GPIO.output(M1A_INPUT2_GPIO, True)
GPIO.output(M1A_ENABLE_GPIO, True)
time.sleep(2)
# stop
GPIO.output(M1A_INPUT1_GPIO, False)
GPIO.output(M1A_INPUT2_GPIO, False)
def port_init():
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.cleanup()
def test_pwm(speed):
#define GPIO for controlling
#front
# front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
# front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
GPIO.output(M1A_ENABLE_GPIO,True)
pwm1 = GPIO.PWM(M1A_ENABLE_GPIO, 10) # freq 10Hz
pwm1.start(0) # start with zero duty cycle
GPIO.output(M1A_ENABLE_GPIO, True)
pwm2 = GPIO.PWM(M2A_ENABLE_GPIO, 10) # duty cycle 0 - 100 %
pwm2.start(0)
for x in range(1, 10):
print "duty cycle =", x * 10, "%"
pwm1.ChangeDutyCycle(x * 10)
pwm2.ChangeDutyCycle(x * 10)
time.sleep(1)
def port_init():
# initialize GPIO
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)
GPIO.cleanup()
def motor_gpio_init():
# initialize GPIO
# front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
# pwm = GPIO.PWM(M1A_ENABLE_GPIO, 500)
# pwm.start(0)
# front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
# rear - left
GPIO.setup(M3A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT2_GPIO, GPIO.OUT)
# rear - right
GPIO.setup(M4A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT2_GPIO, GPIO.OUT)
def counter_clockwise():
GPIO.output(M1A_INPUT1_GPIO, False)
GPIO.output(M1A_INPUT2_GPIO, True)
def motor_setup(motor, direction):
if direction == FORWARD:
input1 = True
input2 = False
elif direction == BACKWARD:
input1 = False
input2 = True
else:
print "Undefined Direction"
exit(1)
# Check if motor is forward or rear
if motor == FWL_MOTOR:
GPIO.output(fwl_port_info[LEFT][INPUT1], input1)
GPIO.output(fwl_port_info[LEFT][INPUT2], input2)
elif motor == FWR_MOTOR:
GPIO.output(fwl_port_info[RIGHT][INPUT1], input1)
GPIO.output(fwl_port_info[RIGHT][INPUT2], input2)
elif motor == BWL_MOTOR:
GPIO.output(rwl_port_info[LEFT][INPUT1], input1)
GPIO.output(rwl_port_info[LEFT][INPUT2], input2)
elif motor == BWR_MOTOR:
GPIO.output(rwl_port_info[RIGHT][INPUT1], input1)
GPIO.output(rwl_port_info[RIGHT][INPUT2], input2)
else:
print "Undefined Motors"
exit(1)
def test_wheel():
M1A_ENABLE_GPIO = 14
M1A_INPUT1_GPIO = 23
M1A_INPUT2_GPIO =24
GPIO.setmode(10)
GPIO.setmode(GPIO.BCM)
#front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
GPIO.output(M1A_INPUT1_GPIO, True)
GPIO.output(M1A_INPUT2_GPIO, False)
time.sleep(2)
# stop
GPIO.output(M1A_INPUT1_GPIO, False)
GPIO.output(M1A_INPUT2_GPIO, False)
def test_pwm():
#define GPIO for controlling
#front
M1A_ENABLE_GPIO = 18
M1A_INPUT1_GPIO = 23
M1A_INPUT2_GPIO =24
M2A_ENABLE_GPIO = 18
M2A_INPUT1_GPIO = 23
M2A_INPUT2_GPIO =24
#rear
M1A_ENABLE_GPIO = 18
M1A_INPUT1_GPIO = 23
M1A_INPUT2_GPIO =24
GPIO.setmode(10)
GPIO.setmode(GPIO.BCM)
#front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
pwm = GPIO.PWM(M1A_ENABLE_GPIO, 500)
pwm.start(0)
#front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
pwm = GPIO.PWM(M2A_ENABLE_GPIO, 500)
pwm.start(0)
def rear_test_left_wheel(dir, enable):
M2A_ENABLE_GPIO = 14
M2A_INPUT1_GPIO = 23
M2A_INPUT2_GPIO = 24
#front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
if dir == FORWARD:
GPIO.output(M2A_INPUT1_GPIO, True)
GPIO.output(M2A_INPUT2_GPIO, False)
else:
GPIO.output(M2A_INPUT1_GPIO, False)
GPIO.output(M2A_INPUT2_GPIO, True)
if enable == True:
GPIO.output(M2A_ENABLE_GPIO, True)
else:
GPIO.output(M2A_ENABLE_GPIO, False)
def front_test_right_wheel(dir, enable):
M2A_ENABLE_GPIO = 4
M2A_INPUT1_GPIO = 11
M2A_INPUT2_GPIO = 10
#front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
if dir == FORWARD:
GPIO.output(M2A_INPUT1_GPIO, True)
GPIO.output(M2A_INPUT2_GPIO, False)
else:
GPIO.output(M2A_INPUT1_GPIO, False)
GPIO.output(M2A_INPUT2_GPIO, True)
if enable == True:
GPIO.output(M2A_ENABLE_GPIO, True)
else:
GPIO.output(M2A_ENABLE_GPIO, False)
def port_init():
GPIO.setmode(10)
GPIO.setmode(GPIO.BCM)
# front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
#pwm = GPIO.PWM(M1A_ENABLE_GPIO, 500)
#pwm.start(0)
# front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
# rear - left
GPIO.setup(M3A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M3A_INPUT2_GPIO, GPIO.OUT)
# rear - right
GPIO.setup(M4A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M4A_INPUT2_GPIO, GPIO.OUT)
def test_pwm(speed):
#define GPIO for controlling
#front
#front - left
GPIO.setup(M1A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M1A_INPUT2_GPIO, GPIO.OUT)
#front - right
GPIO.setup(M2A_ENABLE_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT1_GPIO, GPIO.OUT)
GPIO.setup(M2A_INPUT2_GPIO, GPIO.OUT)
GPIO.output(M1A_ENABLE_GPIO,True)
pwm1 = GPIO.PWM(M1A_ENABLE_GPIO, 10)
pwm1.start(0)
pwm2 = GPIO.PWM(M2A_ENABLE_GPIO, 10)
pwm2.start(0)
for x in range(1,10):
pwm1.ChangeDutyCycle(x * 10)
pwm2.ChangeDutyCycle(x * 10)
time.sleep(1)
