Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests Actions Packages Projects Releases Wiki Activity

【类 型】：style

Browse Source 
【主	题】：代码格式化 较上版本 无任何变动
【描	述】：
	[原因]：
	[过程]：
	[影响]：
【结	束】

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
This commit is contained in:
tk 2024-07-02 18:24:51 +08:00
parent f3583e17e5
commit 811a89f8b2
9 changed files with 1453 additions and 1317 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

707
src/FoodDeliveryBase.cpp
View File

File diff suppressed because it is too large Load Diff

142
src/FoodDeliveryBase.h
View File

@ -15,28 +15,29 @@
#include "WiFiUdp.h"
#define MAVLINK_SYSTEM_ID 0xFF
#define MAVLINK_COMPONENT_ID 0xBE
class FoodCube {
#define MAVLINK_COMPONENT_ID 0xBE
class FoodCube
{
public:
/*飞行航点任务相关属性*/
bool writeState = false; //是否是写入状态
int8_t writeSeq = -1; //飞控反馈 需写入航点序列号
int8_t futureSeq = 0; //记录将来要写入 航点序列号
int8_t missionArkType = -1; //航点写入是否成功
bool writeState = false; // 是否是写入状态
int8_t writeSeq = -1; // 飞控反馈 需写入航点序列号
int8_t futureSeq = 0; // 记录将来要写入 航点序列号
int8_t missionArkType = -1; // 航点写入是否成功
/*航点任务 送餐信息喊话*/
String questVoiceStr;
/*前端模拟遥控的油门通道*/
uint16_t channels[4] = { 1500, 1500, 1500, 1500 };
uint16_t channels[4] = {1500, 1500, 1500, 1500};
/*初始化*/
FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial,char* userUdpServerIP, uint32_t userUdpServerPort);
FoodCube(char *userSsid, char *userPassword, char *userMqttServer, int userMqttPort, char *userMqttName, char *userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char *userUdpServerIP, uint32_t userUdpServerPort);
/*日志打印*/
void log(String val);
void log(char* val);
void log(char *val);
void log(int val);
void log(IPAddress val);
void log(bool val);
void logln(String val);
void logln(char* val);
void logln(char *val);
void logln(int val);
void logln(IPAddress val);
void logln(bool val);
@ -47,7 +48,7 @@ public:
/*wifi*/
void connectWifi();
/*mqtt*/
PubSubClient* mqttClient; //指向 mqtt服务器连接 对象
PubSubClient *mqttClient; // 指向 mqtt服务器连接 对象
void connectMqtt(String topicSub[], int topicSubCount);
void mqttLoop(String topicSub[], int topicSubCount);
void subscribeTopic(String topicString, int Qos);
@ -55,7 +56,7 @@ public:
/*串口输出*/
void SWrite(uint8_t buf[], int len, uint8_t swSerial);
/*声音模块控制*/
void playText(String str,bool flying=false);
void playText(String str, bool flying = false);
void SetplayvolMax();
uint8_t chekVoiceMcu();
void stopVoice();
@ -69,74 +70,71 @@ public:
void mav_command(uint8_t controlType, uint16_t param[]);
void mav_send_command(mavlink_command_long_t msg_cmd);
void mav_channels_override(uint16_t chan[]);
void mav_send_text(uint8_t severity,const char *text);
void mav_send_text(uint8_t severity, const char *text);
/*云台相机控制*/
void udpSendToCamera(uint8_t* p_command, uint32_t len);
void udpSendToCamera(uint8_t *p_command, uint32_t len);
void Camera_action_down();
void Camera_action_ret();
void Camera_action_move(uint8_t vpitch,uint8_t vyaw );
void Camera_action_zoom(uint8_t vzoom ) ;
void Camera_action_move(uint8_t vpitch, uint8_t vyaw);
void Camera_action_zoom(uint8_t vzoom);
bool checkWiFiStatus();
private:
char* ssid; //wifi帐号
char* password; //wifi密码
char* mqttServer; //mqtt服务器地址
int mqttPort; //mqtt服务器端口
char* mqttName; //mqtt帐号
char* mqttPassword; //mqtt密码
uint8_t mavlinkSerial; //飞控占用的串口号
uint8_t voiceSerial; //飞控占用的串口号
bool isInit = true; //用来判断接收到第一次心跳时 重新向飞控 发送一个请求数据类型
char *ssid; // wifi帐号
char *password; // wifi密码
char *mqttServer; // mqtt服务器地址
int mqttPort; // mqtt服务器端口
char *mqttName; // mqtt帐号
char *mqttPassword; // mqtt密码
uint8_t mavlinkSerial; // 飞控占用的串口号
uint8_t voiceSerial; // 飞控占用的串口号
bool isInit = true; // 用来判断接收到第一次心跳时 重新向飞控 发送一个请求数据类型
WiFiClient wifiClient; //网络客户端
IPAddress localIp; //板子的IP地址
String macAdd; //板子的物理地址(已去掉冒号分隔符)
bool wificonnected; //网络是否连接
/*云台相机控制*/
WiFiUDP udp; //udp信息操作对象
char* udpServerIP; //云台相机ip地址
uint32_t udpServerPort; //云台相机端口
unsigned long _tm_mqttconnect; //mqtt上次连接时间
WiFiClient wifiClient; // 网络客户端
IPAddress localIp; // 板子的IP地址
String macAdd; // 板子的物理地址(已去掉冒号分隔符)
bool wificonnected; // 网络是否连接
/*云台相机控制*/
WiFiUDP udp; // udp信息操作对象
char *udpServerIP; // 云台相机ip地址
uint32_t udpServerPort; // 云台相机端口
unsigned long _tm_mqttconnect; // mqtt上次连接时间
//摄像头控制 校验代码
const uint16_t crc16_tab[256] = { 0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
0x1231, 0x210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
0x2462, 0x3443, 0x420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
0x3653, 0x2672, 0x1611, 0x630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840, 0x1861, 0x2802, 0x3823,
0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50, 0x3a33, 0x2a12,
0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0xc60, 0x1c41,
0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0xe70,
0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
0x1080, 0xa1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
0x2b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
0x34e2, 0x24c3, 0x14a0, 0x481, 0x7466, 0x6447, 0x5424, 0x4405,
0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
0x26d3, 0x36f2, 0x691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1, 0x3882, 0x28a3,
0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1, 0x1ad0, 0x2ab3, 0x3a92,
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1,
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1, 0x1ef0 };
uint16_t CRC16_cal(uint8_t* ptr, uint32_t len, uint16_t crc_init);
void crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result);
// 摄像头控制 校验代码
const uint16_t crc16_tab[256] = {0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
0x1231, 0x210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
0x2462, 0x3443, 0x420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
0x3653, 0x2672, 0x1611, 0x630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840, 0x1861, 0x2802, 0x3823,
0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50, 0x3a33, 0x2a12,
0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0xc60, 0x1c41,
0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0xe70,
0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
0x1080, 0xa1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
0x2b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
0x34e2, 0x24c3, 0x14a0, 0x481, 0x7466, 0x6447, 0x5424, 0x4405,
0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
0x26d3, 0x36f2, 0x691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1, 0x3882, 0x28a3,
0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1, 0x1ad0, 0x2ab3, 0x3a92,
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1,
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0xed1, 0x1ef0};
uint16_t CRC16_cal(uint8_t *ptr, uint32_t len, uint16_t crc_init);
void crc_check_16bites(uint8_t *pbuf, uint32_t len, uint16_t *p_result);
};
#endif

1123
src/commser.cpp
View File

File diff suppressed because it is too large Load Diff

14
src/commser.h
View File

@ -6,16 +6,16 @@
#include "FoodDeliveryBase.h"
#include "config.h"
void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length);
void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length);
void mavThread();
void pubThread();
void flashThread() ;
void flashThread();
void writeRoute(String topicStr);
void mavlink_receiveCallback(uint8_t c) ;
void mavlink_receiveCallback(uint8_t c);
extern String topicPub[];
extern int topicPubCount;
extern FoodCube fc; //创建项目对象
extern Ticker pubTicker; //定时发布主题 线程
extern Ticker mavTicker; //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
extern Ticker flashTicker; //单片机主动 按钮主动发布主题 线程
extern FoodCube fc; // 创建项目对象
extern Ticker pubTicker; // 定时发布主题 线程
extern Ticker mavTicker; // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
extern Ticker flashTicker; // 单片机主动 按钮主动发布主题 线程
#endif

49
src/config.h
View File

@ -3,8 +3,8 @@
// 定义公共结构，变量，硬件接口等
///
//
#define VERSION "0.90" //软件版本
#define VERSION_HW 1 //硬件版本1:第一块硬件 2:目前版本
#define VERSION "0.90" // 软件版本
#define VERSION_HW 1 // 硬件版本1:第一块硬件 2:目前版本
// 硬件接口定义////////////////////////////
// 按钮
#define BTN_UP 23 // 收线开关 接线：BTN_UP---GND
@ -12,37 +12,36 @@
#define BTN_CT 21 // 到顶检测开关
#define BTN_TEST 18 // 测试开关
// 称重传感器- HX711
#define LOADCELL_DOUT_PIN 13 //16
#define LOADCELL_SCK_PIN 33 //17
#define LOADCELL_DOUT_PIN 13 // 16
#define LOADCELL_SCK_PIN 33 // 17
///////////////////////////////////////////
#define SERVO_PIN 14 // 锁定舵机PWM控制脚
////LED
#define LED_DATA_PIN 25
#if (VERSION_HW == 1)
// Moto-CAN //第一版本硬件参数---1号机使用
#define MOTO_CAN_RX 26
#define MOTO_CAN_TX 27
#define WEIGHT_SCALE 165 // //A通道是165,B通道是41
#define HX711_GAIN 128
// Moto-CAN //第一版本硬件参数---1号机使用
#define MOTO_CAN_RX 26
#define MOTO_CAN_TX 27
#define WEIGHT_SCALE 165 // //A通道是165,B通道是41
#define HX711_GAIN 128
#else
// Moto-CAN //第二版本硬件参数---2号机使用
#define MOTO_CAN_RX 27 //PCB画板需要，做了调整
#define MOTO_CAN_TX 26 //PCB画板需要，做了调整
#define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41
#define HX711_GAIN 32 //减少零点漂移用B通道的感度
// Moto-CAN //第二版本硬件参数---2号机使用
#define MOTO_CAN_RX 27 // PCB画板需要，做了调整
#define MOTO_CAN_TX 26 // PCB画板需要，做了调整
#define WEIGHT_SCALE 41 // 减少零点漂移用B通道是41
#define HX711_GAIN 32 // 减少零点漂移用B通道的感度
#endif
///serial1
/// serial1
#define SERIAL_REPORT_TX 5
#define SERIAL_REPORT_RX 18
/////
//#define WEIGHT_SCALE 41 // //A通道是165,B通道是41
#define TM_INSTORE_WEIGHT_DELAY 200 //200 // 入仓动力延时关闭时间 ms
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
// #define WEIGHT_SCALE 41 // //A通道是165,B通道是41
#define TM_INSTORE_WEIGHT_DELAY 200 // 200 // 入仓动力延时关闭时间 ms
#define TM_INSTORE_DELAY_WEIGHT 200 // 入仓动力延时关闭时间生效的重量(g)，免得空钩也延时关闭导致线拉紧 ms
#define TM_INSTORE_NOWEIGHT_DELAY 50 // 空钩完全不延时也可能顶部按键按不下去，加一点延时 ms
enum HookStatus
{
HS_UnInit, // 还未初始化
@ -67,9 +66,9 @@ enum Initstatus
IS_CheckWeightZero, // 检查称重传感器是否归0
IS_OK, // 4.所有初始化已经OK，可以正常操作
IS_Error // 5.初始化遇到错误
} ;
const uint16_t MAV_CMD_FC_HOOK_AUTODOWN=40; //飞控发的---自动放线
const uint16_t MAV_CMD_FC_HOOK_PAUSE=41; //飞控发的---暂停
const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态
const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下
};
const uint16_t MAV_CMD_FC_HOOK_AUTODOWN = 40; // 飞控发的---自动放线
const uint16_t MAV_CMD_FC_HOOK_PAUSE = 41; // 飞控发的---暂停
const uint16_t MAV_CMD_FC_HOOK_STATUS = 42; // 发给飞控的状态
const uint16_t MAV_CMD_FC_HOOK_RECOVERY = 43; // 飞控发的---收线
const uint16_t MAV_CMD_FC_DESCENTSTART = 44; // 飞开始下降，需要调整相机向下

556
src/main.cpp
View File

@ -36,7 +36,7 @@ Motocontrol motocontrol;
void led_show(uint8_t cr, uint8_t cg, uint8_t cb);
void sendinfo();
Ticker tksendinfo(sendinfo, 1000); // 发送状态
// 收
void upbtn_click();
void upbtn_dbclick();
@ -57,23 +57,23 @@ void btn_presssatonce();
int get_pullweight();
void Task1(void *pvParameters);
//void led_show(CRGB ledcolor);
// void Task1code( void *pvParameters );
// void led_show(CRGB ledcolor);
// void Task1code( void *pvParameters );
void showledidel();
int pullweight = 0; //检测重量-克
int pullweightoktimes=0; //校准成功次数
int8_t btn_pressatonce=0; //是否同时按下
int pullweight = 0; // 检测重量-克
int pullweightoktimes = 0; // 校准成功次数
int8_t btn_pressatonce = 0; // 是否同时按下
unsigned long _tm_bengstop;
bool _bengstop = false;
bool _needweightalign = false;
HookStatus _lasthooktatus=HS_UnInit; //前一个钩子状态
bool curr_armed=false;
uint8_t curr_mode=0;
bool _checkweightcal=false; //检测是否要检测称重传感器是否需要校准
uint8_t _checkweighttimes=0; //
HookStatus _lasthooktatus = HS_UnInit; // 前一个钩子状态
bool curr_armed = false;
uint8_t curr_mode = 0;
bool _checkweightcal = false; // 检测是否要检测称重传感器是否需要校准
uint8_t _checkweighttimes = 0; //
unsigned long _tm_checkweigh;
static const char* MOUDLENAME = "MAIN";
static const char *MOUDLENAME = "MAIN";
// 称重校准状态
enum Weightalign_status
{
@ -82,18 +82,12 @@ enum Weightalign_status
WAS_AlignOK,
WAS_Alignfault
} _weightalign_status;
unsigned long _weightalign_begintm; //校准开始时间
unsigned long _weightalign_begintm; // 校准开始时间
// 需要做的初始化工作
Initstatus initstatus;
unsigned long _tm_waitinit;
void setup()
{
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
@ -102,13 +96,10 @@ void setup()
// 调试串口
Serial.begin(57600);
ESP_LOGI(MOUDLENAME,"Starting PullupDevice... Ver:%s",VERSION);
ESP_LOGI(MOUDLENAME, "Starting PullupDevice... Ver:%s", VERSION);
preferences.begin("PullupDev", false);
wei_offset = preferences.getLong("wei_offset", 0);
ESP_LOGD(MOUDLENAME,"wei_offset:%d",wei_offset);
ESP_LOGD(MOUDLENAME, "wei_offset:%d", wei_offset);
// 初始化按钮
button_up.attachClick(upbtn_click);
@ -137,41 +128,38 @@ void setup()
// 初始化RGB LED 显示黄色灯表示需要注意 LED
FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
if (!motocontrol.init(&myservo)) // 初始化电机控制
ESP_LOGE(MOUDLENAME,"motocontrol init fault");
ESP_LOGE(MOUDLENAME, "motocontrol init fault");
tksendinfo.start();
initstatus = IS_WaitStart;
_tm_waitinit = millis();
_needweightalign = (wei_offset==0);
led_show(255,255,255);// 连接wifi中
_needweightalign = (wei_offset == 0);
led_show(255, 255, 255); // 连接wifi中
/////////////////////////////////MQTT_语音_MAVLINK 部分
/*初始化*/
Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); //声音模块引串口脚映射
Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); // 声音模块引串口脚映射
fc.playText("开始启动");
fc.connectWifi(); //连接wifi
fc.connectWifi(); // 连接wifi
fc.playText("正在连接服务器");
fc.connectMqtt(topicPub, topicPubCount); //连接mqtt
fc.mqttClient->setCallback(mqtt_receiveCallback); //设置订阅成功 回调
fc.mav_request_data(); //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
fc.connectMqtt(topicPub, topicPubCount); // 连接mqtt
fc.mqttClient->setCallback(mqtt_receiveCallback); // 设置订阅成功 回调
fc.mav_request_data(); // 指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
/*异步线程*/
pubTicker.start(); //定时 发布主题
mavTicker.start(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
flashTicker.start(); //监听 按flash键时 主动发布对频主题
pubTicker.start(); // 定时 发布主题
mavTicker.start(); // 定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
flashTicker.start(); // 监听 按flash键时 主动发布对频主题
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
// if (motocontrol.getstatus()==MS_Stop)
// {
// //slowup();
// }
ESP_LOGI(MOUDLENAME,"PullupDevice is ready!");
// fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
ESP_LOGI(MOUDLENAME, "PullupDevice is ready!");
// fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
}
void slowup()
{
@ -180,106 +168,108 @@ void slowup()
}
void checkstatus()
{
HookStatus vhooktatus= motocontrol.gethooktatus();
//入仓把云台回中
if ((_lasthooktatus!=vhooktatus)&&(vhooktatus==HS_InStore))
fc.Camera_action_ret();
HookStatus vhooktatus = motocontrol.gethooktatus();
// 入仓把云台回中
if ((_lasthooktatus != vhooktatus) && (vhooktatus == HS_InStore))
fc.Camera_action_ret();
if (_checkweightcal && (vhooktatus==HS_Stop))
if (_checkweightcal && (vhooktatus == HS_Stop))
{
//第一次进来
if (_lasthooktatus!=vhooktatus)
_tm_checkweigh=millis();
// 第一次进来
if (_lasthooktatus != vhooktatus)
_tm_checkweigh = millis();
else
{ //1秒内检测连续大于5次就认为重了
if ((millis()-_tm_checkweigh)<1000)
{
if (abs(pullweight) > 100)
_checkweighttimes++;
else _checkweighttimes=0;
//ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
if (_checkweighttimes>10)
{
_checkweightcal=false;
//ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量");
}
}else
{
_checkweightcal=false;
{ // 1秒内检测连续大于5次就认为重了
if ((millis() - _tm_checkweigh) < 1000)
{
if (abs(pullweight) > 100)
_checkweighttimes++;
else
_checkweighttimes = 0;
// ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
if (_checkweighttimes > 10)
{
_checkweightcal = false;
// ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量");
}
}
else
{
_checkweightcal = false;
}
}
}
_lasthooktatus=vhooktatus;
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
_lasthooktatus = vhooktatus;
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
}
//1秒调用一次了发mqtt到地面
// 1秒调用一次了发mqtt到地面
void showinfo()
{
// moto_measure_t mf=motocontrol.getmotoinfo() ;
// printf("total_cnt:%.3f;speed:%.2f,curr:%.2fA \n ", mf.output_round_cnt, mf.output_speed_rpm,(float)mf.real_current/1000.0f);
// if (pullweight > 10)
// printf("PullWeight:%d\n", pullweight); //发送重量到mqtt
// printf("PullWeight:%d\n", pullweight); //发送重量到mqtt
// topicPubMsg[14]=motocontrol.gethooktatus_str() ;
// topicPubMsg[13]=pullweight;
//topicPubMsg[14]=motocontrol.gethooktatus_str() ;
//topicPubMsg[13]=pullweight;
// control_status_t cs=motocontrol.getcontrolstatus() ;
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
HookStatus vhooktatus= motocontrol.gethooktatus();
HookStatus vhooktatus = motocontrol.gethooktatus();
mavlink_command_long_t msg_cmd;
msg_cmd.command=MAV_CMD_FC_HOOK_STATUS;
msg_cmd.param1=vhooktatus;
msg_cmd.param2=pullweight;
msg_cmd.command = MAV_CMD_FC_HOOK_STATUS;
msg_cmd.param1 = vhooktatus;
msg_cmd.param2 = pullweight;
msg_cmd.target_system = 1;
msg_cmd.target_component = 1;
msg_cmd.confirmation = 0;
fc.mav_send_command(msg_cmd);
}
//校准称重
// 校准称重
void begin_tare()
{
ESP_LOGD(MOUDLENAME,"begin_tare");
_weightalign_status=WAS_Aligning;
ESP_LOGD(MOUDLENAME, "begin_tare");
_weightalign_status = WAS_Aligning;
_needweightalign = true;
_weightalign_begintm=millis();
_weightalign_begintm = millis();
}
//检查校准结果
// 检查校准结果
bool check_tare()
{
if (_weightalign_status!=WAS_Aligning)
return false;
//超时失败
if ((millis()-_weightalign_begintm)>2000)
if (_weightalign_status != WAS_Aligning)
return false;
// 超时失败
if ((millis() - _weightalign_begintm) > 2000)
{
_weightalign_status=WAS_Alignfault;
_weightalign_status = WAS_Alignfault;
return false;
}
if ((pullweight < 10)&&((pullweight >-10)))
if ((pullweight < 10) && ((pullweight > -10)))
{
pullweightoktimes++;
if (pullweightoktimes>20)
if (pullweightoktimes > 20)
{
_needweightalign = false;
_weightalign_status=WAS_AlignOK;
wei_offset=scale.get_offset();
_weightalign_status = WAS_AlignOK;
wei_offset = scale.get_offset();
preferences.putLong("wei_offset", wei_offset);
motocontrol.weightalign(true);
ESP_LOGD(MOUDLENAME,"check_tare ok: %d,offset:%d", pullweight,wei_offset);
ESP_LOGD(MOUDLENAME, "check_tare ok: %d,offset:%d", pullweight, wei_offset);
return true;
}else _needweightalign = true;
}
else
_needweightalign = true;
}
else
{
// 没成功继续校准
ESP_LOGD(MOUDLENAME,"weight not zero: %d", pullweight);
ESP_LOGD(MOUDLENAME, "weight not zero: %d", pullweight);
_needweightalign = true;
pullweightoktimes=0;
pullweightoktimes = 0;
}
return false;
}
@ -299,18 +289,18 @@ void checkinited()
case Initstatus::IS_Start:
{
//一开始没有锁定状态
// 一开始没有锁定状态
if (motocontrol.gethooktatus() != HS_Locked)
{
// 开始自动慢速上升，直到顶部按钮按下
ESP_LOGD(MOUDLENAME,"IS_Start: startup wait locked");
ESP_LOGD(MOUDLENAME, "IS_Start: startup wait locked");
motocontrol.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_Wait_Locked;
}
else //开机就是锁定状态--开始校准称重传感器
else // 开机就是锁定状态--开始校准称重传感器
{
initstatus = IS_begin_WeightZero;
initstatus = IS_begin_WeightZero;
}
break;
}
@ -318,47 +308,47 @@ void checkinited()
{
if (motocontrol.gethooktatus() == HS_Locked)
{
ESP_LOGD(MOUDLENAME,"IS_Wait_LengthZero: is locked");
ESP_LOGD(MOUDLENAME, "IS_Wait_LengthZero: is locked");
initstatus = IS_begin_WeightZero;
}
break;
}
case Initstatus::IS_begin_WeightZero :
case Initstatus::IS_begin_WeightZero:
{
//如果没有保存的校准数据就需要校准
if (wei_offset!=0)
// 如果没有保存的校准数据就需要校准
if (wei_offset != 0)
{
ESP_LOGD(MOUDLENAME,"offset is loaded weight: %d,offset:%d", pullweight,wei_offset);
ESP_LOGD(MOUDLENAME, "offset is loaded weight: %d,offset:%d", pullweight, wei_offset);
scale.set_offset(wei_offset);
motocontrol.weightalign(true);
_needweightalign = false;
// fc.playText("挂钩已锁定");
// fc.playText("挂钩已锁定");
initstatus = IS_OK;
}else
}
else
{
ESP_LOGD(MOUDLENAME,"begin_tare");
ESP_LOGD(MOUDLENAME, "begin_tare");
begin_tare();
initstatus = IS_CheckWeightZero;
}
break;
}
case Initstatus::IS_CheckWeightZero :
case Initstatus::IS_CheckWeightZero:
{
if (_weightalign_status==WAS_AlignOK)
if (_weightalign_status == WAS_AlignOK)
{
// fc.playText("挂钩已锁定");
// fc.playText("挂钩已锁定");
initstatus = IS_OK;
}else
if (_weightalign_status==WAS_Alignfault)
}
else if (_weightalign_status == WAS_Alignfault)
{
ESP_LOGD(MOUDLENAME,"weightalign fault! again");
ESP_LOGD(MOUDLENAME, "weightalign fault! again");
initstatus = IS_begin_WeightZero;
}
break;
}
/*
case Initstatus::IS_LengthZero:
{
@ -410,26 +400,25 @@ void set_locked(bool locked)
// 在核心1上执行，重要的延迟低的
void loop()
{
tksendinfo.update(); // 定时发送信息任务
//pubTicker.update(); //定时 发布主题
//mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
// sercomm.getcommand(); // 得到控制命令
tksendinfo.update(); // 定时发送信息任务
// pubTicker.update(); //定时 发布主题
// mavTicker.update(); //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
// sercomm.getcommand(); // 得到控制命令
button_checktop.tick(); // 按钮
button_down.tick(); // 按钮
button_up.tick(); // 按钮
button_test.tick();
motocontrol.setweight(pullweight); // 告诉电机拉的重量
motocontrol.update(); // 电机控制
showledidel(); // 显示LED灯光
checkstatus(); //检测状态，执行一些和状态有关的功能
//showinfo(); // 显示一些调试用信息-
// 到顶后延迟关闭动力电和舵机
showledidel(); // 显示LED灯光
checkstatus(); // 检测状态，执行一些和状态有关的功能
// showinfo(); // 显示一些调试用信息-
// 到顶后延迟关闭动力电和舵机
if (_bengstop)
{
if ((initstatus==IS_OK)&&(pullweight>TM_INSTORE_DELAY_WEIGHT) )
if ((initstatus == IS_OK) && (pullweight > TM_INSTORE_DELAY_WEIGHT))
{
if ((millis() - _tm_bengstop) > TM_INSTORE_WEIGHT_DELAY)
{
@ -437,7 +426,7 @@ void loop()
set_locked(true);
}
}
else
else
{
if ((millis() - _tm_bengstop) > TM_INSTORE_NOWEIGHT_DELAY)
{
@ -446,25 +435,24 @@ void loop()
}
}
}
check_tare(); //检查看是否需要校准称重
check_tare(); // 检查看是否需要校准称重
// 检测执行初始化工作
checkinited();
/////////////////////////////////MQTT_语音_MAVLINK 部分
/*从飞控拿数据*/
fc.comm_receive(mavlink_receiveCallback);
/*保持mqtt心跳*/
/*保持mqtt心跳*/
fc.mqttLoop(topicPub, topicPubCount);
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
delay(1);
}
// 在核心0上执行耗时长的低优先级的
void Task1(void *pvParameters)
{
// 初始化称重传感器
scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN,HX711_GAIN); //2号机用的B通道,1号用的A通道
scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f
scale.tare(); // 重置为0
scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN, HX711_GAIN); // 2号机用的B通道,1号用的A通道
scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f
scale.tare(); // 重置为0
// 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了）
while (1)
@ -475,22 +463,22 @@ void Task1(void *pvParameters)
scale.tare();
}
pullweight = get_pullweight();
//显示重量
//printf("pullweight: %d \n", pullweight);
// 显示重量
// printf("pullweight: %d \n", pullweight);
/*保持mqtt心跳*/
// fc.mqttLoop(topicSub, topicSubCount);
///px1
// if (fc.checkWiFiStatus())
/*保持mqtt心跳,如果Mqtt没有连接会自动连接*/
// fc.mqttLoop(topicSub, topicSubCount);
// fc.mqttLoop(topicSub, topicSubCount);
/// px1
// if (fc.checkWiFiStatus())
/*保持mqtt心跳,如果Mqtt没有连接会自动连接*/
// fc.mqttLoop(topicSub, topicSubCount);
vTaskDelay(10);
}
}
void sendinfo() // 每500ms发送状态信息
{
//sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
// sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
showinfo();
}
@ -505,7 +493,7 @@ int get_pullweight()
else
NewValue = 0;
Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波
Value = constrain(Value, -10000, 10000); // 限制到0-10公斤
Value = constrain(Value, -10000, 10000); // 限制到0-10公斤
return round(Value);
}
// 提示灯光控制
@ -523,98 +511,96 @@ void showledidel()
led_show(255, 0, 255); // 紫色
return;
}
switch (motocontrol.gethooktatus())
switch (motocontrol.gethooktatus())
{
case HookStatus::HS_UnInit:
{
led_show(255, 255, 0); // 黄色
break;
}
case HookStatus::HS_Down :
case HookStatus::HS_DownSlow:
case HookStatus::HS_WaitUnhook:
{
led_show(0, 0, 255); // 蓝色
break;
}
case HookStatus::HS_Up:
{
led_show(200, 0, 50); // 粉红
break;
}
case HookStatus::HS_InStore:
{
led_show(255, 0, 0); // 红
break;
}
case HookStatus::HS_Locked:
{
led_show(0, 255, 0); // 绿色
break;
}
case HookStatus::HS_Stop:
{
led_show(255, 255, 255); // 白色
break;
}
case HookStatus::HS_UnInit:
{
led_show(255, 255, 0); // 黄色
break;
}
/*
switch (motocontrol.getcontrolstatus().motostatus)
{
case MotoStatus::MS_Down:
case HookStatus::HS_Down:
case HookStatus::HS_DownSlow:
case HookStatus::HS_WaitUnhook:
{
led_show(0, 0, 255); // 蓝色
break;
}
case MotoStatus::MS_Up:
case HookStatus::HS_Up:
{
if (motocontrol.getcontrolstatus().is_instore)
led_show(255, 0, 0); // 红
else
led_show(200, 0, 50); // 粉红
led_show(200, 0, 50); // 粉红
break;
}
case HookStatus::HS_InStore:
{
led_show(255, 0, 0); // 红
break;
}
case MotoStatus::MS_Stop:
case HookStatus::HS_Locked:
{
if (motocontrol.getcontrolstatus().is_toplocked)
{
if (initstatus == IS_OK)
led_show(0, 255, 0); // 绿色
else
{
// Serial.println("not IS_OK");
led_show(255, 255, 0); // 黄色
}
}
else
{
if (motocontrol.getcontrolstatus().is_setzero)
led_show(255, 255, 255); // 白色
else
{
// Serial.println("not is_setzero");
led_show(255, 255, 0); // 黄色
}
}
led_show(0, 255, 0); // 绿色
break;
}
case HookStatus::HS_Stop:
{
led_show(255, 255, 255); // 白色
break;
}
}
*/
/*
switch (motocontrol.getcontrolstatus().motostatus)
{
case MotoStatus::MS_Down:
{
led_show(0, 0, 255); // 蓝色
break;
}
case MotoStatus::MS_Up:
{
if (motocontrol.getcontrolstatus().is_instore)
led_show(255, 0, 0); // 红
else
led_show(200, 0, 50); // 粉红
break;
}
case MotoStatus::MS_Stop:
{
if (motocontrol.getcontrolstatus().is_toplocked)
{
if (initstatus == IS_OK)
led_show(0, 255, 0); // 绿色
else
{
// Serial.println("not IS_OK");
led_show(255, 255, 0); // 黄色
}
}
else
{
if (motocontrol.getcontrolstatus().is_setzero)
led_show(255, 255, 255); // 白色
else
{
// Serial.println("not is_setzero");
led_show(255, 255, 0); // 黄色
}
}
break;
}
}
*/
}
/////////////////////////////////////按钮处理
// 顶部按钮，检测是否到顶部
void ctbtn_pressstart()
{
ESP_LOGD(MOUDLENAME,"Top_pressstart");
//只在上升时停止
if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore))
ESP_LOGD(MOUDLENAME, "Top_pressstart");
// 只在上升时停止
if ((motocontrol.gethooktatus() == HS_UnInit) || (motocontrol.gethooktatus() == HS_Up) || (motocontrol.gethooktatus() == HS_InStore))
{
_tm_bengstop = millis();
_bengstop = true;
@ -623,14 +609,14 @@ void ctbtn_pressstart()
// 顶部按钮，抬起
void ctbtn_pressend()
{
ESP_LOGD(MOUDLENAME,"Top_pressend");
ESP_LOGD(MOUDLENAME, "Top_pressend");
set_locked(false);
_bengstop = false;
}
void down_action(float motospeed,float length = 0.0f)
void down_action(float motospeed, float length = 0.0f)
{
ESP_LOGD(MOUDLENAME,"Down_action sp:%.2f len:%.2f cm",motospeed,length);
ESP_LOGD(MOUDLENAME, "Down_action sp:%.2f len:%.2f cm", motospeed, length);
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{
@ -638,14 +624,15 @@ void down_action(float motospeed,float length = 0.0f)
}
else
{
//如果在自动放线状态，只是恢复自动放线
// 如果在自动放线状态，只是恢复自动放线
if (motocontrol.getcontrolstatus().is_autogoodsdown)
{
motocontrol.moto_goodsdownresume();
}else
}
else
{
motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Down,length);
motocontrol.moto_run(MotoStatus::MS_Down, length);
fc.playText("放线");
}
}
@ -653,52 +640,55 @@ void down_action(float motospeed,float length = 0.0f)
void up_action(float motospeed)
{
ESP_LOGD(MOUDLENAME,"Up_action sp:%.2f",motospeed);
ESP_LOGD(MOUDLENAME, "Up_action sp:%.2f", motospeed);
if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
{
motocontrol.stopautodown(); //终止自动放线
motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Up);
fc.playText("收线");
}else
motocontrol.stopautodown(); // 终止自动放线
motocontrol.setspeed(motospeed);
motocontrol.moto_run(MotoStatus::MS_Up);
fc.playText("收线");
}
else
motocontrol.stoprun();
}
// 放线按钮--单击
void downbtn_click()
{
ESP_LOGD(MOUDLENAME,"Down_click");
_checkweightcal=true;//检测是否需要校准称重传感器--在下放停止时检测
_checkweighttimes=0;
down_action(SPEED_BTN_SLOW,10);
ESP_LOGD(MOUDLENAME, "Down_click");
_checkweightcal = true; // 检测是否需要校准称重传感器--在下放停止时检测
_checkweighttimes = 0;
down_action(SPEED_BTN_SLOW, 10);
}
// 放线按钮--双击
void downbtn_dbclick()
{
ESP_LOGD(MOUDLENAME,"Down_dbclick");
ESP_LOGD(MOUDLENAME, "Down_dbclick");
down_action(SPEED_BTN_MID);
}
// 放线按钮--长按
void downbtn_pressstart()
{
ESP_LOGD(MOUDLENAME,"Down_pressstart");
//两个同时按用于对频
ESP_LOGD(MOUDLENAME, "Down_pressstart");
// 两个同时按用于对频
btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2;
if (btn_pressatonce==2)
if (btn_pressatonce > 2)
btn_pressatonce = 2;
if (btn_pressatonce == 2)
{
btn_presssatonce();
return;
}
}
down_action(SPEED_BTN_FAST);
}
// 放线按钮--长按抬起
void downbtn_pressend()
{
ESP_LOGD(MOUDLENAME,"Down_pressend");
ESP_LOGD(MOUDLENAME, "Down_pressend");
btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0;
//不是恢复自动放线抬起按键就停止
if (btn_pressatonce < 0)
btn_pressatonce = 0;
// 不是恢复自动放线抬起按键就停止
if (!motocontrol.getcontrolstatus().is_autogoodsdown)
motocontrol.stoprun();
}
@ -706,111 +696,113 @@ void downbtn_pressend()
// 收线按钮-单击
void upbtn_click()
{
// fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
ESP_LOGD(MOUDLENAME,"UP_click");
up_action(SPEED_BTN_SLOW);
// fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
ESP_LOGD(MOUDLENAME, "UP_click");
up_action(SPEED_BTN_SLOW);
}
// 收线按钮-双击
void upbtn_dbclick()
{
ESP_LOGD(MOUDLENAME,"UP_dbclick");
ESP_LOGD(MOUDLENAME, "UP_dbclick");
up_action(SPEED_BTN_MID);
}
// 两个按钮同时按下
void btn_presssatonce()
{
ESP_LOGD(MOUDLENAME,"UP_presssatonce");
led_show(255,255, 255); // 高亮一下
ESP_LOGD(MOUDLENAME, "UP_presssatonce");
led_show(255, 255, 255); // 高亮一下
fc.playText("发送对频信息");
///px1
//fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
/// px1
// fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
}
// 收线按钮-长按
void upbtn_pressstart()
{
ESP_LOGD(MOUDLENAME,"UP_pressstart");
//两个同时按用于对频
ESP_LOGD(MOUDLENAME, "UP_pressstart");
// 两个同时按用于对频
btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2;
if (btn_pressatonce==2)
if (btn_pressatonce > 2)
btn_pressatonce = 2;
if (btn_pressatonce == 2)
{
btn_presssatonce();
return;
}
}
up_action(SPEED_BTN_FAST);
}
// 收线按钮-长按抬起
void upbtn_pressend()
{
ESP_LOGD(MOUDLENAME,"UP_pressend");
ESP_LOGD(MOUDLENAME, "UP_pressend");
btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0;
if (btn_pressatonce < 0)
btn_pressatonce = 0;
motocontrol.stoprun();
}
//自动下放
// 自动下放
void Hook_autodown(float length_cm)
{
if (motocontrol.gethooktatus()==HS_Locked)
if (motocontrol.gethooktatus() == HS_Locked)
{
ESP_LOGD(MOUDLENAME,"Hook_autodown %.2f cm",length_cm);
ESP_LOGD(MOUDLENAME, "Hook_autodown %.2f cm", length_cm);
fc.Camera_action_down();
motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40
}else
ESP_LOGE(MOUDLENAME,"autodown fault, not HS_Locked ");
}
else
ESP_LOGE(MOUDLENAME, "autodown fault, not HS_Locked ");
}
void Hook_stop()
{
ESP_LOGD(MOUDLENAME,"Hook_stop");
motocontrol.stoprun();
ESP_LOGD(MOUDLENAME, "Hook_stop");
motocontrol.stoprun();
}
void Hook_resume()
{
if (motocontrol.gethooktatus()==HS_Stop)
if (motocontrol.gethooktatus() == HS_Stop)
{
ESP_LOGD(MOUDLENAME,"Hook_resume");
ESP_LOGD(MOUDLENAME, "Hook_resume");
motocontrol.moto_goodsdownresume();
}else
ESP_LOGE(MOUDLENAME,"resume fault, not HS_Stop ");
}
else
ESP_LOGE(MOUDLENAME, "resume fault, not HS_Stop ");
}
void Hook_recovery()
{
ESP_LOGD(MOUDLENAME,"Hook_recovery");
if (motocontrol.gethooktatus()!=HS_Locked)
{
motocontrol.stoprun();
up_action(SPEED_HOOK_UP);
}
ESP_LOGD(MOUDLENAME, "Hook_recovery");
if (motocontrol.gethooktatus() != HS_Locked)
{
motocontrol.stoprun();
up_action(SPEED_HOOK_UP);
}
}
// 测试按钮
void testbtn_click()
{
ESP_LOGD(MOUDLENAME,"testbtn_click");
ESP_LOGD(MOUDLENAME, "testbtn_click");
switch (motocontrol.gethooktatus())
{
case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
{
ESP_LOGD(MOUDLENAME,"moto_goodsdown");
ESP_LOGD(MOUDLENAME, "moto_goodsdown");
motocontrol.moto_goodsdown(22); // 二楼340 //桌子40
break;
}
case HS_Stop:
{
ESP_LOGD(MOUDLENAME,"moto_resume");
ESP_LOGD(MOUDLENAME, "moto_resume");
motocontrol.moto_goodsdownresume();
break;
}
default:
{
ESP_LOGD(MOUDLENAME,"stop");
ESP_LOGD(MOUDLENAME, "stop");
motocontrol.stoprun();
}
}
}
//////////////////////////////MQTT_语音_MAVLINK 部分

12
src/moto.cpp
View File

@ -5,20 +5,20 @@
uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis;
PID_TypeDef moto_pid;
static const char* MOUDLENAME = "MOTO";
static const char *MOUDLENAME = "MOTO";
moto::moto()
{
_closed = true;
}
bool moto::init()
{
ESP_LOGD(MOUDLENAME,"init moto");
ESP_LOGD(MOUDLENAME, "init moto");
pid_init();
CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
// start the CAN bus at 500 kbps
if (!CAN.begin(1000E3))
{
ESP_LOGE(MOUDLENAME,"Starting CAN failed!");
ESP_LOGE(MOUDLENAME, "Starting CAN failed!");
return false;
}
CAN.onReceive(onReceive);
@ -30,7 +30,7 @@ moto::~moto()
void moto::update()
{
//printf("u1:%d\n",_closed);
// printf("u1:%d\n",_closed);
if (!_closed)
{
unsigned long dt = millis() - moto_chassis.starttime; // 时间差
@ -45,7 +45,7 @@ void moto::update()
_msoftspeed = _start_speed + mspeed * _ds;
}
pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed);
// printf("u2\n");
// printf("u2\n");
set_moto_current(moto_pid.output);
// printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n",
// moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);
@ -66,7 +66,7 @@ void moto::setspeedtarget(float new_target)
_start_speed = moto_chassis.speed_rpm;
_ds = moto_pid.target - _start_speed; // 速度差
_closed = false;
// printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
// printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
}
void moto::set_moto_current(int16_t iq1)

146
src/motocontrol.cpp
View File

@ -6,7 +6,7 @@
#include "config.h"
#define DEBUG_OUT false
static const char* MOUDLENAME = "MOTO_C";
static const char *MOUDLENAME = "MOTO_C";
Motocontrol::Motocontrol()
{
@ -23,7 +23,7 @@ Motocontrol::Motocontrol()
_weightalign = false;
_overweightcount = 0;
_notweightcount = 0;
_controlstatus.is_autogoodsdown=false;
_controlstatus.is_autogoodsdown = false;
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
_unblocktimes = 0;
}
@ -51,55 +51,53 @@ void Motocontrol::setweight(int pullweight) // 设置重量
_pullweight = pullweight;
checkgoods();
}
String Motocontrol::gethooktatus_str(bool chstr)
String Motocontrol::gethooktatus_str(bool chstr)
{
String hookstatusstr="未知";
String hookstatusstr_en="unknown";
switch (_hooksstatus)
{
String hookstatusstr = "未知";
String hookstatusstr_en = "unknown";
switch (_hooksstatus)
{
case HS_UnInit:
hookstatusstr="未初始化";
hookstatusstr_en="HS_UnInit";
break;
case HS_Down:
hookstatusstr="放钩";
hookstatusstr_en="HS_Down";
break;
hookstatusstr = "未初始化";
hookstatusstr_en = "HS_UnInit";
break;
case HS_Down:
hookstatusstr = "放钩";
hookstatusstr_en = "HS_Down";
break;
case HS_DownSlow:
hookstatusstr="慢速放钩";
hookstatusstr_en="HS_DownSlow";
break;
hookstatusstr = "慢速放钩";
hookstatusstr_en = "HS_DownSlow";
break;
case HS_WaitUnhook:
hookstatusstr="等待脱钩";
hookstatusstr_en="HS_WaitUnhook";
break;
hookstatusstr = "等待脱钩";
hookstatusstr_en = "HS_WaitUnhook";
break;
case HS_Up:
hookstatusstr="回收";
hookstatusstr_en="HS_Up";
break;
hookstatusstr = "回收";
hookstatusstr_en = "HS_Up";
break;
case HS_InStore:
hookstatusstr="入仓中";
hookstatusstr_en="HS_InStore";
break;
hookstatusstr = "入仓中";
hookstatusstr_en = "HS_InStore";
break;
case HS_Locked:
hookstatusstr="已锁定";
hookstatusstr_en="HS_Locked";
break;
hookstatusstr = "已锁定";
hookstatusstr_en = "HS_Locked";
break;
case HS_Stop:
hookstatusstr="停止";
hookstatusstr_en="HS_Stop";
break;
default:
hookstatusstr="未知";
hookstatusstr_en="HS_UnInit";
break;
}
hookstatusstr = "停止";
hookstatusstr_en = "HS_Stop";
break;
default:
hookstatusstr = "未知";
hookstatusstr_en = "HS_UnInit";
break;
}
if (chstr)
return hookstatusstr;
else
else
return hookstatusstr_en;
}
void Motocontrol::checkgoods() // 检测是否超重
@ -141,13 +139,13 @@ void Motocontrol::checkgoods() // 检测是否超重
}
void Motocontrol::lockservo() // 锁定舵机
{
ESP_LOGD(MOUDLENAME,"start_lockservo");
ESP_LOGD(MOUDLENAME, "start_lockservo");
_servotatus = SS_WaitMotoStop;
_tm_servotatus = millis();
}
void Motocontrol::unlockservo() // 解锁舵机
{
ESP_LOGD(MOUDLENAME,"unlockservo");
ESP_LOGD(MOUDLENAME, "unlockservo");
// 解锁操作
_lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
@ -157,25 +155,25 @@ void Motocontrol::unlockservo() // 解锁舵机
}
void Motocontrol::stoprun(float acctime) // 停止
{
ESP_LOGD(MOUDLENAME,"stoprun after time:%.2f",acctime);
ESP_LOGD(MOUDLENAME, "stoprun after time:%.2f", acctime);
_moto_dji.settime_acc(acctime);
_moto_dji.setspeedtarget(0.0f);
_controlstatus.motostatus = MS_Stop;
if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked) )
if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) && (_hooksstatus != HS_Locked))
{
_hooksstatus_prv = _hooksstatus;
sethooksstatus(HS_Stop);
}
lockservo();
}
void Motocontrol::stopautodown()
void Motocontrol::stopautodown()
{
_controlstatus.is_autogoodsdown = false;
}
void Motocontrol::sethooksstatus(HookStatus hookstatus)
{
_hooksstatus=hookstatus;
ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false));
_hooksstatus = hookstatus;
ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false));
}
void Motocontrol::setlocked(bool locked)
{
@ -199,7 +197,6 @@ int16_t Motocontrol::getlength() // 得到长度
return 0;
}
// 重量传感器已经校准
void Motocontrol::weightalign(bool weightalign)
{
@ -220,14 +217,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
// 如果没有货物--开始回收
if (!_controlstatus.is_havegoods)
{
ESP_LOGD(MOUDLENAME,"Not goods wait %d ms for unhook",HOOK_UNHOOK_TIME);
ESP_LOGD(MOUDLENAME, "Not goods wait %d ms for unhook", HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis();
break;
}
if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
{
ESP_LOGD(MOUDLENAME,"HS_Down target is arrived curr: %.2f",mf.output_round_cnt);
ESP_LOGD(MOUDLENAME, "HS_Down target is arrived curr: %.2f", mf.output_round_cnt);
sethooksstatus(HS_DownSlow);
_moto_dji.settime_acc(500);
set_hook_speed(SPEED_HOOK_SLOW);
@ -239,7 +236,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{
if (!_controlstatus.is_havegoods)
{
ESP_LOGD(MOUDLENAME,"Not havegoods wait %d ms unhook",HOOK_UNHOOK_TIME);
ESP_LOGD(MOUDLENAME, "Not havegoods wait %d ms unhook", HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis();
}
@ -250,7 +247,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
{
ESP_LOGD(MOUDLENAME,"HS_WaitUnhook ok startup");
ESP_LOGD(MOUDLENAME, "HS_WaitUnhook ok startup");
_moto_dji.settime_acc(1000);
set_hook_speed(-SPEED_HOOK_UP);
sethooksstatus(HS_Up);
@ -316,14 +313,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
if (!checkservoblock() || (_unblocktimes > 2))
{
ESP_LOGD(MOUDLENAME,"Close moto");
ESP_LOGD(MOUDLENAME, "Close moto");
_moto_dji.close();
_servotatus = SS_ServoLocked;
}
else
// 堵转
{
ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto");
ESP_LOGD(MOUDLENAME, "SS servoblock unlock servo and turn moto");
_tm_servotatus = millis();
_servotatus = SS_WaitUnBlock;
// 写一个不会堵转的位置
@ -339,7 +336,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{
if ((millis() - _tm_servotatus) > TM_UNBLOCK)
{
ESP_LOGD(MOUDLENAME,"SS lock servo");
ESP_LOGD(MOUDLENAME, "SS lock servo");
// 继续锁定等待舵机检测
_lockservo->write(SERVO_LOCKPOS);
_servotatus = SS_WaitServo;
@ -373,12 +370,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
_moto_dji.settime_acc(500);
set_hook_speed(-SPEED_INSTORE);
_controlstatus.is_instore = true;
ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
ESP_LOGD(MOUDLENAME, "Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
}
}
else
{
//已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
// 已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
if (_hooksstatus != HS_InStore)
_controlstatus.is_instore = false;
}
@ -389,7 +386,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{
if (mf.output_round_cnt > _target_cnt)
{
ESP_LOGD(MOUDLENAME,"stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
ESP_LOGD(MOUDLENAME, "stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止
}
}
@ -397,7 +394,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{
if (mf.output_round_cnt < _target_cnt)
{
ESP_LOGD(MOUDLENAME,"stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
ESP_LOGD(MOUDLENAME, "stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止
}
}
@ -432,7 +429,8 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
void Motocontrol::moto_goodsdownresume()
{
if (!_controlstatus.is_autogoodsdown) return;
if (!_controlstatus.is_autogoodsdown)
return;
if (_hooksstatus == HS_Stop)
{
_runspeed = _goods_speed;
@ -449,29 +447,29 @@ void Motocontrol::moto_goodsdownresume()
// _moto_dji.setspeedtarget(_goods_speed);
}
}
//长度cm
// 长度cm
void Motocontrol::moto_goodsdown(float length)
{
if (length < 0.0)
{
ESP_LOGD(MOUDLENAME,"length<0 fault");
ESP_LOGD(MOUDLENAME, "length<0 fault");
return;
}
// 没设置零点
if (!_controlstatus.is_setzero)
{
ESP_LOGD(MOUDLENAME,"not lengthzero fault");
ESP_LOGD(MOUDLENAME, "not lengthzero fault");
return;
}
if (!_weightalign)
{
ESP_LOGD(MOUDLENAME,"weight not align fault");
ESP_LOGD(MOUDLENAME, "weight not align fault");
return;
}
// 没挂东西
if (!_controlstatus.is_havegoods)
{
ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight);
ESP_LOGD(MOUDLENAME, "goods min fault:%d", _pullweight);
return;
}
@ -487,12 +485,12 @@ void Motocontrol::moto_goodsdown(float length)
}
else
{
float tarleng=length;
if (length>HOOK_SLOWDOWN_LENGTH)
tarleng-=HOOK_SLOWDOWN_LENGTH;
float tarleng = length;
if (length > HOOK_SLOWDOWN_LENGTH)
tarleng -= HOOK_SLOWDOWN_LENGTH;
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
_goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER;
ESP_LOGD(MOUDLENAME,"start down %.2f cm,tar:%.2f",tarleng,_goods_down_target_cnt);
ESP_LOGD(MOUDLENAME, "start down %.2f cm,tar:%.2f", tarleng, _goods_down_target_cnt);
setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
_goods_speed = SPEED_HOOK_FAST;
}
@ -512,25 +510,25 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
// 运动中暂时不管
if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
{
ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up");
ESP_LOGD(MOUDLENAME, "motostatus is MS_Down\\MS_Up");
return;
}
// 没设置速度不转
if (_controlstatus.speed_targe == 0)
{
ESP_LOGD(MOUDLENAME,"not set speed_targe");
ESP_LOGD(MOUDLENAME, "not set speed_targe");
return;
}
if (updown == MS_Up)
{
if (_controlstatus.is_toplocked)
{
ESP_LOGD(MOUDLENAME,"is_toplocked return");
ESP_LOGD(MOUDLENAME, "is_toplocked return");
return;
}
if (_controlstatus.is_overweight)
{
ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight);
ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight);
return;
}
sethooksstatus(HS_Up);
@ -567,7 +565,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
runspeed = -_controlstatus.speed_targe;
}
// 开始转
// ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
// ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
_runspeed = runspeed;
// _moto_dji.setspeedtarget(runspeed);
}

21
src/motocontrol.h
View File

@ -9,7 +9,7 @@
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数
#define INSTORE_LENGTH_MIN_NONE 12.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_MIN_NONE 12.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_MIN_GOODS 10.0f // 最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
@ -20,8 +20,8 @@
#define SPEED_BTN_MID 100 // 按键中等收放线速度
#define SPEED_BTN_FAST 200 // 按键最快收放线速度
#define SPEED_HOOK_FAST SPEED_MAX // 货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP SPEED_MAX // 钩子回收速度
#define SPEED_UNBLOCK 0.1 // 舵机堵转需要缓慢转的速度
@ -29,8 +29,8 @@
#define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
#define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些
#define SERVO_LOCKPOS 1000 //1920 // 舵机锁定位置
#define SERVO_UNLOCKPOS 1120 //1800 // 舵机解锁位置
#define SERVO_LOCKPOS 1000 // 1920 // 舵机锁定位置
#define SERVO_UNLOCKPOS 1120 // 1800 // 舵机解锁位置
#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)
@ -55,7 +55,7 @@ typedef struct
bool is_toplocked; // 已到顶锁住
bool is_overweight; // 是否超重
bool is_havegoods; // 是否有货物
bool is_autogoodsdown; //正在自动放线中
bool is_autogoodsdown; // 正在自动放线中
float zero_cnt; // 0长度位置
float speed_targe; // 当前目标速度
MotoStatus motostatus; // 电机运行状态
@ -111,12 +111,13 @@ private:
void unlockservo();
void set_hook_speed(float hooksspeed);
void sethooksstatus(HookStatus hookstatus);
public:
Motocontrol(); // 构造函数
~Motocontrol(); // 析构函数
void setspeed(float motospeed, float acctime = -1); // 设置速度
void stoprun(float acctime = 0); // 停止运行
void stopautodown(); //停止自动下放模式
void stoprun(float acctime = 0); // 停止运行
void stopautodown(); // 停止自动下放模式
void setzero(); // 设置0长度位置
int16_t getlength(); // 得到长度
void update(); // 更新
@ -130,7 +131,7 @@ public:
void moto_goodsdown(float length = 0.0f); // 放下货物,自动回收
void moto_goodsdownresume(); // 恢复自动放线
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
String gethooktatus_str(bool chstr=true); // 得到挂钩状态(中英文）
String gethooktatus_str(bool chstr = true); // 得到挂钩状态(中英文）
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
};