#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]);
}