Skip to content

Commit 8d15918

Browse files
VladimirVincanVladimirVincan
authored
Fix UDP bugs (#278)
* fix interpreter and chmod +x * remove time from client * add time measurement to esp32 * fix box_driver
1 parent f48f0b1 commit 8d15918

File tree

2 files changed

+67
-49
lines changed

2 files changed

+67
-49
lines changed

‎firmware/box_v2/box_v2.ino

Lines changed: 63 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,14 @@
77

88

99
// Define the pins for the motors and drivers
10-
const int stepPin1 = 5;
11-
const int dirPin1 = 15;
12-
const int enablePin = 19;
13-
const int stepPin2 = 12; //D6
10+
const int stepPin1 = 5;
11+
const int dirPin1 = 15;
12+
const int enablePin = 19;
13+
const int stepPin2 = 12; //D6
1414
const int dirPin2 = 14; //D5
1515

16+
const int DC_motor = 4;
17+
1618
// Define the maximum speed and acceleration of the motors
1719
const float maxSpeed = 200; // in steps per second
1820
const float acceleration = 200; // in steps per second per second
@@ -41,11 +43,14 @@ unsigned long previousSensorTime = 0;
4143
unsigned int object_measured = 0;
4244
unsigned long previousWiFiTime = 0;
4345
unsigned long previousDisplayTime = 0;
46+
unsigned long startMotorTime = 0;
47+
unsigned long TOTAL_TIME_MS = 90000;
4448

4549
const unsigned long sensorInterval = 20; // interval in milliseconds
46-
const unsigned long WiFiInterval = 1500;
50+
const unsigned long WiFiInterval = 720;
4751
const unsigned long WiFiInterval2 = 2000;
48-
const unsigned long displayInterval = 1000;
52+
const unsigned long displayInterval = 3000;
53+
4954

5055
//SERVER SETUP
5156
// Replace with your network credentials
@@ -62,6 +67,8 @@ void setup() {
6267
// Set up the motor driver pins
6368
pinMode(enablePin, OUTPUT);
6469
digitalWrite(enablePin, LOW);
70+
pinMode(DC_motor, OUTPUT);
71+
digitalWrite(DC_motor, HIGH);
6572

6673
Serial.begin(115200);
6774

@@ -73,21 +80,24 @@ void setup() {
7380
// Set up the OLED display
7481
display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
7582
display.clearDisplay();
76-
display.setTextSize(1);
83+
display.setTextSize(2);
7784
display.setTextColor(SSD1306_WHITE);
78-
display.setCursor(0, 0);
85+
display.setCursor(0, 0);
7986
display.print("Memristor Robotics");
8087
display.display();
8188
delay(2000);
8289
display.clearDisplay();
90+
display.setCursor(0, 0);
91+
display.print("WAIT START");
92+
display.display();
8393

8494
// Connect to Wi-Fi network
8595
WiFi.begin(ssid, password);
8696
while (WiFi.status() != WL_CONNECTED) {
8797
delay(1000);
8898
Serial.println("Connecting to WiFi...");
8999
}
90-
100+
91101
// Start UDP server
92102
if (!udp.begin(localUdpPort)) {
93103
Serial.println("Failed to start UDP server");
@@ -96,7 +106,7 @@ void setup() {
96106
Serial.print("Local IP address: ");
97107
Serial.println(WiFi.localIP());
98108
Serial.print("UDP server started on port ");
99-
Serial.println(localUdpPort);
109+
Serial.println(localUdpPort);
100110

101111
Serial.println("Waiting for 1212 to start loop.");
102112

@@ -120,31 +130,43 @@ void setup() {
120130
stepper1.setAcceleration(acceleration);
121131
stepper2.setMaxSpeed(1500);
122132
stepper2.setAcceleration(1500);
123-
133+
124134
Serial.println("Starting loop.");
135+
display.clearDisplay();
136+
display.setCursor(0, 10);
137+
display.print("Count: ");
138+
display.setCursor(80, 10);
139+
display.print(objectCount);
140+
display.display();
125141
}
126142

127143

128144
void loop() {
129145
unsigned long currentTime = millis();
130146
sensorData = analogRead(sensorPin);
131-
if(sensorData > 2500 && sensorData < 3800 && motor_enable)
147+
if(sensorData > 1500 && sensorData < 3950 && motor_enable)
132148
{
133149
objectCount++;
134150
delay(300);
135-
}
136-
if (currentTime - previousDisplayTime >= displayInterval)
137-
{
138151
Serial.print("Sensor Data: ");
139152
Serial.println(sensorData);
140153

141-
display.clearDisplay();
142-
display.setCursor(0, 10);
143-
display.print("Object count: ");
154+
display.clearDisplay();
155+
display.setCursor(0, 10);
156+
display.print("Count: ");
144157
display.setCursor(80, 10);
145158
display.print(objectCount);
146159
display.display();
160+
}
161+
if (currentTime - startMotorTime >= TOTAL_TIME_MS) {
162+
// turn off motors
163+
motor_enable = 0;
164+
}
165+
if (currentTime - previousDisplayTime >= displayInterval)
166+
{
147167

168+
Serial.print("Sensor Data: ");
169+
Serial.println(sensorData);
148170
// send points via udp
149171
char message[32];
150172
sprintf(message, "%lu", objectCount);
@@ -156,48 +178,56 @@ void loop() {
156178

157179
previousDisplayTime = currentTime;
158180
}
159-
181+
//if (currentTime - previousWiFiTime >= WiFiInterval)
182+
//{
160183
// Check for incoming UDP messages
161184
int packetSize = udp.parsePacket();
162185
if (packetSize) {
163186
char packetBuffer[UDP_TX_PACKET_MAX_SIZE];
164187
udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
165188
packetBuffer[4] = 0;
166-
if (strcmp(packetBuffer, "0202") == 0)
189+
/*if (strcmp(packetBuffer, "0202") == 0)
167190
motor_enable = 0;
168-
else if (strcmp(packetBuffer, "1212") == 0)
169-
motor_enable = 1;
191+
else */
192+
if (strcmp(packetBuffer, "1212") == 0 && startMotorTime == 0) {
193+
motor_enable = 1;
194+
startMotorTime = currentTime;
195+
delay(200);
196+
}
170197
Serial.print("Received message: ");
171198
Serial.println(packetBuffer);
172199
}
173-
200+
// previousWiFiTime = currentTime;
201+
// }
174202
// check if motors should be turned off
175203
if (motor_enable)
176-
{
204+
{ digitalWrite(DC_motor, LOW);
177205
// Move the first motor continuously
178-
if (!stepper1.distanceToGo())
206+
if (!stepper1.distanceToGo())
179207
{
180-
stepper1.moveTo(-100000);
208+
stepper1.moveTo(-200000);
181209
}
182210
stepper1.run();
183-
211+
/*
184212
// Move the second motor back and forth by 20 degrees
185213
static bool direction = true;
186214
static int currentPosition = 0;
187215
const int targetPosition = direction ? currentPosition + 20 : currentPosition - 20;
188216
stepper2.moveTo(targetPosition);
189-
190-
if (abs(stepper2.distanceToGo()) < 10)
217+
218+
if (abs(stepper2.distanceToGo()) < 10)
191219
{
192220
currentPosition = targetPosition;
193221
direction = !direction;
194222
}
195-
stepper2.run();
223+
stepper2.run();
224+
*/
196225
}
197-
else
198-
{
226+
else
227+
{ digitalWrite(DC_motor, HIGH);
199228
digitalWrite(enablePin, HIGH);
229+
200230
stepper1.disableOutputs();
201231
stepper1.disableOutputs();
202232
}
203-
}
233+
}

‎mep3_hardware/mep3_hardware/box_driver.py

100644100755
Lines changed: 4 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,5 @@
1+
#!/usr/bin/env python3
2+
13
import socket
24
import select
35
import rclpy
@@ -11,7 +13,6 @@
1113
# UDP_IP = "192.168.8.101" # IP address of Vincan's laptop
1214
UDP_PORT = 8888 # Port number of the UDP server
1315
BUFFER_SIZE = 1024
14-
MATCH_END_TIME = 95 # seconds
1516

1617

1718
class ScoreboardSubscriber(Node):
@@ -26,9 +27,6 @@ def __init__(self):
2627
qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL))
2728
self.get_logger().info('Logging level: %d' % self.get_logger().get_effective_level())
2829
self.points = 0
29-
self.current_time = 0
30-
self.start_time = 0
31-
self.state = 0
3230
self.START_MESSAGE = ("0202").encode('utf-8')
3331

3432
# Create a UDP socket and listen for incoming data
@@ -44,22 +42,12 @@ def match_start_callback(self, msg):
4442
if msg.data == 2:
4543
self.START_MESSAGE = ("1212").encode('utf-8')
4644
self.state = 1
47-
if self.state == 0:
48-
self.start_time = self.current_time
4945

5046
def publish_scoreboard(self):
51-
self.current_time += 1
52-
if self.current_time - self.start_time < MATCH_END_TIME:
53-
self.sock.sendto(self.START_MESSAGE, (UDP_IP, UDP_PORT))
54-
self.get_logger().info(self.START_MESSAGE)
55-
else:
56-
END_MESSAGE = ("0202").encode('utf-8')
57-
self.sock.sendto(END_MESSAGE, (UDP_IP, UDP_PORT))
58-
self.get_logger().info(END_MESSAGE)
47+
self.sock.sendto(self.START_MESSAGE, (UDP_IP, UDP_PORT))
48+
self.get_logger().info(self.START_MESSAGE)
5949

6050
ready, _, _ = select.select([self.sock], [], [], 0.1) # Check if data is available to be read
61-
self.get_logger().info('Current time: ' + str(self.current_time))
62-
self.get_logger().info('Start time: ' + str(self.start_time))
6351
if ready:
6452
data, addr = self.sock.recvfrom(BUFFER_SIZE)
6553
self.get_logger().info("Received message:" + data.decode())

0 commit comments

Comments
 (0)