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