diff --git a/Control/Pi/main_server.py b/Control/Pi/main_server.py index 2e894c9..ace4254 100755 --- a/Control/Pi/main_server.py +++ b/Control/Pi/main_server.py @@ -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() diff --git a/Control/Pi/proximity_detector.py b/Control/Pi/proximity_detector.py index 8feebbc..86d9f96 100644 --- a/Control/Pi/proximity_detector.py +++ b/Control/Pi/proximity_detector.py @@ -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 diff --git a/Embedded/proximity_sensor/proximity_sensor.ino b/Embedded/proximity_sensor/proximity_sensor.ino index 8b0320e..de70568 100644 --- a/Embedded/proximity_sensor/proximity_sensor.ino +++ b/Embedded/proximity_sensor/proximity_sensor.ino @@ -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); }