This commit is contained in:
StackForce 2024-10-25 16:24:25 +08:00
parent fdf202a04c
commit ec9c10c613
20 changed files with 642 additions and 0 deletions

View File

@ -0,0 +1,34 @@
import serial
import time
class MotorController:
def __init__(self):
self.ser = None # Initialize serial attribute
def initialize_serial(self, port='COM56', baudrate=115200, timeout=1):
"""Initialize the serial connection."""
self.ser = serial.Serial(port, baudrate, timeout=timeout)
def send_target(self, m0_target, m1_target):
command = f'T{m0_target} {m1_target}\n'
self.ser.write(command.encode())
def send_mode(self, m0_mode, m1_mode):
command = f'M{m0_mode} {m1_mode}\n'
self.ser.write(command.encode())
def send_pid(self, m0_p, m0_i, m0_d, m1_p, m1_i, m1_d):
command = f'P{m0_p} {m0_i} {m0_d} {m1_p} {m1_i} {m1_d}\n'
self.ser.write(command.encode())
def receive_motor_data(self):
if self.ser.in_waiting > 0:
line = self.ser.readline().decode('utf-8').strip()
if line.startswith('D'):
data = line[1:].split()
m0_angle = float(data[0])
m0_speed = float(data[1])
m1_angle = float(data[2])
m1_speed = float(data[3])
return m0_angle, m0_speed, m1_angle, m1_speed
return None

View File

@ -0,0 +1,118 @@
import sensor, image, time, math
from pyb import UART
from ulab import numpy as np
import json
import utime
from SF_Serial import MotorController
motor_controller = MotorController() #创建电机控制对象
motor_controller.initialize_serial(port=3, baudrate=921600) # 选择openmv的串口3波特率921600
#yel_threshold = (54, 100, -27, 44, 40, 71)#黄色
#gre_threshold = (0, 100, -50, -15, 0, 45)#绿色
ora_threshold = (0, 100, 15, 55, 25, 55)#橙色
sensor.reset() # 初始化摄像头
sensor.set_pixformat(sensor.RGB565) # 格式为 RGB565.
sensor.set_framesize(sensor.QVGA) # 使用 QQVGA 速度快一些
sensor.skip_frames(time = 2000) # 跳过2000s使新设置生效,并自动调节白平衡
sensor.set_auto_gain(False) # 关闭自动自动增益。默认开启的,在颜色识别中,一定要关闭白平衡。
#sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False)
sensor.set_vflip(True)
clock = time.clock() # 追踪帧率
def _constrain(amt,low,high):
if amt<low:
amt = low
elif amt>high:
amt = high
return amt
#创建pid类
class PIDController:
def __init__(self, P=0, I=0, D=0, limit=0):
self.P = P
self.I = I
self.D = D
self.limit = limit
self.timestamp_prev = 0
self.integral_prev = 0
self.error_prev = 0
def operator(self,error):
# gloabal
timestamp_now = utime.time()
Ts = timestamp_now - self.timestamp_prev
if Ts <= 0 or Ts > 0.5:
Ts = 1e-3
proportional = self.P * error
integral = self.integral_prev + self.I*Ts*0.5*(error + self.error_prev)
integral = _constrain(integral, -self.limit, self.limit)
derivative = self.D*(error - self.error_prev)/Ts
output = proportional + integral + derivative
output = _constrain(output, -self.limit, self.limit)
self.integral_prev = integral
self.output_prev = output
self.error_prev = error
self.timestamp_prev = timestamp_now
return output
#创建pid对象
angle_loop = PIDController(P=1.7,I=0.1,limit=8) #位置环
inertia_loop = PIDController(P=0.6,limit=3) #大减速环
Minertia_loop = PIDController(P=0.1,limit=8) #小减速环
#返回看到的最大色块
def find_max(blobs):
max_size=0
for blob in blobs:
if blob[2]*blob[3] > max_size:
max_blob=blob
max_size = blob[2]*blob[3]
return max_blob
prev_time = 0
prev_X_targe = 0
while(True):
clock.tick()
img = sensor.snapshot() # 从感光芯片获得一张图像
blobs = img.find_blobs([ora_threshold],area_threshold = 240, pixels_threshold = 240, merge = True)
if blobs:
max_blob = find_max(blobs)#返回看到的最大色块
rect = max_blob.rect()
x, y, w, h = rect[0], rect[1], rect[2], rect[3] #色块左上角x,y坐标和长宽
img.draw_rectangle(rect, color = (255, 0, 0)) #在图像中画框
X_targe = (x+w/2-160)*0.006 #将目标X坐标转为以图像中心为原点的坐标轴坐标并将坐标转为大致角度值
# pid计算
now_time = utime.time() #获取当前时间戳
Ts = now_time - prev_time #获取时间差
if Ts==0: #防止程序卡死
Ts = 0.001
vel_targe = (X_targe - prev_X_targe)/Ts*0.01 #根据目标值变化速度得到电机大致的移动速度
if abs(X_targe) > abs(prev_X_targe): #当前目标值比上一个目标值大,色块往图像外移动,加速追踪阶段不需要减速
outout = X_targe
else:
if X_targe < 0.4: #相机快要追到色块(色块往图像里移动),减速阶段,给非常大的减速环
outout = X_targe + inertia_loop.operator(error=vel_targe)#减速环,速度越大,反向力矩越大
else: #相机还有段距离才能追到色块,小减速阶段,给一个小的减速环
outout = X_targe + Minertia_loop.operator(error=vel_targe)
output_angle = angle_loop.operator(error=outout)#位置环,将减速环作为内环串联到位置环里
output_str="%f"%output_angle #转为字符串
motor_controller.send_target(0, output_str) #串口发送目标位置给M1电机
prev_time = now_time
prev_X_targe = X_targe

View File

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

View File

@ -0,0 +1,10 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
],
"unwantedRecommendations": [
"ms-vscode.cpptools-extension-pack"
]
}

View File

@ -0,0 +1,5 @@
{
"files.associations": {
"functional": "cpp"
}
}

View File

@ -0,0 +1,39 @@
This directory is intended for project header files.
A header file is a file containing C declarations and macro definitions
to be shared between several project source files. You request the use of a
header file in your project source file (C, C++, etc) located in `src` folder
by including it, with the C preprocessing directive `#include'.
```src/main.c
#include "header.h"
int main (void)
{
...
}
```
Including a header file produces the same results as copying the header file
into each source file that needs it. Such copying would be time-consuming
and error-prone. With a header file, the related declarations appear
in only one place. If they need to be changed, they can be changed in one
place, and programs that include the header file will automatically use the
new version when next recompiled. The header file eliminates the labor of
finding and changing all the copies as well as the risk that a failure to
find one copy will result in inconsistencies within a program.
In C, the usual convention is to give header files names that end with `.h'.
It is most portable to use only letters, digits, dashes, and underscores in
header file names, and at most one dot.
Read more about using header files in official GCC documentation:
* Include Syntax
* Include Operation
* Once-Only Headers
* Computed Includes
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

View File

@ -0,0 +1,46 @@
This directory is intended for project specific (private) libraries.
PlatformIO will compile them to static libraries and link into executable file.
The source code of each library should be placed in an own separate directory
("lib/your_library_name/[here are source files]").
For example, see a structure of the following two libraries `Foo` and `Bar`:
|--lib
| |
| |--Bar
| | |--docs
| | |--examples
| | |--src
| | |- Bar.c
| | |- Bar.h
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
| |
| |--Foo
| | |- Foo.c
| | |- Foo.h
| |
| |- README --> THIS FILE
|
|- platformio.ini
|--src
|- main.c
and a contents of `src/main.c`:
```
#include <Foo.h>
#include <Bar.h>
int main (void)
{
...
}
```
PlatformIO Library Dependency Finder will find automatically dependent
libraries scanning project source files.
More information about PlatformIO Library Dependency Finder
- https://docs.platformio.org/page/librarymanager/ldf.html

View File

@ -0,0 +1,54 @@
#ifndef _SF_BLDC_h
#define _SF_BLDC_h
#include <Arduino.h>
#include "SF_BLDC_shared_struct.h"
class SF_BLDC{
public:
SF_BLDC(HardwareSerial &serial);
// char Command_1,Command_7; //包头和包尾
// float Command_2,Command_3,Command_4,Command_5,Command_6; //中间指令
uint8_t M0_mode, M1_mode;
float M0_target, M1_target;
void init();
SF_BLDC_DATA getBLDCData();
void setTargets(float M0_tar,float M1_tar);
void setModes(uint8_t M0_mod, uint8_t M1_mod);
void setM0VelPID(float P,float I,float D,float limit);
void setM1VelPID(float P,float I,float D,float limit);
void setM0AnglePID(float P,float I,float D,float limit);
void setM1AnglePID(float P,float I,float D,float limit);
void setM0CurrentPID(float P,float I,float D,float limit);
void setM1CurrentPID(float P,float I,float D,float limit);
void alignSensors(float M0_AlignVoltage,float M1_AlignVoltage);
private:
HardwareSerial *_serial;
String receivedChars;
float uart_temp_M0_angle,uart_temp_M1_angle,uart_temp_M0_vel,uart_temp_M1_vel,uart_temp_M0_Uq,uart_temp_M1_Uq,uart_temp_M0_Ud,uart_temp_M1_Ud,uart_temp_M0_Iq,uart_temp_M1_Iq,uart_temp_M0_Id,uart_temp_M1_Id;
float recM0Angle,recM1Angle,recM0Vel,recM1Vel,recM0Uq,recM1Uq,recM0Ud,recM1Ud,recM0Iq,recM1Iq,recM0Id,recM1Id;
SF_BLDC_DATA values;
float M0_VelP,M0_VelI,M0_VelD;
float M1_VelP,M1_VelI,M1_VelD;
float M0_AngleP,M0_AngleI,M0_AngleD;
float M1_AngleP,M1_AngleI,M1_AngleD;
float M0_CurrentP,M0_CurrentI,M0_CurrentD;
float M1_CurrentP,M1_CurrentI,M1_CurrentD;
static void appCpuLoopWrapper(void *pvParameters); // 静态包装函数
void appCpuLoop(); // 实际的成员函数
String recFromSerial();
void sendToSerial(String S_Com_1,float S_Com_2,float S_Com_3,float S_Com_4,float S_Com_5,float S_Com_6,String S_Com_7);
};
#endif

View File

@ -0,0 +1,66 @@
#ifndef _SF_BLDC_SHARED_STRUCT_h
#define _SF_BLDC_SHARED_STRUCT_h
struct SF_BLDC_DATA {
float M0_Angle;
float M0_ElecAngle;
float M0_Vel;
float M0_Uq;
float M0_Ud;
float M0_Iq;
float M0_Id;
float M1_Angle;
float M1_ElecAngle;
float M1_Vel;
float M1_Uq;
float M1_Ud;
float M1_Iq;
float M1_Id;
};
struct SF_BLDC_Wireless_DATA {
float M0_Target;
float M1_Target;
float M0_Mode;
float M1_Mode;
//PID 无线设置相关
//M0
float M0_POS_P;
float M0_POS_I;
float M0_POS_D;
float M0_POS_LIM;
float M0_VEL_P;
float M0_VEL_I;
float M0_VEL_D;
float M0_VEL_LIM;
float M0_CURR_P;
float M0_CURR_I;
float M0_CURR_D;
float M0_CURR_LIM;
//M1
float M1_POS_P;
float M1_POS_I;
float M1_POS_D;
float M1_POS_LIM;
float M1_VEL_P;
float M1_VEL_I;
float M1_VEL_D;
float M1_VEL_LIM;
float M1_CURR_P;
float M1_CURR_I;
float M1_CURR_D;
float M1_CURR_LIM;
};
#endif

View File

@ -0,0 +1,61 @@
#include "pid.h"
#include <Arduino.h>
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
: P(P)
, I(I)
, D(D)
, output_ramp(ramp) // output derivative limit [volts/second]
, limit(limit) // output supply limit [volts]
, error_prev(0.0f)
, output_prev(0.0f)
, integral_prev(0.0f)
{
timestamp_prev = micros();
}
// PID controller function
float PIDController::operator() (float error){
// calculate the time from the last call
unsigned long timestamp_now = micros();
float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
// quick fix for strange cases (micros overflow)
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
// u(s) = (P + I/s + Ds)e(s)
// Discrete implementations
// proportional part
// u_p = P *e(k)
float proportional = P * error;
// Tustin transform of the integral part
// u_ik = u_ik_1 + I*Ts/2*(ek + ek_1)
float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
// antiwindup - limit the output
integral = _constrain(integral, -limit/3, limit/3);
// Discrete derivation
// u_dk = D(ek - ek_1)/Ts
float derivative = D*(error - error_prev)/Ts;
// sum all the components
float output = proportional + integral + derivative;
// antiwindup - limit the output variable
output = _constrain(output, -limit, limit);
// if output ramp defined
if(output_ramp > 0){
// limit the acceleration by ramping the output
float output_rate = (output - output_prev)/Ts;
if (output_rate > output_ramp)
output = output_prev + output_ramp*Ts;
else if (output_rate < -output_ramp)
output = output_prev - output_ramp*Ts;
}
// saving for the next pass
integral_prev = integral;
output_prev = output;
error_prev = error;
timestamp_prev = timestamp_now;
return output;
}

View File

@ -0,0 +1,37 @@
#ifndef PID_H
#define PID_H
/**
* PID controller class
*/
class PIDController
{
public:
/**
*
* @param P - Proportional gain
* @param I - Integral gain
* @param D - Derivative gain
* @param ramp - Maximum speed of change of the output value
* @param limit - Maximum output value
*/
PIDController(float P, float I, float D, float ramp, float limit);
~PIDController() = default;
float operator() (float error);
float P; //!< Proportional gain
float I; //!< Integral gain
float D; //!< Derivative gain
float output_ramp; //!< Maximum speed of change of the output value
float limit; //!< Maximum output value
protected:
float error_prev; //!< last tracking error value
float output_prev; //!< last pid output value
float integral_prev; //!< last integral component value
unsigned long timestamp_prev; //!< Last execution timestamp
};
#endif // PID_H

View File

@ -0,0 +1,18 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:esp32-s3-devkitc-1]
platform = espressif32@6.4.0
board = esp32-s3-devkitc-1
framework = arduino
build_flags =
-Llib/SF_BLDC
-lSF_BLDC

View File

@ -0,0 +1,73 @@
#include <Arduino.h>
#include "SF_Serial.h"
SF_Serial_DATA receive_Data;
SF_Serial_DATA send_data;
uint8_t Serial_Contact(void){
if (Serial1.available()) {
char header = Serial1.read(); // Read the header: 'T', 'M', or 'P'
if (header == 'T') {
receive_Target_Data(); // Handle target data
} else if (header == 'M') {
receive_Mode_Data(); // Handle mode data
} else if (header == 'P') {
receive_PID_Data(); // Handle PID data
}
return 1;
}
return 0;
}
// Function to receive and process target data
void receive_Target_Data() {
char buffer[50]; // Adjust size as needed
if (Serial1.readBytesUntil('\n', buffer, sizeof(buffer))) {
sscanf(buffer, "%f %f", &receive_Data.M0_Target, &receive_Data.M1_Target);
}
}
SF_Serial_DATA Serial_Data(void){
return receive_Data;
}
// Function to receive and process mode data
void receive_Mode_Data() {
char buffer[50]; // Adjust size as needed
if (Serial1.readBytesUntil('\n', buffer, sizeof(buffer))) {
sscanf(buffer, "%f %f", &receive_Data.M0_Mode, &receive_Data.M1_Mode);
}
}
// Function to receive and process PID data
void receive_PID_Data() {
char buffer[50]; // Adjust size as needed
if (Serial1.readBytesUntil('\n', buffer, sizeof(buffer))) {
sscanf(buffer, "%f %f %f %f %f %f", &receive_Data.M0_P, &receive_Data.M0_I, &receive_Data.M0_D, &receive_Data.M1_P, &receive_Data.M1_I, &receive_Data.M1_D);
}
}
// Function to send motor data to Python
void send_Motor_Data(float M0_angle,float M0_speed,float M1_angle,float M1_speed) {
// motorData.M0_angle = 45.0; // Sample data, replace with actual values
// motorData.M0_speed = 100.0;
// motorData.M1_angle = 60.0;
// motorData.M1_speed = 120.0;
send_data.M0_angle = M0_angle; // Sample data, replace with actual values
send_data.M0_speed = M0_speed;
send_data.M1_angle = M1_angle;
send_data.M1_speed = M1_speed;
// Sending formatted data to Python
Serial1.printf("D%f %f %f %f\n", send_data.M0_angle, send_data.M0_speed, send_data.M1_angle, send_data.M1_speed);
}

View File

@ -0,0 +1,35 @@
#ifndef __SERIAL_H
#define __SERIAL_H
uint8_t Serial_Contact(void);//判断包头
void receive_Target_Data();
void receive_Mode_Data();
void receive_PID_Data();
void send_Motor_Data(float M0_angle,float M0_speed,float M1_angle,float M1_speed);
struct SF_Serial_DATA {
float M0_Target;
float M1_Target;
float M0_Mode;
float M1_Mode;
float M0_P;
float M0_I;
float M0_D;
float M1_P;
float M1_I;
float M1_D;
float M0_angle;
float M0_speed;
float M1_angle;
float M1_speed;
};
SF_Serial_DATA Serial_Data(void);
#endif

View File

@ -0,0 +1,29 @@
#include "SF_BLDC.h"
#include "SF_Serial.h"
#include "pid.h"
SF_BLDC_DATA BLDCData;
SF_BLDC motors = SF_BLDC(Serial2); //定义与S1通信的对象
SF_Serial_DATA receive_openmv; //定义openmv接收数据对象
void setup() {
Serial.begin(921600);
Serial1.begin(921600, SERIAL_8N1, 36, 37); //openmv
motors.init();
motors.setModes(2,2);
}
float M1_angle;
float M0_angle;
void loop() {
BLDCData = motors.getBLDCData();
M1_angle = BLDCData.M1_Angle; //接收Wroom电机运行速度到 driver 对象
M0_angle = BLDCData.M0_Angle;
receive_openmv = Serial_Data();
if( Serial_Contact() == 1){
motors.setTargets(M0_angle,M1_angle - receive_openmv.M1_Target);
}
}

View File

@ -0,0 +1,11 @@
This directory is intended for PlatformIO Test Runner and project tests.
Unit Testing is a software testing method by which individual units of
source code, sets of one or more MCU program modules together with associated
control data, usage procedures, and operating procedures, are tested to
determine whether they are fit for use. Unit testing finds problems early
in the development cycle.
More information about PlatformIO Unit Testing:
- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html

View File

@ -0,0 +1 @@
请将文件夹【comm_with_OMV】放置在纯英文路径下并用platformio打开工程项目

BIN
模型/底盖.STL Normal file

Binary file not shown.

BIN
模型/座.STL Normal file

Binary file not shown.