code ready to test streaming to robot pipeline

Author: rk391222

soccer-robots-esp32/src/goalpost_mechanism.c

@@ -14,7 +14,6 @@
 
 #define IR_SENSOR_PIN       GPIO_NUM_5   // IR receiver wire - GPIO5
 
-//DRV8833 input pins for one motor
 #define MOTOR_IN1_PIN       GPIO_NUM_18  // DRV8833 IN1 (forward)
 #define MOTOR_IN2_PIN       GPIO_NUM_16  // DRV8833 IN2 (reverse)
 
@@ -24,11 +23,7 @@
 #define DETECT_DELAY_MS     500          // wait time after ball detection
 #define SPIN_TIME_MS        2000         // how long to spin the motor
 
-//server config
-// change to your Raspberry Pi / game server IP
-#define GOAL_SERVER_IP   "???"
-
-#define GOAL_SERVER_PORT 30000
+#define GOAL_LISTEN_PORT 30000
 
 static const char *TAG = "IR_MOTOR";
 
@@ -60,20 +55,20 @@ static void goalpost_led_pulse(int times)
 //motor helper functions
 static void motor_init(void)
 {
-    // Configure motor pins as plain GPIO outputs
+    //configure motor pins as plain GPIO outputs
     gpio_reset_pin(MOTOR_IN1_PIN);
     gpio_reset_pin(MOTOR_IN2_PIN);
     gpio_set_direction(MOTOR_IN1_PIN, GPIO_MODE_OUTPUT);
     gpio_set_direction(MOTOR_IN2_PIN, GPIO_MODE_OUTPUT);
 
-    // Start with motor stopped (both low => coast)
+    //start with motor stopped (both low => coast)
     gpio_set_level(MOTOR_IN1_PIN, 0);
     gpio_set_level(MOTOR_IN2_PIN, 0);
 }
 
 static void motor_spin_forward(void)
 {
-    // Spin the motor forward: IN1 = 1, IN2 = 0
+    //spin the motor forward: IN1 = 1, IN2 = 0
     gpio_set_level(MOTOR_IN1_PIN, 1);
     gpio_set_level(MOTOR_IN2_PIN, 0);
 }
@@ -84,11 +79,17 @@ static void motor_stop(void)
     gpio_set_level(MOTOR_IN2_PIN, 0);
 }
 
-//ir sensor functions
+//IR sensor functions
 static void ir_sensor_init(void)
 {
-    // Configure IR sensor as input with internal pull-up
-    // Adafruit break-beam receiver is open-collector and needs a pull-up.
+    //configure IR sensor as input with internal pull-up
+    //Adafruit break-beam receiver is open-collector and needs a pull-up.
+    /*
+    turns on the ESP32’s internal pull-up resistor because 
+    the Adafruit break-beam sensor has an open-collector output 
+    it basically pulls the line low when the beam is broken
+    otherwise it floats and needs a pull-up
+    */
     gpio_config_t io_conf = { 
         .pin_bit_mask = (1ULL << IR_SENSOR_PIN),
         .mode = GPIO_MODE_INPUT,
@@ -107,31 +108,58 @@ static int ir_beam_broken(void)
 }
 
 //network functions
-static int connect_to_goal_server(void)
+static int wait_for_pi_connection(void)
 {
-    struct sockaddr_in dest_addr;
-    int sock = -1;
+    int listen_sock = -1;
+    int client_sock = -1;
+    struct sockaddr_in listen_addr;
+    struct sockaddr_in source_addr;
+    socklen_t addr_len = sizeof(source_addr);
+
+    //create TCP socket
+    listen_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
+    if (listen_sock < 0) {
+        ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
+        return -1;
+    }
 
-    dest_addr.sin_addr.s_addr = inet_addr(GOAL_SERVER_IP);
-    dest_addr.sin_family = AF_INET;
-    dest_addr.sin_port = htons(GOAL_SERVER_PORT);
+    //allow quick reuse of the port if we reset
+    int opt = 1;
+    setsockopt(listen_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
 
-    sock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
-    if (sock < 0) {
-        ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
+    //bind to all local interfaces on GOAL_LISTEN_PORT
+    listen_addr.sin_family = AF_INET;
+    listen_addr.sin_port   = htons(GOAL_LISTEN_PORT);
+    listen_addr.sin_addr.s_addr = htonl(INADDR_ANY);
+
+    int err = bind(listen_sock, (struct sockaddr *)&listen_addr, sizeof(listen_addr));
+    if (err < 0) {
+        ESP_LOGE(TAG, "Socket unable to bind: errno %d", errno);
+        close(listen_sock);
         return -1;
     }
 
-    ESP_LOGI(TAG, "Connecting to %s:%d", GOAL_SERVER_IP, GOAL_SERVER_PORT);
-    int err = connect(sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
-    if (err != 0) {
-        ESP_LOGE(TAG, "Socket unable to connect: errno %d", errno);
-        close(sock);
+    err = listen(listen_sock, 1);
+    if (err < 0) {
+        ESP_LOGE(TAG, "Error during listen: errno %d", errno);
+        close(listen_sock);
         return -1;
     }
 
-    ESP_LOGI(TAG, "Successfully connected to goal server");
-    return sock;
+    ESP_LOGI(TAG, "Waiting for Raspberry Pi to connect on port %d...", GOAL_LISTEN_PORT);
+
+    //block here until the client/Pi connects
+    client_sock = accept(listen_sock, (struct sockaddr *)&source_addr, &addr_len);
+    if (client_sock < 0) {
+        ESP_LOGE(TAG, "Unable to accept connection: errno %d", errno);
+        close(listen_sock);
+        return -1;
+    }
+
+    ESP_LOGI(TAG, "Raspberry Pi connected!");
+
+    close(listen_sock);
+    return client_sock;
 }
 
 static void send_goal_to_server(int sock)
@@ -140,7 +168,7 @@ static void send_goal_to_server(int sock)
         return;
     }
 
-    // Follow the convention of messages ending with '|'
+    //Follow the convention of messages ending with '|'
     const char *msg = "GOAL|\n";
     int len = strlen(msg);
 
@@ -152,51 +180,54 @@ static void send_goal_to_server(int sock)
     }
 }
 
+/*
 //goalpost task
 void goalpost_mechanism_task(void *pvParameters)
 {
-    // Hardware init
+    //hardware init
     ir_sensor_init();
     motor_init();
     goalpost_led_init();
 
     ESP_LOGI(TAG, "Goalpost mechanism starting...");
 
-    // At this point Wi-Fi should already be up (wifi_setup_init was called in app_main)
-    // Now connect to the game server:
+    
+    //wait for the Raspberry Pi to connect to us
     int sock = -1;
     while (sock < 0) {
-        sock = connect_to_goal_server();
+        sock = wait_for_pi_connection();
         if (sock < 0) {
-            ESP_LOGW(TAG, "Retrying server connection in 2 seconds...");
+            ESP_LOGW(TAG, "Failed to get Pi connection, retrying in 2 seconds...");
             vTaskDelay(pdMS_TO_TICKS(2000));
         }
     }
+    
+    int sock = -1;
 
     ESP_LOGI(TAG, "Waiting for ball...");
 
     while (true) {
         if (ir_beam_broken()) {
-            // Beam broken → "ball detected"
+            //beam broken → "ball detected"
             ESP_LOGI(TAG, "Ball detected! Beam broken.");
             goalpost_led_pulse(3);  // 3 quick blinks
 
             vTaskDelay(pdMS_TO_TICKS(DETECT_DELAY_MS));
 
-            // Spin the motor
+            //spin the motor
             goalpost_led_set(1);    // LED ON while spinning
             motor_spin_forward();
 
-            // Notify server about the goal
-            send_goal_to_server(sock);
+            //notify server about the goal
+            //send_goal_to_server(sock);
 
             vTaskDelay(pdMS_TO_TICKS(SPIN_TIME_MS));
 
-            // Stop motor and LED
+            //stop motor and LED
             motor_stop();
             goalpost_led_set(0);
 
-            // Wait until beam is restored before re-arming
+            //wait until beam is restored before re-arming
             while (ir_beam_broken()) {
                 vTaskDelay(pdMS_TO_TICKS(50));
             }
@@ -207,10 +238,61 @@ void goalpost_mechanism_task(void *pvParameters)
         vTaskDelay(pdMS_TO_TICKS(50)); // small poll delay
     }
 
-    // Not really reached, but for completeness:
+    //cleanup on exit (never reached in this example)
     if (sock >= 0) {
         shutdown(sock, 0);
         close(sock);
     }
     vTaskDelete(NULL);
 }
+*/
+
+//task for testing goalpost mechanism without wifi
+void goalpost_mechanism_task(void *pvParameters)
+{
+    //hardware init
+    ir_sensor_init();
+    motor_init();
+    goalpost_led_init();
+
+    ESP_LOGI(TAG, "Goalpost HARDWARE TEST: no Wi-Fi sockets, just IR + motor");
+
+    bool last_broken = false;
+
+    while (true) {
+
+        bool broken = ir_beam_broken();
+
+        //log when the beam state changes
+        if (broken != last_broken) {
+            ESP_LOGI(TAG, "IR beam is now: %s", broken ? "BROKEN" : "OK");
+            last_broken = broken;
+        }
+
+        //trigger ONCE per break (edge trigger)
+        if (broken) {
+            ESP_LOGI(TAG, "Ball detected! Spinning motor once.");
+
+            goalpost_led_pulse(3);   // blink a bit
+            goalpost_led_set(1);     // LED ON while spinning
+
+            motor_spin_forward();    // turn motor ON
+            vTaskDelay(pdMS_TO_TICKS(SPIN_TIME_MS));
+
+            motor_stop();            // turn motor OFF
+            goalpost_led_set(0);     // LED OFF
+
+            ESP_LOGI(TAG, "Motor stopped, waiting for beam to restore...");
+
+            // Wait until beam is no longer broken
+            while (ir_beam_broken()) {
+                vTaskDelay(pdMS_TO_TICKS(50));
+            }
+
+            ESP_LOGI(TAG, "Beam restored, re-armed.");
+        }
+
+        vTaskDelay(pdMS_TO_TICKS(50));
+    }
+}
+