max/new_thea
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max 2021-09-22 14:05:43 +01:00
parent 3b158f733b
commit 3bcdc5afe3
3 changed files with 32 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Control/Pi/main_server.py
View File

@ -30,7 +30,7 @@ from state_machine import StateMachineHandler
STATE_MACHINE = StateMachineHandler(force=True)
STATE_MACHINE.set_state("IDLE")
PROXIMITY_STOP_ENABLED = False
PROXIMITY_STOP_ENABLED = True
KILL_ON_DEAD_SERVICES = False
FORKLIFT = DroneInterface()

11
Control/Pi/proximity_detector.py
View File

@ -17,7 +17,7 @@ class Proximity:
# self.limit_cm = 5
self.state_machine = state_machine
self.vehicle_ctrl = vehicle_ctrl
self.rear_sensors = [3, 5, 2] # TODO: change with more sensors
self.rear_sensors = [4, 5, 6, 7, 8, 9, 10] # TODO: change with more sensors
self.proximity_detects = 0
self.proximity_detect_thresh = 1
try:
@ -28,13 +28,13 @@ class Proximity:
self.log.critical(f"Cannot open serial device, {e}")
self.log.critical('Exiting')
exit(0)
self.proximities = [99, 99, 99, 99, 99, 99, 99, 99]
self.proximities = [99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99]
def get_prox_readings(self):
try:
self.ser.flushInput()
time.sleep(0.05)
p1 = p2 = p3 = p4 = p5 = p6 = p7 = p8 = p9 = p10 = p11 = p12 = 0
p1 = p2 = p3 = p4 = p5 = p6 = p7 = p8 = p9 = p10 = p11 = 0
for _ in range(2):
raw = str(self.ser.readline())
raw2 = raw.split('\\r')[0].split("'")[1]
@ -50,8 +50,7 @@ class Proximity:
p9 += int(raw3[8])
p10 += int(raw3[9])
p11 += int(raw3[10])
p12 += int(raw3[11])
self.proximities = [int(p1/2), int(p2/2), int(p3/2), int(p4/2), int(p5/2), int(p6/2), int(p7/32), int(p8/2)]
self.proximities = [int(p1/2), int(p2/2), int(p3/2), int(p4/2), int(p5/2), int(p6/2), int(p7/32), int(p8/2), int(p9/2), int(p10/2), int(p11/2)]
self.failed_reads = 0
return self.proximities
except Exception as e:
@ -66,7 +65,7 @@ class Proximity:
def check_proximity(self):
sensor, dist = self.perform_thresh()
# self.log.info(f"Proximities: FFR: {self.proximities[7]} | FFL: {self.proximities[6]} | FR: {self.proximities[1]} | FC: {self.proximities[4]} | FL: {self.proximities[0]} | RL: {self.proximities[3]} | RC: {self.proximities[5]} | RR: {self.proximities[2]}")
self.log.info(f"Proximities: FLL: {self.proximities[0]} | FLR: {self.proximities[1]} | FRL: {self.proximities[2]} | FRR: {self.proximities[3]} | RRR: {self.proximities[4]} | RRC: {self.proximities[5]} | RR: {self.proximities[6]} | RC: {self.proximities[7]} | RL: {self.proximities[8]} | RLC: {self.proximities[9]} | RLL: {self.proximities[10]}")
self.log.info(f"Proximities: {self.proximities}")
if sensor != None:
self.proximity_detects +=1

60
Embedded/proximity_sensor/proximity_sensor.ino
View File

@ -9,44 +9,42 @@ x = 3000 µs
#define PULSE_DELAY 3000
// #define PULSE_DELAY 15000
// front fork LL LS RS RR
const int trig_pin_2 = 8;
const int echo_pin_2 = 9;
const int trig_pin_1 = 8;
const int echo_pin_1 = 9;
const int trig_pin_1 = 10;
const int echo_pin_1 = 11;
const int trig_pin_2 = 10;
const int echo_pin_2 = 11;
const int trig_pin_3 = 12;
const int echo_pin_3 = 13;
//---
// |
//---
const int trig_pin_4 = 14;
const int echo_pin_4 = 15;
const int trig_pin_5 = 16;
const int echo_pin_5 = 17;
const int trig_pin_3 = 16;
const int echo_pin_3 = 17;
const int trig_pin_6 = 18;
const int echo_pin_6 = 19;
// Rear Right RRR RRS
const int trig_pin_10 = 22;
const int echo_pin_10 = 24;
const int trig_pin_7 = 20;
const int echo_pin_7 = 21;
// --- <|
const int trig_pin_8 = 22;
const int echo_pin_8 = 24;
const int trig_pin_11 = 26;
const int echo_pin_11 = 28;
const int trig_pin_9 = 26;
const int echo_pin_9 = 28;
//Rear Straight RSR RSL RSS
const int trig_pin_7 = 30;
const int echo_pin_7 = 32;
const int trig_pin_10 = 30;
const int echo_pin_10 = 32;
const int trig_pin_9 = 34;
const int echo_pin_9 = 36;
const int trig_pin_11 = 34;
const int echo_pin_11 = 36;
const int trig_pin_8 = 38;
const int echo_pin_8 = 40;
const int trig_pin_12 = 38;
const int echo_pin_12 = 40;
//Real Left RLL RLR
const int trig_pin_5 = 42;
const int echo_pin_5 = 44;
const int trig_pin_6 = 46;
const int echo_pin_6 = 48;
int distance_1;
int distance_2;
@ -59,7 +57,6 @@ int distance_8;
int distance_9;
int distance_10;
int distance_11;
int distance_12;
void setup() {
pinMode(trig_pin_1, OUTPUT); // Sets the trigPin as an Output
@ -84,8 +81,6 @@ void setup() {
pinMode(echo_pin_10, INPUT);
pinMode(trig_pin_11, OUTPUT);
pinMode(echo_pin_11, INPUT);
pinMode(trig_pin_12, OUTPUT);
pinMode(echo_pin_12, INPUT);
Serial.begin(9600); // Starts the serial communication
}
@ -118,7 +113,6 @@ void loop() {
distance_9 = get_dist(trig_pin_9, echo_pin_9);
distance_10 = get_dist(trig_pin_10, echo_pin_10);
distance_11 = get_dist(trig_pin_11, echo_pin_11);
distance_12 = get_dist(trig_pin_12, echo_pin_12);
Serial.print(distance_1);
Serial.print(" | ");
@ -140,7 +134,5 @@ void loop() {
Serial.print(" | ");
Serial.print(distance_10);
Serial.print(" | ");
Serial.print(distance_11);
Serial.print(" | ");
Serial.println(distance_12);
Serial.println(distance_11);
}