vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix esp32 servo implementation, update rust service to work with it and support print vehicle

Browse Source
This commit is contained in:
vato007
2025-03-08 11:06:02 +10:30
parent 3ea9e30bda
commit 57982a9423
7 changed files with 80 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
car-rs/src/grpcserver.rs
View File

@@ -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;

4
car-rs/src/lib.rs
View File

@@ -1,5 +1,3 @@
use std::sync::{Arc, Mutex};
use serialport::SerialPort;
mod lidar;
@@ -136,7 +134,7 @@ impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
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),

63
car-rs/src/main.rs
View File

@@ -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<dyn std::error::Error>> {
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<T>(vehicle: T, addr: SocketAddr) -> Result<(), Box<dyn std::error::Error>>
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(())
}

22
esp32/README.md
View File

@@ -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.
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)
## 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.

2
esp32/platformio.ini
View File

@@ -12,4 +12,4 @@
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
lib_deps = roboticsbrno/ServoESP32@^1.0.3
monitor_speed = 115200

29
esp32/src/main.cpp
View File

@@ -1,16 +1,8 @@
#include <Arduino.h>
#include <map>
#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<uint8_t, Servo *> 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()

3
pycar/requirements.txt
View File

@@ -6,7 +6,8 @@ paho-mqtt
u-msgpack-python
grpcio-tools
rplidar
breezyslam
# breezyslam
pyzmq
gpiozero
pigpio
esptool