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 counter_clockwise(): GPIO.output(M1A_INPUT1_GPIO, False) GPIO.output(M1A_INPUT2_GPIO, True)
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)