diff --git a/例程/色块识别/OpenMV端/SF_Serial.py b/例程/色块识别/OpenMV端/SF_Serial.py new file mode 100644 index 0000000..415d5e3 --- /dev/null +++ b/例程/色块识别/OpenMV端/SF_Serial.py @@ -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 diff --git a/例程/色块识别/OpenMV端/main.py b/例程/色块识别/OpenMV端/main.py new file mode 100644 index 0000000..a5f0e2f --- /dev/null +++ b/例程/色块识别/OpenMV端/main.py @@ -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 amthigh: + 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 + + + diff --git a/例程/色块识别/StackForce端/comm_with_OMV/.gitignore b/例程/色块识别/StackForce端/comm_with_OMV/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/例程/色块识别/StackForce端/comm_with_OMV/.vscode/extensions.json b/例程/色块识别/StackForce端/comm_with_OMV/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/.vscode/extensions.json @@ -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" + ] +} diff --git a/例程/色块识别/StackForce端/comm_with_OMV/.vscode/settings.json b/例程/色块识别/StackForce端/comm_with_OMV/.vscode/settings.json new file mode 100644 index 0000000..29308c6 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "functional": "cpp" + } +} \ No newline at end of file diff --git a/例程/色块识别/StackForce端/comm_with_OMV/include/README b/例程/色块识别/StackForce端/comm_with_OMV/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/include/README @@ -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 diff --git a/例程/色块识别/StackForce端/comm_with_OMV/lib/README b/例程/色块识别/StackForce端/comm_with_OMV/lib/README new file mode 100644 index 0000000..2593a33 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/lib/README @@ -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 +#include + +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 diff --git a/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC.h b/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC.h new file mode 100644 index 0000000..0e43dd4 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC.h @@ -0,0 +1,54 @@ +#ifndef _SF_BLDC_h +#define _SF_BLDC_h + +#include +#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 \ No newline at end of file diff --git a/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC_shared_struct.h b/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC_shared_struct.h new file mode 100644 index 0000000..b3b6530 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/SF_BLDC_shared_struct.h @@ -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 diff --git a/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/libSF_BLDC.a b/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/libSF_BLDC.a new file mode 100644 index 0000000..3811cdf Binary files /dev/null and b/例程/色块识别/StackForce端/comm_with_OMV/lib/SF_BLDC/libSF_BLDC.a differ diff --git a/例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.cpp b/例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.cpp new file mode 100644 index 0000000..6ae2c3c --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.cpp @@ -0,0 +1,61 @@ +#include "pid.h" +#include +#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; +} diff --git a/例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.h b/例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.h new file mode 100644 index 0000000..6f73741 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/lib/pid/pid.h @@ -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 \ No newline at end of file diff --git a/例程/色块识别/StackForce端/comm_with_OMV/platformio.ini b/例程/色块识别/StackForce端/comm_with_OMV/platformio.ini new file mode 100644 index 0000000..f3ca1d0 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/platformio.ini @@ -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 diff --git a/例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.cpp b/例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.cpp new file mode 100644 index 0000000..b522a0a --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.cpp @@ -0,0 +1,73 @@ +#include +#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); +} + diff --git a/例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.h b/例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.h new file mode 100644 index 0000000..9262961 --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/src/SF_Serial.h @@ -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 + + + diff --git a/例程/色块识别/StackForce端/comm_with_OMV/src/main.cpp b/例程/色块识别/StackForce端/comm_with_OMV/src/main.cpp new file mode 100644 index 0000000..086a0cf --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/src/main.cpp @@ -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); + + } +} diff --git a/例程/色块识别/StackForce端/comm_with_OMV/test/README b/例程/色块识别/StackForce端/comm_with_OMV/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/例程/色块识别/StackForce端/comm_with_OMV/test/README @@ -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 diff --git a/例程/色块识别/StackForce端/读我.txt b/例程/色块识别/StackForce端/读我.txt new file mode 100644 index 0000000..96ca2a6 --- /dev/null +++ b/例程/色块识别/StackForce端/读我.txt @@ -0,0 +1 @@ +请将文件夹【comm_with_OMV】放置在纯英文路径下,并用platformio打开工程项目 \ No newline at end of file diff --git a/模型/底盖.STL b/模型/底盖.STL new file mode 100644 index 0000000..efec29f Binary files /dev/null and b/模型/底盖.STL differ diff --git a/模型/座.STL b/模型/座.STL new file mode 100644 index 0000000..b3cbc97 Binary files /dev/null and b/模型/座.STL differ