diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index b3e61ba..379b2b0 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -15,6 +15,8 @@ "/home/ibrahim/mobile_robotics_basics/lib/Wifi Configuration", "/home/ibrahim/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", "/home/ibrahim/mobile_robotics_basics/lib/Motor Setup", + "/home/ibrahim/mobile_robotics_basics/lib/Encoder Control", + "/home/ibrahim/mobile_robotics_basics/.pio/libdeps/esp32cam/AS5600", "/home/ibrahim/mobile_robotics_basics/.pio/libdeps/esp32cam/GFX Library for Arduino/src", "/home/ibrahim/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", "/home/ibrahim/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", @@ -251,6 +253,8 @@ "/home/ibrahim/mobile_robotics_basics/lib/Wifi Configuration", "/home/ibrahim/.platformio/packages/framework-arduinoespressif32/libraries/WiFi/src", "/home/ibrahim/mobile_robotics_basics/lib/Motor Setup", + "/home/ibrahim/mobile_robotics_basics/lib/Encoder Control", + "/home/ibrahim/mobile_robotics_basics/.pio/libdeps/esp32cam/AS5600", "/home/ibrahim/mobile_robotics_basics/.pio/libdeps/esp32cam/GFX Library for Arduino/src", "/home/ibrahim/.platformio/packages/framework-arduinoespressif32/libraries/Wire/src", "/home/ibrahim/.platformio/packages/framework-arduinoespressif32/libraries/SPI/src", @@ -479,7 +483,7 @@ ] }, "defines": [ - "PLATFORMIO=60110", + "PLATFORMIO=60111", "ARDUINO_ESP32_DEV", "BOARD_HAS_PSRAM", "HAVE_CONFIG_H", diff --git a/lib/Encoder Control/EncoderControl.cpp b/lib/Encoder Control/EncoderControl.cpp new file mode 100644 index 0000000..e44668a --- /dev/null +++ b/lib/Encoder Control/EncoderControl.cpp @@ -0,0 +1,92 @@ +#include "EncoderControl.h" +#include "AS5600.h" +#include +#include + +AS5600 as5600_0(&Wire); // use default Wire +AS5600 as5600_1(&Wire1); +// TwoWire I2CSensors = TwoWire(0); + +void setupEncoder0() +{ + Wire.begin(0,16); + if (!as5600_0.begin()) + { + Serial.println("Encoder 0 not connected!"); + Wire.end(); + } + else + { + as5600_0.setDirection(AS5600_CLOCK_WISE); + Serial.println("Connect device 0: true"); + } +} + +void setupEncoder1() +{ + Wire1.begin(4, 2); + if (!as5600_1.begin()) + { + Serial.println("Encoder 1 not connected!"); + Wire1.end(); + } + else + { + as5600_1.setDirection(AS5600_CLOCK_WISE); + Serial.println("Connect device 1: true"); + } +} + +void readEncoderAngle0() +{ + if (!as5600_0.begin()) + { + } + else + { + int angle1 = as5600_0.rawAngle() * AS5600_RAW_TO_DEGREES; + Serial.print("Encoder 0 Angle: "); + Serial.print(angle1); + } +} +void readEncoderAngle1() +{ + if (!as5600_1.begin()) + { + } + else + { + int angle2 = as5600_1.rawAngle() * AS5600_RAW_TO_DEGREES; + Serial.print("Encoder 1 Angle: "); + Serial.print(angle2); + } +} + +void getEncoderSpeed0() +{ + if (!as5600_0.begin()) + { + } + else + { + float speed1 = (PI * WHEEL_DIAMETER * as5600_0.getAngularSpeed(AS5600_MODE_RPM)) / 60.0; + Serial.print(" degrees | Speed 0 (cm/s): "); + Serial.println(speed1, 2); + Serial.print("Encoder 0 Speed (RPM): "); + Serial.println(as5600_0.getAngularSpeed(AS5600_MODE_RPM)); + } +} +void getEncoderSpeed1() +{ + if (!as5600_1.begin()) + { + } + else + { + float speed2 = (PI * WHEEL_DIAMETER * as5600_1.getAngularSpeed(AS5600_MODE_RPM)) / 60.0; + Serial.print(" degrees | Speed 1 (cm/s): "); + Serial.println(speed2, 2); + Serial.print("Encoder 1 Speed (RPM): "); + Serial.println(as5600_1.getAngularSpeed(AS5600_MODE_RPM)); + } +} \ No newline at end of file diff --git a/lib/Encoder Control/EncoderControl.h b/lib/Encoder Control/EncoderControl.h new file mode 100644 index 0000000..87a5eba --- /dev/null +++ b/lib/Encoder Control/EncoderControl.h @@ -0,0 +1,15 @@ +#ifndef ENCODER_CONTROL_H +#define ENCODER_CONTROL_H + +#include + +extern const float WHEEL_DIAMETER; // Declare WHEEL_DIAMETER as an external variable + +void setupEncoder0(); +void setupEncoder1(); +void readEncoderAngle0(); +void readEncoderAngle1(); +void getEncoderSpeed0(); +void getEncoderSpeed1(); + +#endif // ENCODER_CONTROL_H diff --git a/platformio.ini b/platformio.ini index 4390647..e974dd9 100644 --- a/platformio.ini +++ b/platformio.ini @@ -16,4 +16,5 @@ monitor_speed = 115200 monitor_rts = 0 monitor_dtr = 0 monitor_port = /dev/ttyUSB0 -lib_deps = moononournation/GFX Library for Arduino@^1.3.5 \ No newline at end of file +lib_deps = moononournation/GFX Library for Arduino@^1.3.5 + robtillaart/AS5600@^0.4.0 diff --git a/src/main.cpp b/src/main.cpp index 68067ba..ac312ae 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,21 +9,24 @@ #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include +#include "EncoderControl.h" +#include "AS5600.h" #define CAMERA_MODEL_AI_THINKER #include "camera_pins.h" - #define WIFI_NETWORK "Jhelum.net [Luqman House]" #define WIFI_PASSWORD "7861234786" +const float WHEEL_DIAMETER = 3.28; // in cm (32.8 mm) void startCameraServer(); void setupLedFlash(int pin); -void camera_task(void *pvParameters) { +void camera_task(void *pvParameters) +{ camera_config_t config; - config.ledc_channel = LEDC_CHANNEL_0; + config.ledc_channel = LEDC_CHANNEL_0; config.ledc_timer = LEDC_TIMER_0; config.pin_d0 = Y2_GPIO_NUM; config.pin_d1 = Y3_GPIO_NUM; @@ -44,25 +47,31 @@ void camera_task(void *pvParameters) { config.xclk_freq_hz = 20000000; config.frame_size = FRAMESIZE_UXGA; config.pixel_format = PIXFORMAT_JPEG; // for streaming - //config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition + // config.pixel_format = PIXFORMAT_RGB565; // for face detection/recognition config.grab_mode = CAMERA_GRAB_WHEN_EMPTY; config.fb_location = CAMERA_FB_IN_PSRAM; config.jpeg_quality = 12; config.fb_count = 1; - + // ... (camera configuration from setup() function) - if(config.pixel_format == PIXFORMAT_JPEG){ - if(psramFound()){ + if (config.pixel_format == PIXFORMAT_JPEG) + { + if (psramFound()) + { config.jpeg_quality = 10; config.fb_count = 2; config.grab_mode = CAMERA_GRAB_LATEST; - } else { + } + else + { // Limit the frame size when PSRAM is not available config.frame_size = FRAMESIZE_SVGA; config.fb_location = CAMERA_FB_IN_DRAM; } - } else { + } + else + { // Best option for face detection/recognition config.frame_size = FRAMESIZE_240X240; #if CONFIG_IDF_TARGET_ESP32S3 @@ -77,20 +86,23 @@ void camera_task(void *pvParameters) { // camera init esp_err_t err = esp_camera_init(&config); - if (err != ESP_OK) { + if (err != ESP_OK) + { Serial.printf("Camera init failed with error 0x%x", err); return; } - sensor_t * s = esp_camera_sensor_get(); + sensor_t *s = esp_camera_sensor_get(); // initial sensors are flipped vertically and colors are a bit saturated - if (s->id.PID == OV3660_PID) { - s->set_vflip(s, 1); // flip it back - s->set_brightness(s, 1); // up the brightness just a bit + if (s->id.PID == OV3660_PID) + { + s->set_vflip(s, 1); // flip it back + s->set_brightness(s, 1); // up the brightness just a bit s->set_saturation(s, -2); // lower the saturation } // drop down frame size for higher initial frame rate - if(config.pixel_format == PIXFORMAT_JPEG){ + if (config.pixel_format == PIXFORMAT_JPEG) + { s->set_framesize(s, FRAMESIZE_QVGA); } @@ -108,24 +120,26 @@ void camera_task(void *pvParameters) { setupLedFlash(LED_GPIO_NUM); #endif - // ... (camera sensor adjustments from setup() function) - while (1) { + while (1) + { // Do any camera-related tasks here if needed vTaskDelay(100 / portTICK_PERIOD_MS); // Adjust the delay time as per your requirement } } -void keepWiFiAlive(void * parameter){ +void keepWiFiAlive(void *parameter) +{ - WiFi.begin(WIFI_NETWORK, WIFI_PASSWORD); + WiFi.begin(WIFI_NETWORK, WIFI_PASSWORD); WiFi.setSleep(false); - while (WiFi.status() != WL_CONNECTED) { - vTaskDelay(500 / portTICK_PERIOD_MS); + while (WiFi.status() != WL_CONNECTED) + { + vTaskDelay(500 / portTICK_PERIOD_MS); Serial.print("."); } Serial.println(""); @@ -139,9 +153,8 @@ void keepWiFiAlive(void * parameter){ vTaskDelete(NULL); // End the task once Wi-Fi is connected } - - -void setup() { +void setup() +{ // Set the motor control pins to outputs pinMode(ml_1, OUTPUT); @@ -164,30 +177,38 @@ void setup() { Serial.begin(115200); Serial.setDebugOutput(true); Serial.println(); + Wire.begin(); + // setupEncoder0(); + setupEncoder1(); // Initialize WiFi and camera setupWiFi(); xTaskCreatePinnedToCore(camera_task, "camera_task", 4096, NULL, 1, NULL, 1); // Run camera task on core 1 - xTaskCreatePinnedToCore( - keepWiFiAlive, // Function to run - "keepWiFiAliveTask", // Name of the task - 4096, // Stack size (bytes) - NULL, // Task parameter - 1, // Task priority (0 to configMAX_PRIORITIES - 1) - NULL, // Task handle - 0 // Run on the first core - ); + xTaskCreatePinnedToCore( + keepWiFiAlive, // Function to run + "keepWiFiAliveTask", // Name of the task + 4096, // Stack size (bytes) + NULL, // Task parameter + 1, // Task priority (0 to configMAX_PRIORITIES - 1) + NULL, // Task handle + 0 // Run on the first core + ); // ... (LED flash setup if LED pin is defined in camera_pins.h) } -void loop() { +void loop() +{ - client = server.available(); + client = server.available(); if (!client) return; data = checkClient(); + // readEncoderAngle0(); + // getEncoderSpeed0(); + readEncoderAngle1(); + getEncoderSpeed1(); // Process the received data // processData(data); // handleData(data);