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 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)
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 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_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 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 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_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 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 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 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)
