rgv-kinco/applications/nav/dl-dwd.c

/*
 * dl-dwd.c
 *
 *  Created on: 2019��6��20��
 *      Author: Eric
 */

#include "stdint.h"
#include "stdlib.h"
#include "driver.h"
#include "dl-dwd.h"
#include "obs.h"
#include "guide.h"
#include "mns.h"
#include "senchuang.h"
#include "systick.h"
#include "chassis_bus.h"
#include "kinco.h"
#include "bms.h"
#include "kinco.h"


#define DBG_SECTION_NAME  "dwd"
#define DBG_LEVEL         DBG_INFO
#include <rtdbg.h>

McWalkInit_t McWalkInit;
McWalkProcess_t McWalkProcess;
McWalkQueryProcess_t McWalkQueryProcess;
//McWalkParse_t McWalkParse;

static struct chassis_bus_parse DRBusParse = {0};

static int DrRecvProcess(void *parser, void *msg, int sz)
{
    rt_can_msg_t can_msg = (rt_can_msg_t)msg;
    int16_t mgs = 0;
	ag_bms_parse(can_msg);	//奥冠电池解析
	bms_parse(can_msg);		//维旭电池解析
	motor_msg_parse(can_msg);		//步科电机解析
    switch(can_msg->id)
    {
    case 3:
        mgs = MnsParseCanHs(can_msg->data, S.Branch, &S.FMgsOnline);
        if(mgs != S.FMgsOffset){
            S.FMgsOffset = mgs;
            //LogDebugCan("[%d]mpf:%d  %d", Timer1ms, mgs, S.FMgsOffset);
        }
        break;
    case 4:
        mgs = MnsParseCanHs(can_msg->data, S.Branch, &S.BMgsOnline);
        if(mgs != S.BMgsOffset){
            S.BMgsOffset = mgs;
            //LogDebugCan("[%d]mpb:%d  %d", Timer1ms, mgs, S.BMgsOffset);
        }
        break;
    case 5:
        mgs = MnsParseCanHs(can_msg->data, S.Branch, &S.LMgsOnline);
        if(mgs != S.LMgsOffset){
            S.LMgsOffset = mgs;
            //LogDebugCan("[%d]mpl:%d  %d", Timer1ms,  mgs, S.LMgsOffset);
        }
        break;
    case 6:
        mgs = MnsParseCanHs(can_msg->data, S.Branch, &S.RMgsOnline);
        if(mgs != S.RMgsOffset){
            S.RMgsOffset = mgs;
            //LogDebugCan("[%d]mpr:%d %d", Timer1ms, mgs, S.RMgsOffset);
        }
        break;
    case 0x0671:
    case 0x0672:
	case 0x0676:
    case 0x0677:
    case 0x0700:
    case 0x067a:
    case 0x067b:
        McWalkParseSenChuang(can_msg->id, can_msg->data);
    default:
        return true;
    }
    return true;
}
rt_inline bool drManualFB(bool positive) {
    Set.FWlkRpm = positive?(Set.RpmManual):-Set.RpmManual;
    return true;
}
rt_inline bool drManualLR(bool positive) {
	 Set.FWlkRpm = positive?(-Set.RpmManual):Set.RpmManual;
    return true;
}

/* ��ȡ��С��ƫ���� */
rt_inline int16_t getMinOffset(void){
    int16_t ret = S.FMgsOffset;
    if(S.BMgsOffset < ret){
        ret = S.BMgsOffset;
    }
    if(S.LMgsOffset < ret){
        ret = S.LMgsOffset;
    }
    if(S.RMgsOffset < ret){
        ret = S.RMgsOffset;
    }
    return ret;
}
rt_inline int16_t getMaxOffset(void){
    int16_t ret = S.FMgsOffset;
    if(S.BMgsOffset > ret){
        ret = S.BMgsOffset;
    }
    if(S.LMgsOffset > ret){
        ret = S.LMgsOffset;
    }
    if(S.RMgsOffset > ret){
        ret = S.RMgsOffset;
    }
    return ret;
}
rt_inline bool drRotate(bool positive, bool stop) {
    static volatile uint32_t stopWait = 0;
    int16_t offset = 0;
    int16_t rpm = Set.RpmRotate;
    if(stop){
        rpm = 500;
    }
    Set.FAngle = Cfg.RoteAngle;
    Set.BAngle = Cfg.RoteAngle;
    if(FAngleReach && BAngleReach){
        /* ���ת��ʮ�ּ�����ǰ��ŵ�Ϊ��־����ֹͣ */
        if(stop && AGV_ON_CROSS && Set.RotateCnt <= 0){
            offset = positive?getMinOffset():getMaxOffset();
            LOG_I("Rote:offset=%d", offset);
					rpm = positive?offset*2:-offset*2;
            //LogLocalPrintf("Rote:Stop X%d-%d = %d\r\n", S.FMgsOffset, S.FMgsOffset, rpm);
        }
        Set.FWlkRpm = positive?(-rpm):rpm;
        Set.BWlkRpm = positive?rpm:(-rpm);
        if(Cfg.RoteAngle < 0){
            Set.FWlkRpm = -Set.FWlkRpm;
            Set.BWlkRpm = -Set.BWlkRpm;
        }
        if(abs(rpm) < 80){
            //LogLocalPrintf("Rote:Stop @X = %d\r\n", S.FMgsOffset, S.FMgsOffset, rpm);
            Set.FWlkRpm = 0;
            Set.BWlkRpm = 0;
            if(rt_tick_timeout(stopWait))
            {
                S.StopAtCross = true;
                return true;
            }else{
                return false;
            }
        }
    }else{
        Set.FWlkRpm = 0;
        Set.BWlkRpm = 0;
    }
	stopWait = rt_tick_timeout_reset(RT_TICK_PER_SECOND);
    return false;
}

typedef struct {
    int16_t Offset[256];
    uint8_t Idx;
    int32_t SumOffset;
} Pid_Info_t;
Pid_Info_t pidInfoFA, PidInfoBA;

/* ǰ�󵼺����� */
rt_inline bool drNav(bool positive, bool stop) {
	static volatile uint32_t stopWait = 0;
	static  uint32_t delay_tick = 0,delay_flag = 0;
    if(stop)
	{
        if(S.yOffset < 50 && S.yOffset > -50)
		{
            Set.FWlkRpm = 0;
            if(rt_tick_timeout(stopWait))
            {
            	if (S.Point == S.TgtStation){
        			S.Station = S.TgtStation;
        			S.TgtStation = NULL_POINT;
					delay_flag = 0;
					delay_tick = 0;
        			DRSetAction(ACT_STOP);
        			LOG_I("ACT_STOP");
            	}
            	S.DirPauseStatus = DIR_STOP_STATUS;
    			//LogLocalPrintf("nav: @Point\r\n");
                return true;
            }else{
                return false;
            }
        }
		else
		{
            Set.FWlkRpm = -(S.yOffset)/6;	//之前没改，导致车子过冲，前进后退
        	//LogLocalPrintf("nav: Y rpm=%d\r\n", Set.FWlkRpm);
        	stopWait = rt_tick_timeout_reset(200);
        }
    }
	else
	{
        if(S.TgtStation - S.Point == 1 ||
        		S.Point - S.TgtStation == 1 ||
				(S.TgtStationXvalue - S.xValue == 1 && S.TgtStationYvalue == S.yValue) ||
				(S.xValue - S.TgtStationXvalue == 1 && S.TgtStationYvalue == S.yValue))
		{
			if((Set.FWlkRpm == 0) && (S.estop_flag==0))
			{
				delay_flag = 1;
			}
			if(Set.FWlkRpm)
			{
				S.estop_flag=0;
				S.reset_flag=0;
			}
			if(delay_flag)
			{
				Set.FWlkRpm = positive?(3000):-3000;
				delay_tick++;			
				if(delay_tick > S.near_time_out)
				{
					delay_flag = 0;
					delay_tick = 0;
				}			
			}
			else
			{
				Set.FWlkRpm = positive?(Set.RpmNear):-Set.RpmNear;
			}     	
        }else if(S.TgtStationYvalue != S.yValue){
        	Set.FWlkRpm = positive?(Set.RpmNear):-Set.RpmNear;
        }else{
//        	if(S.cValue == S.TgtCvalue){
        		Set.FWlkRpm = positive?(Set.DRRpmNav):-Set.DRRpmNav;
//        	}
        }
    }
    return false;
}
/* ����Ư�� */
rt_inline bool drDrift(bool positive, bool stop) {
    static  uint32_t delay_tick = 0,delay_flag = 0;
	static volatile uint32_t stopWait = 0;
    if(stop){
        if(S.xOffset < 50 && S.xOffset > -50){
            Set.FWlkRpm = 0;
            if(rt_tick_timeout(stopWait))
            {
            	if (S.Point == S.TgtStation){
        			S.Station = S.TgtStation;
        			S.TgtStation = NULL_POINT;
					delay_flag = 0;
					delay_tick = 0;
        			DRSetAction(ACT_STOP);
        			LOG_I("ACT_STOP");
            	}
            	S.DirPauseStatus = DIR_STOP_STATUS;
    			//LogLocalPrintf("nav: @Point\r\n");
                return true;
            }else{
                return false;
            }
        }else{
            Set.FWlkRpm = -(S.xOffset)/6;
        	//LogLocalPrintf("nav: X rpm=%d\r\n", Set.FWlkRpm);
        	stopWait = rt_tick_timeout_reset(200);
        }
    }else{
        if(S.TgtStation - S.Point == 1 || S.Point - S.TgtStation == 1 || S.TgtStationYvalue - S.yValue == 1 || S.yValue - S.TgtStationYvalue == 1)
		{
			if((Set.FWlkRpm == 0) && (S.estop_flag==0))
			{
				delay_flag = 1;
			}
			if(Set.FWlkRpm)
			{
				S.estop_flag=0;
				S.reset_flag=0;
			}
			if(delay_flag)
			{
				Set.FWlkRpm = positive?(-3000):3000;
				delay_tick++;			
				if(delay_tick > S.near_time_out)
				{
					delay_flag = 0;
					delay_tick = 0;
				}			
			}
			else
			{
				Set.FWlkRpm = positive?(-Set.RpmNear):Set.RpmNear;
			} 	
        }else{
//        	if(S.cValue != S.TgtCvalue){
        		Set.FWlkRpm = positive?(-Set.DRRpmNav):Set.DRRpmNav;
//        	}
        }
    }
    return false;
}
uint8_t drDirFbNav(void){
	if (S.Action == ACT_FORWARD){
		DRSetAction(ACT_FORWARD_STOP_CROSS);
	}
	if (S.Action == ACT_BACKWARD){
		DRSetAction(ACT_BACKWARD_STOP_CROSS);
	}
	if (S.DirPauseStatus == DIR_STOP_STATUS){
		return 1;
	}
	return 0;
}
uint8_t drDirLrNav(void){
	if (S.Action == ACT_LEFT){
		DRSetAction(ACT_LEFT_STOP_CROSS);
	}
	if (S.Action == ACT_RIGHT){
		DRSetAction(ACT_RIGHT_STOP_CROSS);
	}
	if (S.DirPauseStatus == DIR_STOP_STATUS){
		return 1;
	}
	return 0;
}
rt_inline bool drStop(void) {
    //LogLocalPrintf("drStop\n");
    Set.FWlkRpm = 0;
    return true;
}
char cross[4] = {'O', '-', '|', '+'};
rt_inline void checkCross(void) {
    static uint8_t preCross = CROSS_OFF;
    if(S.FMgsOnline && S.BMgsOnline && S.LMgsOnline && S.RMgsOnline){
        S.CrossType = CROSS_XY;
    }else if(S.FMgsOnline && S.BMgsOnline){
        S.StopAtCross = false;
        S.CrossType = CROSS_FB;
    }else if(S.RMgsOnline && S.LMgsOnline){
        S.StopAtCross = false;
        S.CrossType = CROSS_LR;
    }else{
        S.StopAtCross = false;
        S.CrossType = CROSS_OFF;
    }
    if(S.CrossType != preCross){
        preCross = S.CrossType;
        //LogLocalPrintf("CROSS: %c\r\n", cross[S.CrossType]);
    }
}

static void DRProcess(void *param)	//驱动器进程
{
	static	uint8_t bms_cnt;
	struct rt_can_msg msg;	
    DRBusParse.parse = DrRecvProcess;
    chassis_bus_attach_parse(&DRBusParse);
    while(S.DRStatus == DR_STATUS_INIT)
    {
        //LogLocalPrintf("DR_STATUS_INIT\r\n");
        Set.FWlkRpm = 0;
        Set.BWlkRpm = 0;
        if(McWalkInit())
        {
            S.DRStatus = DR_STATUS_RUN;
        }
        rt_thread_mdelay(100);
    }
    while(1)
    {
        rt_thread_mdelay(8);
        checkCross();
        //LogLocalPrintf("DR_Process:%x:%d,%d\n", Set.DRAction, Set.FAngle, Set.BAngle);
        S.DRAction = Set.DRAction;
        switch(Set.DRAction){
        case ACT_FORWARD:
            drNav(true, false);
            break;
        case ACT_FORWARD_STOP_CROSS:
            drNav(true, true);
            break;
        case ACT_FORWARD_LEFT:
            if(AGV_ON_LR || S.StopAtCross){
                drDrift(true, false);
            }else{
                drNav(true, true);
            }
            break;
        case ACT_FORWARD_RIGHT:
            if(AGV_ON_LR || S.StopAtCross){
                drDrift(false, false);
            }else{
                drNav(true, true);
            }
            break;
        case ACT_BACKWARD:
            drNav(false, false);
            break;
        case ACT_BACKWARD_STOP_CROSS:
            drNav(false, true);
            break;
        case ACT_BACKWARD_LEFT:
            if(AGV_ON_LR || S.StopAtCross){
                drDrift(true, false);
            }else{
                drNav(false, true);
            }
            break;
        case ACT_BACKWARD_RIGHT:
            if(AGV_ON_LR || S.StopAtCross){
                drDrift(false, false);
            }else{
                drNav(false, true);
            }
            break;
        case ACT_LEFT:
            drDrift(true, false);
            break;
        case ACT_LEFT_STOP_CROSS:
            drDrift(true, true);
            break;
        case ACT_LEFT_FORWARD:
            if(AGV_ON_FB || S.StopAtCross){
                drNav(true, false);
            }else{
                drDrift(true, true);
            }
            break;
        case ACT_LEFT_BACKWARD:
            if(AGV_ON_FB || S.StopAtCross){
                drNav(false, false);
            }else{
                drDrift(true, true);
            }
            break;
        case ACT_RIGHT:
            drDrift(false, false);
            break;
        case ACT_RIGHT_STOP_CROSS:	//有用
            drDrift(false, true);
            break;
        case ACT_RIGHT_FORWARD:
            if(AGV_ON_FB || S.StopAtCross){
                drNav(true, false);
            }else{
                drDrift(false, true);
            }
            break;
        case ACT_RIGHT_BACKWARD:
            if(AGV_ON_FB || S.StopAtCross){
                drNav(false, false);
            }else{
                drDrift(false, true);
            }
            break;
        case ACT_MANUAL_FORWARD:
            drManualFB(true);
            break;
        case ACT_MANUAL_BACKWARD:
            drManualFB(false);
            break;
        case ACT_MANUAL_DRIFT_LEFT:
            drManualLR(true);
            break;
        case ACT_MANUAL_DRIFT_RIGHT:
            drManualLR(false);
            break;
        case ACT_ROTATE_LEFT:
            drRotate(true, true);
            break;
        case ACT_MANUAL_ROTATE_LEFT:
            drRotate(true, false);
            break;
        case ACT_ROTATE_RIGHT:
            drRotate(false, true);
            break;
        case ACT_MANUAL_ROTATE_RIGHT:
            drRotate(false, false);
            break;
        default:
            drStop();
        }
		
//        McWalkProcess();	
		//步科电机
		if(motor_init_ok()==0)
		{
			set_motor_rpm(Set.FWlkRpm);	//设置电机转速
			msg = send_motor_rpm();
			chassis_bus_write(&msg, sizeof(msg));		
		}
		bms_cnt++;
		if(bms_cnt>150)	//增加奥冠的bms协议发送，每1200ms获取一次容量
		{			
			bms_cnt = 0;
			msg = require_ag_bms_msg();
			chassis_bus_write(&msg, sizeof(msg));
		}

    }
}

static int DRInit(void) {
    LOG_I("DRInit");
    Set.DRAction = ACT_STOP;
    S.DRStatus = DR_STATUS_INIT;
    switch(Cfg.WlkMcType){
    case CFG_WlkMcType_SenChuang:
        McWalkInit = McWalkInitSenChuang;
        McWalkProcess = McWalkProcessSenChuang;
        //McWalkQueryProcess = McWalkProcessSenChuang;
        break;
    default:
        McWalkInit = McWalkInitSenChuang;
        McWalkProcess = McWalkProcessSenChuang;
        LOG_E("NotSupport WlkMcType %d", Cfg.WlkMcType);
    }

    // thread
    rt_thread_t tid;
    tid = rt_thread_create("dr",
            DRProcess, RT_NULL,
                              2048,
                              9, 20);

    if (tid != RT_NULL)
    {
        rt_thread_startup(tid);
    }
    return 0;
}
INIT_APP_EXPORT(DRInit);