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

修改日志框架

Browse Source 
放物体时相机自动向下和回中
This commit is contained in:
pxzleo 2023-08-16 23:34:11 +08:00
parent 034bde27df
commit 8da4ad4693
9 changed files with 239 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
platformio.ini
View File

@ -12,6 +12,7 @@
platform = espressif32 platform = espressif32
board = esp32doit-devkit-v1 board = esp32doit-devkit-v1
framework = arduino framework = arduino
build_flags = -DCORE_DEBUG_LEVEL=5
monitor_speed = 115200 monitor_speed = 115200
upload-port = COM[14] upload-port = COM[14]
lib_deps = lib_deps =

51
src/FoodDeliveryBase.cpp
View File

@ -2,6 +2,7 @@
/** /**
* @description: 初始化 * @description: 初始化
*/ */
static const char* MOUDLENAME = "FC";
FoodCube::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::FoodCube(char* userSsid, char* userPassword, char* userMqttServer, int userMqttPort, char* userMqttName, char* userMqttPassword, uint8_t userMavlinkSerial, uint8_t userVoiceSerial, char* userUdpServerIP, uint32_t userUdpServerPort) {
/*初始化*/ /*初始化*/
ssid = userSsid; //wifi帐号 ssid = userSsid; //wifi帐号
@ -64,16 +65,16 @@ void FoodCube::log(bool val) {
Serial.print(val); Serial.print(val);
} }
void FoodCube::logln(String val) { void FoodCube::logln(String val) {
Serial.println(val); ESP_LOGD(MOUDLENAME,"%s",val);
} }
void FoodCube::logln(char* val) { void FoodCube::logln(char* val) {
Serial.println(val); ESP_LOGD(MOUDLENAME,"%s",val);
} }
void FoodCube::logln(int val) { void FoodCube::logln(int val) {
Serial.println(val); ESP_LOGD(MOUDLENAME,"%d",val);
} }
void FoodCube::logln(IPAddress val) { void FoodCube::logln(IPAddress val) {
Serial.println(val); ESP_LOGD(MOUDLENAME,"%s",val.toString());
} }
void FoodCube::logln(bool val) { void FoodCube::logln(bool val) {
Serial.print(val); Serial.print(val);
@ -107,7 +108,7 @@ IPAddress secondaryDNS(8, 8, 4, 4); //optional
void FoodCube::connectWifi() { void FoodCube::connectWifi() {
if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) { if (!WiFi.config(local_IP, gateway, subnet, primaryDNS, secondaryDNS)) {
Serial.println("STA Failed to configure"); ESP_LOGD(MOUDLENAME,"STA Failed to configure");
} }
//设置wifi帐号密码 //设置wifi帐号密码
@ -180,7 +181,7 @@ void FoodCube::connectMqtt(String topicSub[], int topicSubCount) {
subscribeTopic(topicSub[i], 1); subscribeTopic(topicSub[i], 1);
} }
delay(500); delay(500);
playText("[v1]指令服务器已连接"); playText("[v1]服务器已连接");
} else { } else {
//失败返回状态码 //失败返回状态码
log("MQTT Server Connect Failed. Client State:"); log("MQTT Server Connect Failed. Client State:");
@ -263,6 +264,7 @@ void FoodCube::SWrite(uint8_t buf[], int len, uint8_t swSerial) {
* @param {String} str 播放内容 * @param {String} str 播放内容
*/ */
void FoodCube::playText(String str) { void FoodCube::playText(String str) {
// str="[v10]"+str;
//return ; //return ;
/*消息长度*/ /*消息长度*/
int len = str.length(); //修改信息长度 消息长度 int len = str.length(); //修改信息长度 消息长度
@ -365,6 +367,43 @@ void FoodCube::crc_check_16bites(uint8_t* pbuf, uint32_t len, uint16_t* p_result
crc_result = CRC16_cal(pbuf, len, 0); crc_result = CRC16_cal(pbuf, len, 0);
*p_result = crc_result; *p_result = crc_result;
} }
/**
* @description: 相机 向下
*/
void FoodCube::Camera_action_down() {
Camera_action_move(-100,0);//俯仰 向下
}
/**
* @description: 相机 角度控制
*/
void FoodCube::Camera_action_move(uint8_t vpitch,uint8_t vyaw ) {
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
command[8] =vyaw; //旋转
command[9] =vpitch; //俯仰
udpSendToCamera(command, sizeof(command));
}
/**
* @description: 相机 角度控制
*/
void FoodCube::Camera_action_zoom(uint8_t vzoom ) {
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 };
command[8] =vzoom; //变焦
udpSendToCamera(command, sizeof(command));
}
/**
* @description: 相机 回中
*/
void FoodCube::Camera_action_ret() {
uint8_t command1[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 };
udpSendToCamera(command1, sizeof(command1));
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 };
udpSendToCamera(command, sizeof(command));
}
/** /**
* @description: 相机 * @description: 相机
* @param {uint8_t[]} 命令帧 数组 * @param {uint8_t[]} 命令帧 数组

4
src/FoodDeliveryBase.h
View File

@ -71,6 +71,10 @@ public:
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 ) ;
bool checkWiFiStatus(); bool checkWiFiStatus();
private: private:

4
src/config.h
View File

@ -18,8 +18,8 @@
////LED ////LED
#define LED_DATA_PIN 25 #define LED_DATA_PIN 25
// Moto-CAN // Moto-CAN
#define MOTO_CAN_RX 26 #define MOTO_CAN_RX 26 //1号机 26 后面 27
#define MOTO_CAN_TX 27 #define MOTO_CAN_TX 27 //1号机 27 后面 26
///serial1 ///serial1
#define SERIAL_REPORT_TX 5 #define SERIAL_REPORT_TX 5
#define SERIAL_REPORT_RX 18 #define SERIAL_REPORT_RX 18

250
src/main.cpp
View File

@ -66,9 +66,13 @@ unsigned long _tm_bengstop;
bool _bengstop = false; bool _bengstop = false;
bool _needweightalign = false; bool _needweightalign = false;
HookStatus _lasthooktatus=HS_UnInit; //前一个钩子状态
bool curr_armed=false; bool curr_armed=false;
uint8_t curr_mode=0; uint8_t curr_mode=0;
bool _checkweightcal=false; //检测是否要检测称重传感器是否需要校准
uint8_t _checkweighttimes=0; //
unsigned long _tm_checkweigh;
static const char* MOUDLENAME = "MAIN";
// 称重校准状态 // 称重校准状态
enum Weightalign_status enum Weightalign_status
{ {
@ -99,6 +103,7 @@ 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_PAUSE=41; //飞控发的---暂停
const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态 const uint16_t MAV_CMD_FC_HOOK_STATUS=42; //发给飞控的状态
const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线 const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
const uint16_t MAV_CMD_FC_DESCENTSTART=44; //飞开始下降，需要调整相机向下
/////////////////////// ///////////////////////
@ -106,12 +111,12 @@ const uint16_t MAV_CMD_FC_HOOK_RECOVERY=43; //飞控发的---收线
/*项目对象*/ /*项目对象*/
//char* ssid = "szdot"; //wifi帐号 //char* ssid = "szdot"; //wifi帐号
//char* password = "Ttaj@#*.com"; //wifi密码 //char* password = "Ttaj@#*.com"; //wifi密码
char* ssid = (char*)"fxmf_sc01"; //wifi帐号 char* ssid = (char*)"flicube"; //wifi帐号 "flicube";// "fxmf_sc01"
char* password = (char*)"12345678"; //wifi密码 char* password = (char*)"fxmf0622"; //wifi密码 "fxmf0622"; //"12345678"
char* mqttServer = (char*)"114.115.137.239";//"szdot.top"; //mqtt地址 char* mqttServer = (char*)"114.115.137.239";//"szdot.top"; //114.115.137.239 mqtt地址
int mqttPort = 1883; //mqtt端口 int mqttPort = 1883; //mqtt端口
char* mqttName = (char*)"admin"; //mqtt帐号 char* mqttName = (char*)"admin"; //mqtt帐号
char* mqttPassword = (char*)"123456"; //mqtt密码 char* mqttPassword = (char*)"1234567"; //mqtt密码
uint8_t mavlinkSerial = 2; //飞控占用的串口号 uint8_t mavlinkSerial = 2; //飞控占用的串口号
uint8_t voiceSerial = 1; //声音模块占用串口 uint8_t voiceSerial = 1; //声音模块占用串口
char* udpServerIP = (char*) "192.168.8.210"; //云台相机ip char* udpServerIP = (char*) "192.168.8.210"; //云台相机ip
@ -131,7 +136,7 @@ String topicSub[] = { "questAss", "setPlaneState", "getPlaneState", "resetState"
int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数 int topicSubCount = sizeof(topicSub) / sizeof(topicSub[0]); //订阅主题总数
/*有更新主动发送 主题*/ /*有更新主动发送 主题*/
//0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15:位置(经纬度,海拔高度) //0:心跳信息 1:电压信息 2:电流信息 3:电池电量 4:高度信息 5:对地速度 6:卫星数量 7:纬度 8:经度 9:定位状态 10:飞机状态 11:网速测试 12:飞机模式,13:负载重量(g),14：钩子状态,15:位置(经纬度,海拔高度)
String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus","position",}; String topicPub[] = { "heartBeat", "voltagBattery", "currentBattery", "batteryRemaining", "positionAlt", "groundSpeed", "satCount", "latitude", "longitude", "fixType", "planeState", "pingNet", "getPlaneMode","loadweight","hookstatus","position"};
int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数 int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); //发送主题总数
String topicPubMsg[16]; //发送数据存放 对应topicPub String topicPubMsg[16]; //发送数据存放 对应topicPub
String oldMsg[16]; //记录旧的数据 用来对比有没有更新 String oldMsg[16]; //记录旧的数据 用来对比有没有更新
@ -154,10 +159,10 @@ void setup()
// 调试串口 // 调试串口
Serial.begin(57600); Serial.begin(57600);
printf("Starting PullupDevice... Ver:%s\n",VERSION); ESP_LOGI(MOUDLENAME,"Starting PullupDevice... Ver:%s",VERSION);
preferences.begin("PullupDev", false); preferences.begin("PullupDev", false);
wei_offset = preferences.getLong("wei_offset", 0); wei_offset = preferences.getLong("wei_offset", 0);
printf("wei_offset:%d\n",wei_offset); ESP_LOGD(MOUDLENAME,"wei_offset:%d",wei_offset);
@ -189,8 +194,7 @@ void setup()
// 初始化RGB LED 显示黄色灯表示需要注意 LED // 初始化RGB LED 显示黄色灯表示需要注意 LED
FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
if (!motocontrol.init(&myservo)) // 初始化电机控制 if (!motocontrol.init(&myservo)) // 初始化电机控制
printf("motocontrol init fault\n"); ESP_LOGE(MOUDLENAME,"motocontrol init fault");
tksendinfo.start(); tksendinfo.start();
initstatus = IS_WaitStart; initstatus = IS_WaitStart;
@ -223,8 +227,7 @@ void setup()
// //slowup(); // //slowup();
// } // }
fc.playText("[v1]启动完成"); ESP_LOGI(MOUDLENAME,"PullupDevice is ready!");
Serial.println("PullupDevice is ready!");
// fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!"); // fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
} }
void slowup() void slowup()
@ -232,6 +235,45 @@ void slowup()
// motocontrol.setspeed(SPEED_UP_SLOW); // motocontrol.setspeed(SPEED_UP_SLOW);
// motocontrol.up(0); //一直收线直到到顶部 // motocontrol.up(0); //一直收线直到到顶部
} }
void checkstatus()
{
HookStatus vhooktatus= motocontrol.gethooktatus();
//入仓把云台回中
if ((_lasthooktatus!=vhooktatus)&&(vhooktatus==HS_InStore))
fc.Camera_action_ret();
if (_checkweightcal && (vhooktatus==HS_Stop))
{
//第一次进来
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;
}
}
}
_lasthooktatus=vhooktatus;
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
}
//1秒调用一次了发mqtt到地面
void showinfo() void showinfo()
{ {
// moto_measure_t mf=motocontrol.getmotoinfo() ; // moto_measure_t mf=motocontrol.getmotoinfo() ;
@ -245,22 +287,20 @@ void showinfo()
// control_status_t cs=motocontrol.getcontrolstatus() ; // control_status_t cs=motocontrol.getcontrolstatus() ;
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus); // printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
HookStatus vhooktatus= motocontrol.gethooktatus();
mavlink_command_long_t msg_cmd; mavlink_command_long_t msg_cmd;
msg_cmd.command=MAV_CMD_FC_HOOK_STATUS; msg_cmd.command=MAV_CMD_FC_HOOK_STATUS;
msg_cmd.param1=motocontrol.gethooktatus(); msg_cmd.param1=vhooktatus;
msg_cmd.param2=pullweight; msg_cmd.param2=pullweight;
msg_cmd.target_system = 1; msg_cmd.target_system = 1;
msg_cmd.target_component = 1; msg_cmd.target_component = 1;
msg_cmd.confirmation = 0; msg_cmd.confirmation = 0;
fc.mav_send_command(msg_cmd); fc.mav_send_command(msg_cmd);
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
} }
//校准称重 //校准称重
void begin_tare() void begin_tare()
{ {
printf("begin_tare \n"); ESP_LOGD(MOUDLENAME,"begin_tare");
_weightalign_status=WAS_Aligning; _weightalign_status=WAS_Aligning;
_needweightalign = true; _needweightalign = true;
_weightalign_begintm=millis(); _weightalign_begintm=millis();
@ -287,14 +327,14 @@ bool check_tare()
wei_offset=scale.get_offset(); wei_offset=scale.get_offset();
preferences.putLong("wei_offset", wei_offset); preferences.putLong("wei_offset", wei_offset);
motocontrol.weightalign(true); motocontrol.weightalign(true);
printf("check_tare ok: %d,offset:%d \n", pullweight,wei_offset); ESP_LOGD(MOUDLENAME,"check_tare ok: %d,offset:%d", pullweight,wei_offset);
return true; return true;
}else _needweightalign = true; }else _needweightalign = true;
} }
else else
{ {
// 没成功继续校准 // 没成功继续校准
printf("weight not zero: %d \n", pullweight); ESP_LOGD(MOUDLENAME,"weight not zero: %d", pullweight);
_needweightalign = true; _needweightalign = true;
pullweightoktimes=0; pullweightoktimes=0;
} }
@ -320,7 +360,7 @@ void checkinited()
if (motocontrol.gethooktatus() != HS_Locked) if (motocontrol.gethooktatus() != HS_Locked)
{ {
// 开始自动慢速上升，直到顶部按钮按下 // 开始自动慢速上升，直到顶部按钮按下
printf("IS_Start: startup wait locked\n"); ESP_LOGD(MOUDLENAME,"IS_Start: startup wait locked");
motocontrol.setspeed(SPEED_BTN_SLOW); motocontrol.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Up); motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_Wait_Locked; initstatus = IS_Wait_Locked;
@ -335,7 +375,7 @@ void checkinited()
{ {
if (motocontrol.gethooktatus() == HS_Locked) if (motocontrol.gethooktatus() == HS_Locked)
{ {
printf("IS_Wait_LengthZero: is locked\n"); ESP_LOGD(MOUDLENAME,"IS_Wait_LengthZero: is locked");
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
} }
break; break;
@ -346,14 +386,15 @@ void checkinited()
//如果没有保存的校准数据就需要校准 //如果没有保存的校准数据就需要校准
if (wei_offset!=0) if (wei_offset!=0)
{ {
printf("offset is loaded weight: %d,offset:%d \n", pullweight,wei_offset); ESP_LOGD(MOUDLENAME,"offset is loaded weight: %d,offset:%d", pullweight,wei_offset);
scale.set_offset(wei_offset); scale.set_offset(wei_offset);
motocontrol.weightalign(true); motocontrol.weightalign(true);
_needweightalign = false; _needweightalign = false;
fc.playText("[v1]挂钩已锁定");
initstatus = IS_OK; initstatus = IS_OK;
}else }else
{ {
printf("begin_tare \n"); ESP_LOGD(MOUDLENAME,"begin_tare");
begin_tare(); begin_tare();
initstatus = IS_CheckWeightZero; initstatus = IS_CheckWeightZero;
} }
@ -363,11 +404,12 @@ void checkinited()
{ {
if (_weightalign_status==WAS_AlignOK) if (_weightalign_status==WAS_AlignOK)
{ {
fc.playText("[v1]挂钩已锁定");
initstatus = IS_OK; initstatus = IS_OK;
}else }else
if (_weightalign_status==WAS_Alignfault) if (_weightalign_status==WAS_Alignfault)
{ {
printf("weightalign fault! again \n"); ESP_LOGD(MOUDLENAME,"weightalign fault! again");
initstatus = IS_begin_WeightZero; initstatus = IS_begin_WeightZero;
} }
break; break;
@ -433,7 +475,7 @@ void loop()
motocontrol.update(); // 电机控制 motocontrol.update(); // 电机控制
showledidel(); // 显示LED灯光 showledidel(); // 显示LED灯光
checkstatus(); //检测状态，执行一些和状态有关的功能
//showinfo(); // 显示一些调试用信息- //showinfo(); // 显示一些调试用信息-
// 到顶后延迟关闭动力电和舵机 // 到顶后延迟关闭动力电和舵机
if (_bengstop) if (_bengstop)
@ -617,7 +659,7 @@ void showledidel()
// 顶部按钮，检测是否到顶部 // 顶部按钮，检测是否到顶部
void ctbtn_pressstart() void ctbtn_pressstart()
{ {
Serial.println("Top_pressstart"); ESP_LOGD(MOUDLENAME,"Top_pressstart");
//只在上升时停止 //只在上升时停止
if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore)) if ((motocontrol.gethooktatus()== HS_UnInit)||(motocontrol.gethooktatus()== HS_Up)||(motocontrol.gethooktatus()== HS_InStore))
{ {
@ -628,14 +670,14 @@ void ctbtn_pressstart()
// 顶部按钮，抬起 // 顶部按钮，抬起
void ctbtn_pressend() void ctbtn_pressend()
{ {
Serial.println("Top_pressend"); ESP_LOGD(MOUDLENAME,"Top_pressend");
motocontrol.setlocked(false); motocontrol.setlocked(false);
_bengstop = false; _bengstop = false;
} }
void down_action(float motospeed,float length = 0.0f) void down_action(float motospeed,float length = 0.0f)
{ {
printf("Down_action sp:%.2f len:%.2f cm \n",motospeed,length); ESP_LOGD(MOUDLENAME,"Down_action sp:%.2f len:%.2f cm",motospeed,length);
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{ {
@ -658,7 +700,7 @@ void down_action(float motospeed,float length = 0.0f)
void up_action(float motospeed) void up_action(float motospeed)
{ {
printf("Up_action sp:%.2f \n",motospeed); ESP_LOGD(MOUDLENAME,"Up_action sp:%.2f",motospeed);
if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop) if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
{ {
@ -672,19 +714,21 @@ void up_action(float motospeed)
// 放线按钮--单击 // 放线按钮--单击
void downbtn_click() void downbtn_click()
{ {
Serial.println("Down_click"); ESP_LOGD(MOUDLENAME,"Down_click");
_checkweightcal=true;//检测是否需要校准称重传感器--在下放停止时检测
_checkweighttimes=0;
down_action(SPEED_BTN_SLOW,10); down_action(SPEED_BTN_SLOW,10);
} }
// 放线按钮--双击 // 放线按钮--双击
void downbtn_dbclick() void downbtn_dbclick()
{ {
Serial.println("Down_dbclick"); ESP_LOGD(MOUDLENAME,"Down_dbclick");
down_action(SPEED_BTN_MID); down_action(SPEED_BTN_MID);
} }
// 放线按钮--长按 // 放线按钮--长按
void downbtn_pressstart() void downbtn_pressstart()
{ {
Serial.println("Down_pressstart"); ESP_LOGD(MOUDLENAME,"Down_pressstart");
//两个同时按用于对频 //两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce>2) btn_pressatonce=2;
@ -698,7 +742,7 @@ void downbtn_pressstart()
// 放线按钮--长按抬起 // 放线按钮--长按抬起
void downbtn_pressend() void downbtn_pressend()
{ {
Serial.println("Down_pressend"); ESP_LOGD(MOUDLENAME,"Down_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0; if (btn_pressatonce<0) btn_pressatonce=0;
//不是恢复自动放线抬起按键就停止 //不是恢复自动放线抬起按键就停止
@ -710,19 +754,19 @@ void downbtn_pressend()
void upbtn_click() void upbtn_click()
{ {
// fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click"); // fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
Serial.println("UP_click"); ESP_LOGD(MOUDLENAME,"UP_click");
up_action(SPEED_BTN_SLOW); up_action(SPEED_BTN_SLOW);
} }
// 收线按钮-双击 // 收线按钮-双击
void upbtn_dbclick() void upbtn_dbclick()
{ {
Serial.println("UP_dbclick"); ESP_LOGD(MOUDLENAME,"UP_dbclick");
up_action(SPEED_BTN_MID); up_action(SPEED_BTN_MID);
} }
// 两个按钮同时按下 // 两个按钮同时按下
void btn_presssatonce() void btn_presssatonce()
{ {
Serial.println("UP_presssatonce"); ESP_LOGD(MOUDLENAME,"UP_presssatonce");
led_show(255,255, 255); // 高亮一下 led_show(255,255, 255); // 高亮一下
fc.playText("[v1]发送对频信息"); fc.playText("[v1]发送对频信息");
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
@ -730,7 +774,7 @@ void btn_presssatonce()
// 收线按钮-长按 // 收线按钮-长按
void upbtn_pressstart() void upbtn_pressstart()
{ {
Serial.println("UP_pressstart"); ESP_LOGD(MOUDLENAME,"UP_pressstart");
//两个同时按用于对频 //两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce>2) btn_pressatonce=2; if (btn_pressatonce>2) btn_pressatonce=2;
@ -744,7 +788,7 @@ void upbtn_pressstart()
// 收线按钮-长按抬起 // 收线按钮-长按抬起
void upbtn_pressend() void upbtn_pressend()
{ {
Serial.println("UP_pressend"); ESP_LOGD(MOUDLENAME,"UP_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce<0) btn_pressatonce=0; if (btn_pressatonce<0) btn_pressatonce=0;
@ -756,28 +800,29 @@ void Hook_autodown(float length_cm)
{ {
if (motocontrol.gethooktatus()==HS_Locked) if (motocontrol.gethooktatus()==HS_Locked)
{ {
printf("Hook_autodown %.2f cm \n",length_cm); ESP_LOGD(MOUDLENAME,"Hook_autodown %.2f cm",length_cm);
fc.Camera_action_down();
motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40
}else }else
Serial.println("autodown fault, not HS_Locked "); ESP_LOGE(MOUDLENAME,"autodown fault, not HS_Locked ");
} }
void Hook_stop() void Hook_stop()
{ {
Serial.println("Hook_stop"); ESP_LOGD(MOUDLENAME,"Hook_stop");
motocontrol.stoprun(); motocontrol.stoprun();
} }
void Hook_resume() void Hook_resume()
{ {
if (motocontrol.gethooktatus()==HS_Stop) if (motocontrol.gethooktatus()==HS_Stop)
{ {
printf("Hook_resume\n"); ESP_LOGD(MOUDLENAME,"Hook_resume");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
}else }else
Serial.println("resume fault, not HS_Stop "); ESP_LOGE(MOUDLENAME,"resume fault, not HS_Stop ");
} }
void Hook_recovery() void Hook_recovery()
{ {
Serial.println("Hook_recovery"); ESP_LOGD(MOUDLENAME,"Hook_recovery");
if (motocontrol.gethooktatus()!=HS_Locked) if (motocontrol.gethooktatus()!=HS_Locked)
{ {
motocontrol.stoprun(); motocontrol.stoprun();
@ -788,25 +833,25 @@ void Hook_recovery()
// 测试按钮 // 测试按钮
void testbtn_click() void testbtn_click()
{ {
Serial.println("testbtn_click"); ESP_LOGD(MOUDLENAME,"testbtn_click");
switch (motocontrol.gethooktatus()) switch (motocontrol.gethooktatus())
{ {
case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放 case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
{ {
Serial.println("moto_goodsdown"); ESP_LOGD(MOUDLENAME,"moto_goodsdown");
motocontrol.moto_goodsdown(22); // 二楼340 //桌子40 motocontrol.moto_goodsdown(22); // 二楼340 //桌子40
break; break;
} }
case HS_Stop: case HS_Stop:
{ {
Serial.println("moto_resume"); ESP_LOGD(MOUDLENAME,"moto_resume");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
break; break;
} }
default: default:
{ {
Serial.println("stop"); ESP_LOGD(MOUDLENAME,"stop");
motocontrol.stoprun(); motocontrol.stoprun();
} }
} }
@ -925,12 +970,12 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
}else }else
if (cutTopic == topicSub[8]) { //8:钩子控制 if (cutTopic == topicSub[8]) { //8:钩子控制
uint16_t strInt = (uint16_t)topicStr.toInt(); // uint16_t strInt = (uint16_t)topicStr.toInt(); //
printf("hookcontrol command: %d \n", strInt); ESP_LOGD(MOUDLENAME,"hookcontrol command: %d", strInt);
switch (strInt) switch (strInt)
{ {
case 0: //收 case 0: //收
/* code */ /* code */
printf("Mqtt_HOOK_UP \n"); ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_UP");
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
up_action(SPEED_BTN_FAST); up_action(SPEED_BTN_FAST);
break; break;
@ -939,13 +984,13 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
break; break;
case 2: //暂停 case 2: //暂停
/* code */ /* code */
printf("Mqtt_HOOK_PAUSE \n"); ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_PAUSE");
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
Hook_stop(); Hook_stop();
break; break;
case 3: //继续 case 3: //继续
/* code */ /* code */
printf("Mqtt_HOOK_resume \n"); ESP_LOGD(MOUDLENAME,"Mqtt_HOOK_resume");
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
Hook_resume(); Hook_resume();
break; break;
@ -957,36 +1002,29 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
JsonObject obj = doc.as<JsonObject>(); JsonObject obj = doc.as<JsonObject>();
//相机控制 //相机控制
uint8_t item=obj["item"]; uint8_t item=obj["item"];
switch (item)
size_t len; {
if (item ==0) { //0:一键回中 //一键回中
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01 }; case 0:
len = sizeof(command); fc.Camera_action_ret();
fc.udpSendToCamera(command, len); break;
//1:变焦
} else if (item == 1) { //1:变焦 case 1:
uint8_t command[] = { 0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00 }; fc.Camera_action_zoom(obj["val"]);
command[8] = obj["val"]; break;
len = sizeof(command); //2:俯仰,旋转
fc.udpSendToCamera(command, len); case 2:
} else if (item == 2) { //2:俯仰,旋转 fc.Camera_action_move(obj["pitch"],obj["yaw"]);
uint8_t command[] = { 0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00 }; break;
int8_t pitchvalue=obj["pitch"];
int8_t yawvalue=obj["yaw"];
command[9] =pitchvalue; //俯仰
command[8] =yawvalue; //旋转
len = sizeof(command);
fc.udpSendToCamera(command, len);
// printf("camercontrol:pith:%d,yaw:%d\n",pitchvalue,yawvalue);
} }
}else if (cutTopic == topicSub[10]) { //通用命令 }else if (cutTopic == topicSub[10]) { //通用命令
// json 反序列化 // json 反序列化
DynamicJsonDocument doc(0x2FFF); DynamicJsonDocument doc(0x2FFF);
deserializeJson(doc, topicStr); deserializeJson(doc, topicStr);
JsonObject obj = doc.as<JsonObject>(); JsonObject obj = doc.as<JsonObject>();
uint8_t cmd_id=obj["id"]; uint8_t cmd_id=obj["id"];
printf("Mqtt_cmd: %d \n",cmd_id); ESP_LOGD(MOUDLENAME,"Mqtt_cmd: %d",cmd_id);
switch (cmd_id) switch (cmd_id)
{ {
//校准称重 //校准称重
@ -1005,7 +1043,7 @@ void mqtt_receiveCallback(char* topic, byte* payload, unsigned int length) {
void writeRoute(String topicStr) { void writeRoute(String topicStr) {
if (fc.writeState) // 如果正在写入状态 跳出 if (fc.writeState) // 如果正在写入状态 跳出
{ {
fc.logln((char*)"正在写航点"); // 提示正在写入中 ESP_LOGD(MOUDLENAME,"正在写航点"); // 提示正在写入中
return; return;
} }
//改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到 //改变飞机状态 PS:mqtt发送写在这里 同步执行下面while循环时 写在loop函数里面 不会被执行到
@ -1026,7 +1064,7 @@ void writeRoute(String topicStr) {
fc.comm_receive(mavlink_receiveCallback); fc.comm_receive(mavlink_receiveCallback);
if (fc.missionArkType == 0) { //写入成功 if (fc.missionArkType == 0) { //写入成功
fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态 fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态
fc.logln((char*)"misson_bingo..."); ESP_LOGD(MOUDLENAME,"misson_bingo...");
//改变飞机状态 //改变飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 1); //航点写入成功状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
@ -1034,7 +1072,7 @@ void writeRoute(String topicStr) {
break; break;
} else if (fc.missionArkType > 0) { //写入失败 } else if (fc.missionArkType > 0) { //写入失败
fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态 fc.missionArkType = -1; //“航点写入是否成功” 改成-1默认状态
fc.logln((char*)"misson_error..."); ESP_LOGE(MOUDLENAME,"misson_error...");
//改变飞机状态 //改变飞机状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); //航点写入失败状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 2, 0); //航点写入失败状态
topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态 topicPubMsg[10] = fc.setNBit(topicPubMsg[10], 1, 0); //结束写入状态
@ -1063,7 +1101,7 @@ void writeRoute(String topicStr) {
// if (fc.writeSeq==0) // if (fc.writeSeq==0)
//fc.playText(fc.questVoiceStr); //fc.playText(fc.questVoiceStr);
} }
fc.logln((char*)"frame--"); ESP_LOGD(MOUDLENAME,"frame--");
fc.logln(frame); fc.logln(frame);
//写入航点 //写入航点
fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); fc.mav_mission_item(fc.writeSeq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
@ -1101,7 +1139,7 @@ void mavlink_receiveCallback(uint8_t c) {
if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁 if (heartbeat.base_mode & 128) { //从心跳里面 判断已经解锁
if (!curr_armed) if (!curr_armed)
{ {
printf("armed\n"); ESP_LOGD(MOUDLENAME,"armed");
fc.playText("[v1]已解锁"); fc.playText("[v1]已解锁");
} }
curr_armed=true; curr_armed=true;
@ -1109,7 +1147,7 @@ void mavlink_receiveCallback(uint8_t c) {
} else { } else {
if (curr_armed) if (curr_armed)
{ {
printf("disarm\n"); ESP_LOGD(MOUDLENAME,"disarm");
fc.playText("[v1]已加锁"); fc.playText("[v1]已加锁");
} }
curr_armed=false; //心跳里面 判断没有解锁 curr_armed=false; //心跳里面 判断没有解锁
@ -1314,10 +1352,8 @@ void mavlink_receiveCallback(uint8_t c) {
mavlink_mission_request_t mission_request; // 解构的数据放到这个对象 mavlink_mission_request_t mission_request; // 解构的数据放到这个对象
mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据 mavlink_msg_mission_request_decode(&msg, &mission_request); // 解构msg数据
//日志 //日志
fc.logln((char*)"MsgID:"); ESP_LOGD(MOUDLENAME,"MsgID:%d No:%d",msg.msgid,mission_request.seq);
fc.logln(msg.msgid);
fc.logln((char*)"No:");
fc.logln(mission_request.seq);
//飞控 反馈当前要写入航点的序号 //飞控 反馈当前要写入航点的序号
fc.writeSeq = mission_request.seq; fc.writeSeq = mission_request.seq;
} }
@ -1328,10 +1364,8 @@ void mavlink_receiveCallback(uint8_t c) {
mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象 mavlink_mission_ack_t mission_ark; // 解构的数据放到这个对象
mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据 mavlink_msg_mission_ack_decode(&msg, &mission_ark); // 解构msg数据
/*日志*/ /*日志*/
fc.logln((char*)"MsgID:"); ESP_LOGD(MOUDLENAME,"MsgID:%d re:",msg.msgid,mission_ark.type);
fc.logln(msg.msgid);
fc.logln((char*)"re:");
fc.logln(mission_ark.type);
/*记录航点的写入状态 */ /*记录航点的写入状态 */
fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败 fc.missionArkType = mission_ark.type; //0：写入成功 非0表示失败
/*当有成果反馈之后 初始化下列数据*/ /*当有成果反馈之后 初始化下列数据*/
@ -1345,54 +1379,74 @@ void mavlink_receiveCallback(uint8_t c) {
{ {
uint16_t fc_hook_cmd=mavlink_msg_command_long_get_command(&msg); uint16_t fc_hook_cmd=mavlink_msg_command_long_get_command(&msg);
uint16_t rngalt_cm= (uint16_t)mavlink_msg_command_long_get_param1(&msg); uint16_t rngalt_cm= (uint16_t)mavlink_msg_command_long_get_param1(&msg);
printf("COMMAND_LONG ID:%d,rng:%dcm \n",fc_hook_cmd,rngalt_cm); HookStatus hss=motocontrol.gethooktatus();
ESP_LOGD(MOUDLENAME,"COMMAND_LONG ID:%d,rng:%dcm",fc_hook_cmd,rngalt_cm);
//如果没初始化就退出
if (initstatus != IS_OK)
{
ESP_LOGE(MOUDLENAME,"not initstatus:%d",initstatus);
break;
}
switch (fc_hook_cmd) switch (fc_hook_cmd)
{ {
//自动放线 //自动放线
case MAV_CMD_FC_HOOK_AUTODOWN: case MAV_CMD_FC_HOOK_AUTODOWN:
{ {
HookStatus hss=motocontrol.gethooktatus(); // HookStatus hss=motocontrol.gethooktatus();
printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n",hss,rngalt_cm); ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm",hss,rngalt_cm);
//没有激光高度直接退出 //没有激光高度直接退出
if (rngalt_cm==0) if (rngalt_cm==0)
{ {
printf("exit rngalt_cm==0"); ESP_LOGE(MOUDLENAME,"exit rngalt_cm==0");
break; break;
} }
if (hss==HS_Locked) if (hss==HS_Locked)
{ {
Hook_autodown(rngalt_cm); Hook_autodown(rngalt_cm);
//语音播报3次 //最大音量语音播报1次
if (fc.questVoiceStr!="") if (fc.questVoiceStr!="")
fc.playText(fc.questVoiceStr+"。"+fc.questVoiceStr+"。"+fc.questVoiceStr); fc.playText("[v10]"+fc.questVoiceStr);
} }
else else
{ {
if (hss==HS_Stop) if (hss==HS_Stop)
Hook_resume(); Hook_resume();
else else
printf("exit hooktatus error"); ESP_LOGE(MOUDLENAME,"exit hooktatus error");
} }
break; break;
} }
//暂停 //暂停
case MAV_CMD_FC_HOOK_PAUSE: case MAV_CMD_FC_HOOK_PAUSE:
{ {
printf("MAV_CMD_FC_HOOK_PAUSE \n"); ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_PAUSE");
Hook_stop(); Hook_stop();
break; break;
} }
//暂停 //暂停
case MAV_CMD_FC_HOOK_RECOVERY: case MAV_CMD_FC_HOOK_RECOVERY:
{ {
printf("MAV_CMD_FC_HOOK_RECOVERY \n"); ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_HOOK_RECOVERY ");
Hook_recovery(); Hook_recovery();
break; break;
} }
//开始下降
case MAV_CMD_FC_DESCENTSTART:
{
ESP_LOGD(MOUDLENAME,"MAV_CMD_FC_DESCENTSTART ");
fc.Camera_action_down();
break;
}
} }
} }
break; break;
//当前执行到的任务号
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
{
break;
}
default: default:
break; break;
} }

6
src/moto.cpp
View File

@ -5,20 +5,20 @@
uint8_t CaninBuffer[8]; // 接收指令缓冲区 uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis; moto_measure_t moto_chassis;
PID_TypeDef moto_pid; PID_TypeDef moto_pid;
static const char* MOUDLENAME = "MOTO";
moto::moto() moto::moto()
{ {
_closed = true; _closed = true;
} }
bool moto::init() bool moto::init()
{ {
Serial.println("init moto"); ESP_LOGD(MOUDLENAME,"init moto");
pid_init(); pid_init();
CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX); CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
// start the CAN bus at 500 kbps // start the CAN bus at 500 kbps
if (!CAN.begin(1000E3)) if (!CAN.begin(1000E3))
{ {
Serial.println("Starting CAN failed!"); ESP_LOGE(MOUDLENAME,"Starting CAN failed!");
return false; return false;
} }
CAN.onReceive(onReceive); CAN.onReceive(onReceive);

2
src/moto.h
View File

@ -2,7 +2,7 @@
#include <Arduino.h> #include <Arduino.h>
#include <CAN.h> #include <CAN.h>
#include <ESP32SJA1000.h> #include <ESP32SJA1000.h>
#define MOTO_REVERSED 1 // 正反转1为正转，-1为反转 #define MOTO_REVERSED -1 // 正反转1为正转，-1为反转
#define MOTO_REDUCTION 36 #define MOTO_REDUCTION 36
#define MOTO_MAXANG 8192 // 0-8191 #define MOTO_MAXANG 8192 // 0-8191
#define MOTO_MAXANG_HALF MOTO_MAXANG / 2 #define MOTO_MAXANG_HALF MOTO_MAXANG / 2

55
src/motocontrol.cpp
View File

@ -6,6 +6,8 @@
#include "config.h" #include "config.h"
#define DEBUG_OUT false #define DEBUG_OUT false
static const char* MOUDLENAME = "MOTO_C";
Motocontrol::Motocontrol() Motocontrol::Motocontrol()
{ {
_controlstatus.is_instore = false; _controlstatus.is_instore = false;
@ -139,13 +141,13 @@ void Motocontrol::checkgoods() // 检测是否超重
} }
void Motocontrol::lockservo() // 锁定舵机 void Motocontrol::lockservo() // 锁定舵机
{ {
printf("start_lockservo\n"); ESP_LOGD(MOUDLENAME,"start_lockservo");
_servotatus = SS_WaitMotoStop; _servotatus = SS_WaitMotoStop;
_tm_servotatus = millis(); _tm_servotatus = millis();
} }
void Motocontrol::unlockservo() // 解锁舵机 void Motocontrol::unlockservo() // 解锁舵机
{ {
printf("unlockservo\n"); ESP_LOGD(MOUDLENAME,"unlockservo");
// 解锁操作 // 解锁操作
_lockservo->write(SERVO_UNLOCKPOS); _lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动 _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
@ -155,7 +157,7 @@ void Motocontrol::unlockservo() // 解锁舵机
} }
void Motocontrol::stoprun(float acctime) // 停止 void Motocontrol::stoprun(float acctime) // 停止
{ {
printf("stoprun after time:%.2f \n",acctime); ESP_LOGD(MOUDLENAME,"stoprun after time:%.2f",acctime);
_moto_dji.settime_acc(acctime); _moto_dji.settime_acc(acctime);
_moto_dji.setspeedtarget(0.0f); _moto_dji.setspeedtarget(0.0f);
_controlstatus.motostatus = MS_Stop; _controlstatus.motostatus = MS_Stop;
@ -173,7 +175,7 @@ void Motocontrol::stopautodown()
void Motocontrol::sethooksstatus(HookStatus hookstatus) void Motocontrol::sethooksstatus(HookStatus hookstatus)
{ {
_hooksstatus=hookstatus; _hooksstatus=hookstatus;
printf("Set HS:%s \n",gethooktatus_str(false)); ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false));
} }
void Motocontrol::setlocked(bool locked) void Motocontrol::setlocked(bool locked)
{ {
@ -218,14 +220,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
// 如果没有货物--开始回收 // 如果没有货物--开始回收
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
printf("Not goods wait %d ms for unhook \n",HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME,"Not goods wait %d ms for unhook",HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
break; break;
} }
if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt)) if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
{ {
printf("HS_Down target is arrived curr: %.2f \n",mf.output_round_cnt); ESP_LOGD(MOUDLENAME,"HS_Down target is arrived curr: %.2f",mf.output_round_cnt);
sethooksstatus(HS_DownSlow); sethooksstatus(HS_DownSlow);
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(SPEED_HOOK_SLOW); set_hook_speed(SPEED_HOOK_SLOW);
@ -237,7 +239,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
printf("Not havegoods wait %d ms unhook \n",HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME,"Not havegoods wait %d ms unhook",HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
} }
@ -248,7 +250,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
{ {
printf("HS_WaitUnhook ok startup \n"); ESP_LOGD(MOUDLENAME,"HS_WaitUnhook ok startup");
_moto_dji.settime_acc(1000); _moto_dji.settime_acc(1000);
set_hook_speed(-SPEED_HOOK_UP); set_hook_speed(-SPEED_HOOK_UP);
sethooksstatus(HS_Up); sethooksstatus(HS_Up);
@ -314,14 +316,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
if (!checkservoblock() || (_unblocktimes > 2)) if (!checkservoblock() || (_unblocktimes > 2))
{ {
printf("Close moto \n"); ESP_LOGD(MOUDLENAME,"Close moto");
_moto_dji.close(); _moto_dji.close();
_servotatus = SS_ServoLocked; _servotatus = SS_ServoLocked;
} }
else else
// 堵转 // 堵转
{ {
printf("SS servoblock unlock servo and turn moto \n"); ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto");
_tm_servotatus = millis(); _tm_servotatus = millis();
_servotatus = SS_WaitUnBlock; _servotatus = SS_WaitUnBlock;
// 写一个不会堵转的位置 // 写一个不会堵转的位置
@ -337,7 +339,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if ((millis() - _tm_servotatus) > TM_UNBLOCK) if ((millis() - _tm_servotatus) > TM_UNBLOCK)
{ {
printf("SS lock servo \n"); ESP_LOGD(MOUDLENAME,"SS lock servo");
// 继续锁定等待舵机检测 // 继续锁定等待舵机检测
_lockservo->write(SERVO_LOCKPOS); _lockservo->write(SERVO_LOCKPOS);
_servotatus = SS_WaitServo; _servotatus = SS_WaitServo;
@ -371,7 +373,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(-SPEED_INSTORE); set_hook_speed(-SPEED_INSTORE);
_controlstatus.is_instore = true; _controlstatus.is_instore = true;
printf("Instorelen:%.2f,speed:%.2f \n", instlen, mf.output_speed_rpm); ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
} }
} }
else else
@ -387,7 +389,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt > _target_cnt) if (mf.output_round_cnt > _target_cnt)
{ {
printf("stop down tar:%.2f\n", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME,"stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -395,7 +397,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt < _target_cnt) if (mf.output_round_cnt < _target_cnt)
{ {
printf("stop up tar:%.2f\n", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME,"stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -410,13 +412,8 @@ void Motocontrol::set_hook_speed(float hooksspeed)
} }
void Motocontrol::update() // 更新 void Motocontrol::update() // 更新
{ {
// printf("3.01 \n");
_moto_dji.update(); _moto_dji.update();
// printf("3.02 \n");
checkmotostatus(); checkmotostatus();
// printf("3.03 \n");
checkhookstatus(); checkhookstatus();
} }
void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用 void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
@ -457,24 +454,24 @@ void Motocontrol::moto_goodsdown(float length)
{ {
if (length < 0.0) if (length < 0.0)
{ {
printf("length<0 fault\n"); ESP_LOGD(MOUDLENAME,"length<0 fault");
return; return;
} }
// 没设置零点 // 没设置零点
if (!_controlstatus.is_setzero) if (!_controlstatus.is_setzero)
{ {
printf("not lengthzero fault\n"); ESP_LOGD(MOUDLENAME,"not lengthzero fault");
return; return;
} }
if (!_weightalign) if (!_weightalign)
{ {
printf("weight not align fault\n"); ESP_LOGD(MOUDLENAME,"weight not align fault");
return; return;
} }
// 没挂东西 // 没挂东西
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
printf("goods min fault:%d \n", _pullweight); ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight);
return; return;
} }
@ -495,7 +492,7 @@ void Motocontrol::moto_goodsdown(float length)
tarleng-=HOOK_SLOWDOWN_LENGTH; tarleng-=HOOK_SLOWDOWN_LENGTH;
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt; _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
_goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER; _goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER;
printf("start down %.2f cm,tar:%.2f \n",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); setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
_goods_speed = SPEED_HOOK_FAST; _goods_speed = SPEED_HOOK_FAST;
} }
@ -515,25 +512,25 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
// 运动中暂时不管 // 运动中暂时不管
if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up)) if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
{ {
printf("motostatus is MS_Down\\MS_Up \n"); ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up");
return; return;
} }
// 没设置速度不转 // 没设置速度不转
if (_controlstatus.speed_targe == 0) if (_controlstatus.speed_targe == 0)
{ {
printf("not set speed_targe \n"); ESP_LOGD(MOUDLENAME,"not set speed_targe");
return; return;
} }
if (updown == MS_Up) if (updown == MS_Up)
{ {
if (_controlstatus.is_toplocked) if (_controlstatus.is_toplocked)
{ {
printf("is_toplocked return\n"); ESP_LOGD(MOUDLENAME,"is_toplocked return");
return; return;
} }
if (_controlstatus.is_overweight) if (_controlstatus.is_overweight)
{ {
printf("overweight fault :%d \n", _pullweight); ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight);
return; return;
} }
sethooksstatus(HS_Up); sethooksstatus(HS_Up);
@ -570,7 +567,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
runspeed = -_controlstatus.speed_targe; runspeed = -_controlstatus.speed_targe;
} }
// 开始转 // 开始转
// printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt); // ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
_runspeed = runspeed; _runspeed = runspeed;
// _moto_dji.setspeedtarget(runspeed); // _moto_dji.setspeedtarget(runspeed);
} }

6
src/motocontrol.h
View File

@ -29,9 +29,9 @@
#define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms #define TM_SERVOLOCK 300 // 舵机转到Lock和Unlock等待的时间ms
#define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些 #define TM_ACC_HS 500 // 货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些
#define SERVO_LOCKPOS 1920 // 舵机锁定位置 #define SERVO_LOCKPOS 1000 //1920 // 舵机锁定位置
#define SERVO_UNLOCKPOS 1800 // 舵机解锁位置 #define SERVO_UNLOCKPOS 1120 //1800 // 舵机解锁位置
#define SERVO_BLOCKUNLOCKPOS 1700 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小 #define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
#define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克) #define HOOK_WEIHT_MIN 100 // 最小货物重量 小于这个认为没挂东西 (克)