-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathUltrasonic_Spam.py
76 lines (58 loc) · 3 KB
/
Ultrasonic_Spam.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
from PinAssignment import *
import RPi.GPIO as GPIO
from threading import Thread
from multiprocessing import Process, Value, Queue
from BaseClasses import *
from Robot import Robot
# Rasp pi stuff
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
# Create component classes
left_motor = Motor(motor_left_enable, motor_left_positive, motor_left_negative, motor_left_encoder_a)
right_motor = Motor(motor_right_enable, motor_right_positive, motor_right_negative, motor_right_encoder_a)
front_left_sonic = Ultrasonic(echo_pin=front_left_sonic_echo, trig_pin=front_left_sonic_trig, x_offset=0.08512, y_offset=0.05868, theta=22.5*math.pi/180, reading_index=0, maximum_read_distance=0.3)
front_right_sonic = Ultrasonic(echo_pin=front_right_sonic_echo, trig_pin=front_right_sonic_trig, x_offset=0.08512, y_offset=-0.05868, theta=-22.5*math.pi/180, reading_index=1, maximum_read_distance=0.3)
middle_sonic = Ultrasonic(echo_pin=middle_sonic_echo, trig_pin=middle_sonic_trig, x_offset=0.08478, y_offset=0, theta=0, reading_index=2, maximum_read_distance=0.3)
left_sonic = Ultrasonic(echo_pin=left_sonic_echo, trig_pin=left_sonic_trig, x_offset=-0.0535, y_offset=0.11172, theta=math.pi/2, reading_index=3, maximum_read_distance=0.2)
right_sonic = Ultrasonic(echo_pin=right_sonic_echo, trig_pin=right_sonic_trig, x_offset=-0.535, y_offset=-0.11172, theta=-math.pi/2, reading_index=4, maximum_read_distance=0.2)
limit_switch = LimitSwitch(distance=0.105, switch_pin=limit_switch_pin)
package_sonic = Ultrasonic(echo_pin=package_sonic_echo, trig_pin=package_sonic_trig, x_offset=0, y_offset=0, theta=0, reading_index=-1, maximum_read_distance=0.3)
conveyor_motor = Motor(motor_conveyor_enable, motor_conveyor_positive, motor_conveyor_negative)
# Create robot class and instantiate component classes
pose = Pose(0.9, 0.15, 0)
robot = Robot(pose)
robot.left_motor = left_motor
robot.right_motor = right_motor
robot.front_left_ultrasonic = front_left_sonic
robot.front_right_ultrasonic = front_right_sonic
robot.middle_ultrasonic = middle_sonic
robot.right_ultrasonic = right_sonic
robot.left_ultrasonic = left_sonic
robot.limit_switch = limit_switch
robot.package_ultrasonic = package_sonic
robot.conveyor_motor = conveyor_motor
# Start encoder process
with Manager() as manager:
left_motor = robot.left_motor
right_motor = robot.right_motor
encoder_process = Process(target=robot.encoder_process, args=(left_motor, right_motor))
encoder_process.start()
# Initial thread start for localisation
ultrasonic_thread = Thread(target=robot.ultrasonic_thread)
ultrasonic_thread.start()
drive_thread = Thread(target=robot.drive_thread)
drive_thread.start()
def mainloop():
try:
while True:
distance = robot.package_ultrasonic.measure_dist()
print(distance)
sleep(0.1)
# Handle Control-C to stop motors
except KeyboardInterrupt:
robot.left_motor.speed = 0
robot.right_motor.speed = 0
robot.left_motor.stop()
robot.right_motor.stop()
if __name__ == "__main__":
mainloop()