-
Notifications
You must be signed in to change notification settings - Fork 2
/
Project_AFRMU_car.py
135 lines (133 loc) · 5.06 KB
/
Project_AFRMU_car.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
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
!/usr/bin/python
#----------------------------------------------------------------------------#
#-- PROJECT Autonomous Fire Response & Monitoring Unit
#
#
#-- PROGRAM GOALS #
#-- 1. Control a ground vehicle autonomously or manually using mobile app #
#-- 2. Detect obstacles and provide warnings #
# #
#-- Programmers: Cris Thomas, Jiss Joseph Thomas #
#-- References: Donkey Car #
#----------------------------------------------------------------------------#
import socket
import sys
import atexit
import RPi.GPIO as GPIO
import time
import threading
from Raspi_MotorHAT import Raspi_MotorHAT, Raspi_DCMotor
from Raspi_PWM_Servo_Driver import PWM
from control import *
from IR import *
def get_rx_message():
global data_rx_temp
data_rx_temp_lock = threading.Lock()
global UDP_IP, UDP_PORT
# Create a UDP socket
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
print('Socket created')
except socket.error as msg:
print('Failed to create socket. Error Code : ' + str(msg[0]) + ' Message ' + msg[1])
sys.exit()
# Bind the socket to the port
server_address = (UDP_IP, UDP_PORT)
print >> sys.stderr, 'starting up on %s port %s' % server_address
try:
sock.bind(server_address)
except socket.error as msg:
print('Bind failed. Error Code : ' + str(msg[0]) + ' Message ' + msg[1])
sys.exit()
print('Socket bind complete')
atexit.register(sock.close)
while True:
print >> sys.stderr, '\nwaiting to receive message'
with data_rx_temp_lock:
data_rx_temp, address = sock.recvfrom(4096)
def do_control():
global car_fwd_back, car_left_right, set_speed
atexit.register(turnOffMotors)
atexit.register(reset_all_servos)
while True:
global cmd_mode, control_mode
if cmd_mode == 0 and control_mode == 0: # Manual Cam
do_cam_control()
elif cmd_mode == 0 and control_mode == 1: # Manual Car
do_car_control(car_fwd_back, car_left_right, set_speed)
elif cmd_mode == 1 and control_mode == 0: # Manual Cam + Auto OFF
do_cam_control()
elif cmd_mode == 1 and control_mode == 1: # Manual Cam + Auto ON
do_autonomous()
do_cam_control()
def do_autonomous():
global object_left, object_front, object_right
get_IR_data()
control_lock = threading.Lock()
with control_lock:
object_left_temp = object_left
object_front_temp = object_front
object_right_temp = object_right
if not object_front_temp:
motor_fwd_back.run(Raspi_MotorHAT.FORWARD)
motor_fwd_back_2.run(Raspi_MotorHAT.FORWARD)
motor_left_right.run(Raspi_MotorHAT.RELEASE)
print("\nAuto : Moving forward")
elif not object_right_temp:
print("\nAuto : Turning right")
motor_left_right.run(Raspi_MotorHAT.FORWARD)
motor_fwd_back.run(Raspi_MotorHAT.FORWARD)
motor_fwd_back_2.run(Raspi_MotorHAT.FORWARD)
elif not object_left_temp:
print("\nAuto : Turning left")
motor_left_right.run(Raspi_MotorHAT.BACKWARD)
motor_fwd_back.run(Raspi_MotorHAT.FORWARD)
motor_fwd_back_2.run(Raspi_MotorHAT.FORWARD)
else:
motor_fwd_back.run(Raspi_MotorHAT.BACKWARD)
motor_fwd_back_2.run(Raspi_MotorHAT.BACKWARD)
time.sleep(1)
motor_fwd_back.run(Raspi_MotorHAT.RELEASE)
motor_fwd_back_2.run(Raspi_MotorHAT.RELEASE)
print("\nAuto : Moving Back for re-route")
if not object_right_temp:
motor_left_right.run(Raspi_MotorHAT.FORWARD)
motor_fwd_back.run(Raspi_MotorHAT.FORWARD)
motor_fwd_back_2.run(Raspi_MotorHAT.FORWARD)
time.sleep(1)
print("\nAuto : Turning right for re-route")
elif not object_left_temp:
motor_left_right.run(Raspi_MotorHAT.BACKWARD)
motor_fwd_back.run(Raspi_MotorHAT.FORWARD)
motor_fwd_back_2.run(Raspi_MotorHAT.FORWARD)
time.sleep(1)
print("\nAuto : Turning left for re-route")
########################## MAIN PROGRAM ##############################
# create a default object, no changes to I2C address or frequency
mh = Raspi_MotorHAT(addr=0x6F)
# Initialise the PWM device using the default address
pwm = PWM(0x6F)
# Front motor
motor_fwd_back = mh.getMotor(1)
# Back motor
motor_fwd_back_2 = mh.getMotor(2)
# Turn motor Front to left right
motor_left_right = mh.getMotor(3)
# Speed in range of 0 - 255
motor_fwd_back.setSpeed(50)
motor_fwd_back_2.setSpeed(50)
motor_left_right.setSpeed(50)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
reset_all_servos()
turnOffMotors()
# get_rx_message()
thread_rx = threading.Thread(target=get_rx_message)
# do_control
thread_control = threading.Thread(target=do_control)
thread_rx.setDaemon(True)
thread_control.setDaemon(True)
thread_rx.start()
thread_control.start()
thread_rx.join()
thread_control.join()
GPIO.cleanup()