Last active
November 22, 2017 17:12
-
-
Save manu-chroma/b6643f01eb203f460c86c5a98bfb3c2f to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import RPi.GPIO as GPIO #Import GPIO library | |
| import time | |
| GPIO.setwarnings(False) | |
| GPIO.setmode(GPIO.BCM) # programming the GPIO by BCM pin numbers | |
| # set tigger and echo pins for the ultrasonic sensor | |
| TRIG = 17 | |
| ECHO = 27 | |
| LED = 22 | |
| motor1_1 = 16 | |
| motor1_2 = 12 | |
| motor2_1 = 21 | |
| motor2_2 = 20 | |
| # setup for Ulrasonic sensor | |
| GPIO.setup(TRIG,GPIO.OUT) # initialize GPIO Pin as outputs | |
| GPIO.setup(ECHO,GPIO.IN) # initialize GPIO Pin as input | |
| # setup for LED pin | |
| GPIO.setup(LED, GPIO.OUT) | |
| # setup for motors | |
| GPIO.setup(motor1_1, GPIO.OUT) | |
| GPIO.setup(motor1_2, GPIO.OUT) | |
| GPIO.setup(motor2_1, GPIO.OUT) | |
| GPIO.setup(motor2_2, GPIO.OUT) | |
| # light up the LED pin before start. | |
| GPIO.output(LED, 1) | |
| # Sleep for five seconds after setting up | |
| time.sleep(5) | |
| # Defining functions for the motor. | |
| def stop(): | |
| print("stop") | |
| GPIO.output(motor1_1, 0) | |
| GPIO.output(motor1_2, 0) | |
| GPIO.output(motor2_1, 0) | |
| GPIO.output(motor2_2, 0) | |
| def forward(): | |
| print("Forward") | |
| GPIO.output(motor1_1, 1) | |
| GPIO.output(motor1_2, 0) | |
| GPIO.output(motor2_1, 1) | |
| GPIO.output(motor2_2, 0) | |
| def back(): | |
| print("Back") | |
| GPIO.output(motor1_1, 0) | |
| GPIO.output(motor1_2, 1) | |
| GPIO.output(motor2_1, 0) | |
| GPIO.output(motor2_2, 1) | |
| def left(): | |
| print("Left") | |
| GPIO.output(motor1_1, 0) | |
| GPIO.output(motor1_2, 0) | |
| GPIO.output(motor2_1, 1) | |
| GPIO.output(motor2_2, 0) | |
| def right(): | |
| print("Right") | |
| GPIO.output(motor1_1, 1) | |
| GPIO.output(motor1_2, 0) | |
| GPIO.output(motor2_1, 0) | |
| GPIO.output(motor2_2, 0) | |
| stop() | |
| count = 0 | |
| while True: | |
| avg_distance = 0 | |
| for _ in range(5): | |
| GPIO.output(TRIG, False) # Set TRIG as LOW | |
| time.sleep(0.1) # Delay | |
| GPIO.output(TRIG, True) # Set TRIG as HIGH | |
| time.sleep(0.00001) # Delay of 0.00001 seconds | |
| GPIO.output(TRIG, False) # Set TRIG as LOW | |
| while GPIO.input(ECHO)==0: # Check whether the ECHO is LOW | |
| GPIO.output(LED, False) | |
| pulse_start = time.time() | |
| while GPIO.input(ECHO)==1: # Check whether the ECHO is HIGH | |
| GPIO.output(LED, False) | |
| pulse_end = time.time() | |
| # Time to get back the pulse to sensor | |
| pulse_duration = pulse_end - pulse_start | |
| # Multiply pulse duration by 17150 (34300/2) to get distance, speed of sound | |
| distance = pulse_duration * 17150 | |
| # Round to two decimal points | |
| distance = round(distance,2) | |
| avg_distance += distance | |
| # Take average of 5 readings. | |
| avg_distance /= 5.0 | |
| print(avg_distance) | |
| flag = 0 | |
| # Variable to store if the obstacle has been picked or not | |
| obstacle_picked = False | |
| if avg_distance < THRESHOLD && obstacle_picked == False: | |
| stop() | |
| # use servo motor to pick the robot, | |
| # move ahead for few miliseconds to get closer to the obstacle | |
| forward() | |
| time.sleep(0.5) # @todo: finetune this for better accuracy. | |
| stop() | |
| # Use servo to pick up | |
| GPIO.setmode(GPIO.BOARD) ## Use BOARD pin numbering. | |
| GPIO.setup(22, GPIO.OUT) ## set output. | |
| pwm = GPIO.PWM(22,100) ## PWM Frequency | |
| pwm.start(5) | |
| angle1 = 10 | |
| duty1 = float(angle1)/10 + 2.5 ## Angle To Duty cycle Conversion | |
| angle2 = 160 | |
| duty2 = float(angle2)/10 + 2.5 | |
| ck = 0 | |
| while ck <= 5: | |
| pwm.ChangeDutyCycle(duty1) | |
| time.sleep(0.8) | |
| pwm.ChangeDutyCycle(duty2) | |
| time.sleep(0.8) | |
| ck += 1 | |
| time.sleep(1) | |
| GPIO.cleanup() | |
| obstacle_picked = True | |
| # Check for threshold distance, | |
| # whether the distance is within 15 cm range | |
| if avg_distance < 15 && obstacle_picked == True: | |
| count += 1 | |
| stop() | |
| time.sleep(1) | |
| back() | |
| time.sleep(1.5) | |
| # Randomly take right or left | |
| if count%3 == 1 && flag == 0: | |
| right() | |
| flag = 1 | |
| else: | |
| left() | |
| flag = 0 | |
| time.sleep(1.5) | |
| stop() | |
| time.sleep(1) | |
| else: | |
| forward() | |
| flag = 0 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment