199 lines
6.7 KiB
C++
199 lines
6.7 KiB
C++
#include "TopDefine.h"
|
|
#include "take.hpp"
|
|
#include "remote_control.h"
|
|
#include "calc_lib.h"
|
|
extern RC_ctrl_t rc_ctrl;
|
|
const float Take::TakeRing_speed_PID[3]={10, 0 ,0}; //3508P,I,D(速度环)
|
|
const float Take::TakeRing_angle_PID[3]={10.0, 0 ,0}; //3508P,I,D(角度环)
|
|
const float Take::M3508_speed_PID[3]={12.0,0.01 ,0}; //两边上下pid
|
|
const float Take::M3508_angle_PID[3]={16, 0.0 ,0}; //3508P,I,D(角度环)
|
|
const float Take::Turn_speed_PID[3]={10,0.0,0}; //2006P,I,D(速度环)
|
|
const float Take::Turn_angle_PID[3]={14,0.0,0}; //2006P,I,D(角度环)
|
|
#define CURRENT_UP 1600
|
|
#define CURRENT_DOWN 700
|
|
#define CURRENT_TOP 2200
|
|
#define CURRENT_FALL 30
|
|
#define DEBUG_TAKE 0
|
|
int aaa=0;
|
|
Take::Take()
|
|
{
|
|
for(int i = 0;i < MOTOR_NUM;i ++)
|
|
{
|
|
putMotor[i] = get_motor_point(i);//获取电机数据指针
|
|
if(0 == i)
|
|
{
|
|
putMotor[i]->type = M3508;//设置电机类型
|
|
PID_init(&angle_pid[i],PID_POSITION,TakeRing_angle_PID,3000,1000);//pid初始化
|
|
PID_init(&speed_pid[i],PID_POSITION,TakeRing_speed_PID,7000,2000);//pid初始化
|
|
}
|
|
else if(1 == i)
|
|
{
|
|
putMotor[i]->type = M3508;//设置电机类型
|
|
PID_init(&angle_pid[i],PID_POSITION,Turn_angle_PID,700,0);//pid初始化
|
|
PID_init(&speed_pid[i],PID_POSITION,Turn_speed_PID,4000,1000);//pid初始化
|
|
}
|
|
else
|
|
{
|
|
putMotor[i]->type = M3508;//设置电机类型
|
|
PID_init(&angle_pid[i],PID_POSITION,M3508_angle_PID,800,0);//pid初始化
|
|
PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,6000,1000);//pid初始化
|
|
}
|
|
result[i] = 0;
|
|
angleSet[i] = 0;
|
|
|
|
}
|
|
}
|
|
#if DEBUG_TAKE == 1
|
|
//int16_t current_fall = -30;
|
|
//void Take::fall()
|
|
//{
|
|
// if(putMotor[MOTOR_UP]->total_angle < -10)
|
|
// {result[MOTOR_UP] = current_fall;}
|
|
// else{
|
|
// int16_t delta[1];
|
|
// //下降
|
|
// angleSet[MOTOR_UP] = POINT_BUTTOM;
|
|
//
|
|
// delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
|
|
// result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
|
|
// }
|
|
// // result[MOTOR_UP] = current_fall;
|
|
//}
|
|
#else
|
|
void Take::fall()
|
|
{
|
|
|
|
// if(putMotor[MOTOR_UP]->total_angle > -10)
|
|
// {result[MOTOR_UP] = -CURRENT_FALL;}
|
|
// else{
|
|
int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_UP] = aaa;
|
|
|
|
delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
|
|
result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
|
|
// }
|
|
|
|
}
|
|
#endif
|
|
void Take::mid()
|
|
{
|
|
int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_UP] = POINT_MID;
|
|
|
|
delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
|
|
result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
|
|
|
|
}
|
|
#if DEBUG_TAKE == 1
|
|
int16_t current_top = 1600;
|
|
void Take::top()
|
|
{
|
|
//int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_UP] = POINT_TOP;
|
|
|
|
// delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
|
|
// result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
|
|
result[MOTOR_UP] = -current_top;
|
|
}
|
|
#else
|
|
void Take::top()
|
|
{
|
|
//int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_UP] = POINT_TOP;
|
|
|
|
// delta[MOTOR_UP] = PID_calc(&angle_pid[MOTOR_UP],putMotor[MOTOR_UP]->total_angle,angleSet[MOTOR_UP]);
|
|
// result[MOTOR_UP] = PID_calc(&speed_pid[MOTOR_UP], putMotor[MOTOR_UP]->speed_rpm, delta[MOTOR_UP]);
|
|
result[MOTOR_UP] = -CURRENT_TOP;
|
|
}
|
|
#endif
|
|
void Take::putRight()
|
|
{
|
|
// int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_RING_RIGHT] = POINT_PUT;
|
|
|
|
// delta[MOTOR_RING_RIGHT] = PID_calc(&angle_pid[MOTOR_RING_RIGHT],putMotor[MOTOR_RING_RIGHT]->total_angle,angleSet[MOTOR_RING_RIGHT]);
|
|
// result[MOTOR_RING_RIGHT] = PID_calc(&speed_pid[MOTOR_RING_RIGHT], putMotor[MOTOR_RING_RIGHT]->speed_rpm, delta[MOTOR_RING_RIGHT]);
|
|
result[MOTOR_RING_RIGHT] = CURRENT_DOWN-100;
|
|
}
|
|
|
|
void Take::putLeft()
|
|
{
|
|
// int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_RING_LEFT] = -POINT_PUT;
|
|
|
|
// delta[MOTOR_RING_LEFT] = PID_calc(&angle_pid[MOTOR_RING_LEFT],putMotor[MOTOR_RING_LEFT]->total_angle,angleSet[MOTOR_RING_LEFT]);
|
|
// result[MOTOR_RING_LEFT] = PID_calc(&speed_pid[MOTOR_RING_LEFT], putMotor[MOTOR_RING_LEFT]->speed_rpm, delta[MOTOR_RING_LEFT]);
|
|
result[MOTOR_RING_LEFT] = -CURRENT_DOWN;
|
|
}
|
|
|
|
void Take::takeRight()
|
|
{
|
|
// int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_RING_RIGHT] = POINT_TAKE;
|
|
|
|
// delta[MOTOR_RING_RIGHT] = PID_calc(&angle_pid[MOTOR_RING_RIGHT],putMotor[MOTOR_RING_RIGHT]->total_angle,angleSet[MOTOR_RING_RIGHT]);
|
|
// result[MOTOR_RING_RIGHT] = PID_calc(&speed_pid[MOTOR_RING_RIGHT], putMotor[MOTOR_RING_RIGHT]->speed_rpm, delta[MOTOR_RING_RIGHT]);
|
|
result[MOTOR_RING_RIGHT] = -CURRENT_UP-150;
|
|
}
|
|
|
|
void Take::takeLeft()
|
|
{
|
|
// int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_RING_LEFT] = -POINT_TAKE;
|
|
|
|
// delta[MOTOR_RING_LEFT] = PID_calc(&angle_pid[MOTOR_RING_LEFT],putMotor[MOTOR_RING_LEFT]->total_angle,angleSet[MOTOR_RING_LEFT]);
|
|
// result[MOTOR_RING_LEFT] = PID_calc(&speed_pid[MOTOR_RING_LEFT], putMotor[MOTOR_RING_LEFT]->speed_rpm, delta[MOTOR_RING_LEFT]);
|
|
result[MOTOR_RING_LEFT] = CURRENT_UP;
|
|
}
|
|
|
|
void Take::right()
|
|
{
|
|
int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_TURN] = POINT_TURN_RIGHT;
|
|
|
|
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
|
|
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
|
|
|
|
}
|
|
|
|
void Take::left()
|
|
{
|
|
int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_TURN] = POINT_TURN_LEFT;
|
|
|
|
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
|
|
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
|
|
|
|
}
|
|
|
|
void Take::turnMid()
|
|
{
|
|
int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_TURN] = POINT_TURN_ZERO;
|
|
|
|
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
|
|
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
|
|
|
|
}
|
|
|
|
void Take::turnFlow()
|
|
{
|
|
int16_t delta[1];
|
|
//下降
|
|
angleSet[MOTOR_TURN] = map(rc_ctrl.sw[6],308,1694,POINT_TURN_LEFT-200,POINT_TURN_RIGHT+200);
|
|
|
|
delta[MOTOR_TURN] = PID_calc(&angle_pid[MOTOR_TURN],putMotor[MOTOR_TURN]->total_angle,angleSet[MOTOR_TURN]);
|
|
result[MOTOR_TURN] = PID_calc(&speed_pid[MOTOR_TURN], putMotor[MOTOR_TURN]->speed_rpm, delta[MOTOR_TURN]);
|
|
}
|