71 lines
1.9 KiB
C
71 lines
1.9 KiB
C
|
#pragma once
|
||
|
|
#include "Arduino.h"
|
||
|
|
#include "config.h"
|
||
|
|
#include "moto.h"
|
||
|
|
#define ROPE_MAXLENGTH 600 //最多能放600cm---实际绳子应该比这个长650之类的
|
||
|
|
#define WHEEL_DIAMETER 4.0 //轮子直径cm
|
||
|
|
#define INSTORE_LENGTH 15 //入仓长度,低于该长度要缓慢入仓 cm
|
||
|
|
|
||
|
|
#define SPEED_MAX 330 //最快收放线速度
|
||
|
|
#define SPEED_MIN 3 //入仓速度
|
||
|
|
#define SPEED_SLOW 20 //慢收放线速度
|
||
|
|
#define SPEED_FAST 100 //快收放线速度
|
||
|
|
#define SPEED_BTN_FAST 200 //最快收放线速度
|
||
|
|
|
||
|
|
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长
|
||
|
|
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// 挂钩状态
|
||
|
|
enum MotoStatus
|
||
|
|
{
|
||
|
|
MS_Down, // 自动下放中
|
||
|
|
MS_Up, // 回收中
|
||
|
|
MS_Stop //停止中
|
||
|
|
};
|
||
|
|
|
||
|
|
typedef struct
|
||
|
|
{
|
||
|
|
bool is_instore;
|
||
|
|
bool is_setzero;
|
||
|
|
bool is_toplocked;
|
||
|
|
float zero_cnt;
|
||
|
|
float speed_targe;
|
||
|
|
MotoStatus motostatus;
|
||
|
|
|
||
|
|
} control_status_t;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
class Motocontrol
|
||
|
|
{
|
||
|
|
private:
|
||
|
|
moto _moto_dji;
|
||
|
|
float _start_cnt;
|
||
|
|
float _target_cnt;
|
||
|
|
control_status_t _controlstatus;
|
||
|
|
bool _check_targetlength;
|
||
|
|
|
||
|
|
void checkstatus();
|
||
|
|
public:
|
||
|
|
Motocontrol(); // 构造函数
|
||
|
|
~Motocontrol(); // 析构函数
|
||
|
|
void setspeed(float motospeed); // 设置速度
|
||
|
|
void down(float length); // 收线
|
||
|
|
void up(float length); // 放线
|
||
|
|
void stop(); // 停止
|
||
|
|
void setzero(); // 设置0长度位置
|
||
|
|
int16_t getlength(); // 得到长度
|
||
|
|
void update(); // 更新
|
||
|
|
bool init(); // 初始化
|
||
|
|
void direct_speed(float motospeed); //直接设置电机速度--立马生效,仅用于测试
|
||
|
|
moto_measure_t getmotoinfo(); //得到电机信息
|
||
|
|
control_status_t getcontrolstatus(); //得到控制信息
|
||
|
|
void setlocked(bool locked); //是否到顶锁定
|
||
|
|
void moto_run(MotoStatus updown,float length= 0.0f) ;// 放线
|
||
|
|
|
||
|
|
};
|