mirror of
https://gitee.com/StackForce/stackforce_openmv
synced 2025-09-26 22:19:11 +08:00
update
This commit is contained in:
parent
fdf202a04c
commit
ec9c10c613
34
例程/色块识别/OpenMV端/SF_Serial.py
Normal file
34
例程/色块识别/OpenMV端/SF_Serial.py
Normal 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
|
118
例程/色块识别/OpenMV端/main.py
Normal file
118
例程/色块识别/OpenMV端/main.py
Normal 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
|
||||||
|
|
||||||
|
|
||||||
|
|
5
例程/色块识别/StackForce端/comm_with_OMV/.gitignore
vendored
Normal file
5
例程/色块识别/StackForce端/comm_with_OMV/.gitignore
vendored
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
.pio
|
||||||
|
.vscode/.browse.c_cpp.db*
|
||||||
|
.vscode/c_cpp_properties.json
|
||||||
|
.vscode/launch.json
|
||||||
|
.vscode/ipch
|
10
例程/色块识别/StackForce端/comm_with_OMV/.vscode/extensions.json
vendored
Normal file
10
例程/色块识别/StackForce端/comm_with_OMV/.vscode/extensions.json
vendored
Normal 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"
|
||||||
|
]
|
||||||
|
}
|
5
例程/色块识别/StackForce端/comm_with_OMV/.vscode/settings.json
vendored
Normal file
5
例程/色块识别/StackForce端/comm_with_OMV/.vscode/settings.json
vendored
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
{
|
||||||
|
"files.associations": {
|
||||||
|
"functional": "cpp"
|
||||||
|
}
|
||||||
|
}
|
39
例程/色块识别/StackForce端/comm_with_OMV/include/README
Normal file
39
例程/色块识别/StackForce端/comm_with_OMV/include/README
Normal 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
|
46
例程/色块识别/StackForce端/comm_with_OMV/lib/README
Normal file
46
例程/色块识别/StackForce端/comm_with_OMV/lib/README
Normal 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
|
54
例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC.h
Normal file
54
例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC.h
Normal 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
|
@ -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
|
BIN
例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/libSF_BLDC.a
Normal file
BIN
例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/libSF_BLDC.a
Normal file
Binary file not shown.
61
例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.cpp
Normal file
61
例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.cpp
Normal 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;
|
||||||
|
}
|
37
例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.h
Normal file
37
例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.h
Normal 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
|
18
例程/色块识别/StackForce端/comm_with_OMV/platformio.ini
Normal file
18
例程/色块识别/StackForce端/comm_with_OMV/platformio.ini
Normal 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
|
73
例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.cpp
Normal file
73
例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.cpp
Normal 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);
|
||||||
|
}
|
||||||
|
|
35
例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.h
Normal file
35
例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.h
Normal 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
|
||||||
|
|
||||||
|
|
||||||
|
|
29
例程/色块识别/StackForce端/comm_with_OMV/src/main.cpp
Normal file
29
例程/色块识别/StackForce端/comm_with_OMV/src/main.cpp
Normal 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);
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
11
例程/色块识别/StackForce端/comm_with_OMV/test/README
Normal file
11
例程/色块识别/StackForce端/comm_with_OMV/test/README
Normal 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
|
1
例程/色块识别/StackForce端/读我.txt
Normal file
1
例程/色块识别/StackForce端/读我.txt
Normal file
@ -0,0 +1 @@
|
|||||||
|
请将文件夹【comm_with_OMV】放置在纯英文路径下,并用platformio打开工程项目
|
Loading…
Reference in New Issue
Block a user