From 57982a9423480218c1b2dbbb51004f3b53793948 Mon Sep 17 00:00:00 2001 From: vato007 Date: Sat, 8 Mar 2025 11:06:02 +1030 Subject: [PATCH] Fix esp32 servo implementation, update rust service to work with it and support print vehicle --- car-rs/src/grpcserver.rs | 2 +- car-rs/src/lib.rs | 4 +-- car-rs/src/main.rs | 63 ++++++++++++++++++++++++++++------------ esp32/README.md | 26 ++++++++++------- esp32/platformio.ini | 2 +- esp32/src/main.cpp | 29 +++++++++--------- pycar/requirements.txt | 5 ++-- 7 files changed, 80 insertions(+), 51 deletions(-) diff --git a/car-rs/src/grpcserver.rs b/car-rs/src/grpcserver.rs index 754ff6c..2ea36da 100644 --- a/car-rs/src/grpcserver.rs +++ b/car-rs/src/grpcserver.rs @@ -8,7 +8,7 @@ pub mod slam_controller_service { use std::{sync::Mutex, time::Duration}; -use car_rs::{Servo, Vehicle}; +use car_rs::Vehicle; use futures_util::StreamExt; use motor_control_service::car_control_server::CarControl; use tokio::time; diff --git a/car-rs/src/lib.rs b/car-rs/src/lib.rs index 4c122b0..2832d4f 100644 --- a/car-rs/src/lib.rs +++ b/car-rs/src/lib.rs @@ -1,5 +1,3 @@ -use std::sync::{Arc, Mutex}; - use serialport::SerialPort; mod lidar; @@ -136,7 +134,7 @@ impl Servo for Esp32SerialPwmServo { self.value = temp_value; let bytes_written = self .serial_port - .write(&[self.channel, ((value + 1.) / 2. * 255.) as u8]); + .write(&[self.channel, ((value + 1.) / 2. * 14. + 7.) as u8]); // TODO: Better error handling match bytes_written { Ok(size) => println!("{}", size), diff --git a/car-rs/src/main.rs b/car-rs/src/main.rs index 960f025..011d5b5 100644 --- a/car-rs/src/main.rs +++ b/car-rs/src/main.rs @@ -1,5 +1,7 @@ -use car_rs::{Esp32SerialPwmServo, ServoVehicle}; -use clap::Parser; +use std::net::SocketAddr; + +use car_rs::{Esp32SerialPwmServo, PrintVehicle, ServoVehicle, Vehicle}; +use clap::{Parser, ValueEnum}; use grpcserver::{ motor_control_service::car_control_server::CarControlServer, MotorControlService, }; @@ -8,6 +10,14 @@ use tonic::transport::Server; mod grpcserver; +#[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord, ValueEnum)] +enum VehicleType { + /// Use esp32 to control the car connected via usb + Esp32Serial, + /// Debug vehicle to print changes to std out. + Print, +} + #[derive(Parser, Debug)] #[command(version, about, long_about = None)] struct Args { @@ -19,28 +29,45 @@ struct Args { #[arg(short, long, default_value_t = 115200)] baud_rate: u32, + + #[arg(value_enum, short, long, default_value_t = VehicleType::Print)] + vehicle_type: VehicleType, } #[tokio::main] async fn main() -> Result<(), Box> { let args = Args::parse(); let addr = format!("[::1]:{}", &args.web_port).parse().unwrap(); - - let mut steering_port = serialport::new(&args.serial_port, args.baud_rate) - .open_native() - .expect("Could not open serial port"); - steering_port.set_exclusive(false)?; - let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate) - .open_native() - .expect("Could not open serial port"); - throttle_port.set_exclusive(false)?; - let steering_servo = Esp32SerialPwmServo::new(steering_port, 1, 12); - let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18); - - let motor_control = MotorControlService::new(ServoVehicle::new(steering_servo, throttle_servo)); - - let svc = CarControlServer::new(motor_control); - Server::builder().add_service(svc).serve(addr).await?; + match args.vehicle_type { + VehicleType::Esp32Serial => { + let mut steering_port = serialport::new(&args.serial_port, args.baud_rate) + .open_native() + .expect("Could not open serial port"); + steering_port.set_exclusive(false)?; + let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate) + .open_native() + .expect("Could not open serial port"); + throttle_port.set_exclusive(false)?; + let steering_servo = Esp32SerialPwmServo::new(steering_port, 1, 12); + let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18); + create_service(ServoVehicle::new(steering_servo, throttle_servo), addr).await?; + } + VehicleType::Print => { + let vehicle = PrintVehicle::default(); + create_service(vehicle, addr).await?; + } + } Ok(()) } + +async fn create_service(vehicle: T, addr: SocketAddr) -> Result<(), Box> +where + T: Vehicle + Send + Sync + 'static, +{ + let motor_control = MotorControlService::new(vehicle); + + let svc = CarControlServer::new(motor_control); + Server::builder().add_service(svc).serve(addr).await?; + Ok(()) +} diff --git a/esp32/README.md b/esp32/README.md index 2f54f1b..f7c4090 100644 --- a/esp32/README.md +++ b/esp32/README.md @@ -4,25 +4,29 @@ The sketch takes simple input from serial (ESP32 microusb port), and converts th The protocol specification is as follows, assuming an array of bytes: - -| Byte Number | Value Description | -| :---------- | ----------: | -| 0 | 0 if Calibrating a servo. Higher values indicates channel number to set duty cycle on. | -| 1 | If byte 0 = 0: number of servos to calibrate. Else, the new duty cycle value for the channel specified in byte 0. | +| Byte Number | Value Description | +| :---------- | ----------------------------------------------------------------------------------------------------------------: | +| 0 | 0 if Calibrating a servo. Higher values indicates channel number to set duty cycle on. | +| 1 | If byte 0 = 0: number of servos to calibrate. Else, the new duty cycle value for the channel specified in byte 0. | When setting the duty cycle, the current min angle is 0, and the max angle is 255, which allows the entire byte to be used for setting the duty cycle. The table below describes the byte format for calibrating a servo. This array of bytes can be repeated for the given number of servos in byte 1: -| Byte Number | Value Description | -| :---------- | ----------: | -| 0 | Servo channel to set (this will be byte 0 when setting duty cycle, so don't use 0). | -| 1 | The pin number to setup | +| Byte Number | Value Description | +| :---------- | ----------------------------------------------------------------------------------: | +| 0 | Servo channel to set (this will be byte 0 when setting duty cycle, so don't use 0). | +| 1 | The pin number to setup | -The min/max pulse widths are hardcoded to 1000us/2000us respectively. +The min/max pulse widths are hardcoded to 1000us/2000us respectively. At the end of each loop, the entire array will be read, to flush any data that may cause issues later. Upcoming (TODO): + - Use bit shift to allow 12 bits to be used for the duty cycle range, as there can only be a max of 16 channels anyway (4 bits). -- Consider protobuf or msgpack for serialisation format, for more advanced use cases, and more maintainable communication formats (currently changing the message format will require changes to the entire protocol) \ No newline at end of file +- Consider protobuf or msgpack for serialisation format, for more advanced use cases, and more maintainable communication formats (currently changing the message format will require changes to the entire protocol) + +## Deployment + +Sometimes you'll need to hold the boot button on the ESP32, then press upload (right arrow) on platformIO. Once the upload starts, you may release the boot button and wait for upload to complete. diff --git a/esp32/platformio.ini b/esp32/platformio.ini index 5a014b7..4f4e156 100644 --- a/esp32/platformio.ini +++ b/esp32/platformio.ini @@ -12,4 +12,4 @@ platform = espressif32 board = esp32doit-devkit-v1 framework = arduino -lib_deps = roboticsbrno/ServoESP32@^1.0.3 +monitor_speed = 115200 diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp index 48a54dc..2c40b91 100644 --- a/esp32/src/main.cpp +++ b/esp32/src/main.cpp @@ -1,16 +1,8 @@ #include -#include -#include "Servo.h" - // Min/max widths, used to calculate min/max duty cycles. -#define MIN_PULSE_WIDTH 1000 -#define MAX_PULSE_WIDTH 2000 - -#define MIN_ANGLE 0 -#define MAX_ANGLE 255 - -std::map servos; +const int MIN_ANGLE = 12; +const int MAX_ANGLE = 26; void setup() { @@ -24,19 +16,26 @@ void setupServos(uint8_t size, uint8_t *calibrationValues) { for (int i = 0; i < size; i += 2) { - Servo *newServo = new Servo(); - newServo->attach(calibrationValues[i + 1], calibrationValues[i], MIN_ANGLE, MAX_ANGLE, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); - servos.insert(std::make_pair(calibrationValues[i], newServo)); + int channel = calibrationValues[i]; + int pin = calibrationValues[i + 1]; + pinMode(pin, OUTPUT); + ledcSetup(channel, 50, 8); + ledcAttachPin(pin, channel); } } } void modifyServo(uint8_t channel, uint8_t newAngle) { - if (servos.count(channel) > 0) + if (newAngle > MAX_ANGLE) { - servos[channel]->write(newAngle); + newAngle = MAX_ANGLE; } + else if (newAngle < MIN_ANGLE) + { + newAngle = MIN_ANGLE; + } + ledcWrite(channel, newAngle); } void loop() diff --git a/pycar/requirements.txt b/pycar/requirements.txt index a0850b8..284f991 100644 --- a/pycar/requirements.txt +++ b/pycar/requirements.txt @@ -6,7 +6,8 @@ paho-mqtt u-msgpack-python grpcio-tools rplidar -breezyslam +# breezyslam pyzmq gpiozero -pigpio \ No newline at end of file +pigpio +esptool \ No newline at end of file