rgv-kinco/applications/driver.c

/*
 * driver.c
 *
 *  Created on: 2019��8��12��
 *      Author: Eric
 */
#include "env.h"
#include "driver.h"
#include "guide.h"
#include "obs.h"
#include "systick.h"

#define DBG_SECTION_NAME  "driver"
#define DBG_LEVEL         DBG_INFO
#include <rtdbg.h>
rt_inline uint8_t _GetRotateAct(uint8_t act);
rt_inline void _SetDRSpeed(uint8_t act);
rt_inline uint8_t _calcDirection(uint8_t act);


void DRSetAction(uint8_t act) {
    uint8_t DRAct;
    S.Action = act;
//    if(Set.RotateAct <= 0){
//        Set.RotateAct = ACT_NULL;
//    }
    //LogDebugGuide("DR_SetAction %d", act);
    if(Set.RotateAct == ACT_NULL || ActIsManual(act)){
        DRAct = act;
//				return;
    }else{
        DRAct = _GetRotateAct(act);
    }
    S.Direction = _calcDirection(DRAct);
    if(S.NavStatus == NAV_STATUS_STATION_BRANCH || S.NavStatus == NAV_STATUS_STATION_IN){
        S.Direction = DIRECTION_NULL;
    }
    {
        {
			/* bug106 */
			if(S.ObsStatus != OBS_STATUS_NEAR)
            S.ObsStatus = ObsGetStatus();
            switch(S.ObsStatus){
            case OBS_STATUS_FAR:
                GDLowSpeed();
                break;
            case OBS_STATUS_NEAR:
				/* 为唐山修改 */
            	if(S.Point == (Set.ChargeStation - 1) || (S.Point == Set.ChargeStation + 1) || (S.Point == Set.ChargeStation)){
            		return;
            	}
				
                GDPause();
                DRAct = ACT_STOP;
				
                break;
            default:
            	 break;
//                GDResume();
            }
        }
    }
    if(Set.DRAction != DRAct){
        _SetDRSpeed(DRAct);
        //LogDebugGuide("DR_SetAction: act:0x%x 0x%x-> 0x%x rpm:%d, %d", act, Set.DRAction, DRAct, Set.RpmNav, Set.RpmDft);
        Set.DRAction = DRAct;
    }
}
rt_inline uint8_t _StopAtCross(uint8_t act) {
//    LogNoRepeatVar
//    ;
    uint8_t pre = act >> 4;
    uint8_t next = act & 0x0F;
    if(pre == ACT_NULL){
        pre = next;
    }
    switch(pre){
    case ACT_LEFT:
//        DebugDriverNoRepeat("StopAC_Pre ACT_LEFT Speed %d", S.Speed)
//        ;
        return ACT_LEFT_STOP_CROSS;
    case ACT_RIGHT:
//        DebugDriverNoRepeat("StopAC_Pre ACT_RIGHT Speed %d", S.Speed)
//        ;
        return ACT_RIGHT_STOP_CROSS;
    case ACT_FORWARD:
//        DebugDriverNoRepeat("StopAC_Pre ACT_FORWARD Speed %d", S.Speed)
//        ;
        return ACT_FORWARD_STOP_CROSS;
    case ACT_BACKWARD:
//        DebugDriverNoRepeat("StopAC_Pre ACT_BACKWARD Speed %d ", S.Speed)
//        ;
        return ACT_BACKWARD_STOP_CROSS;
    }
//    DebugDriverNoRepeat("StopAC_Pre-other-Act other %x", act);
    return act;
}
rt_inline void changeRotate2NextAct(uint8_t next) {
    switch(Set.RotateAct){
    case ACT_ROTATE_LEFT:
        switch(next){
        case ACT_FORWARD:
            Set.RotateAct = ACT_RIGHT;
            return;
        case ACT_RIGHT:
            Set.RotateAct = ACT_BACKWARD;
            return;
        case ACT_BACKWARD:
            Set.RotateAct = ACT_LEFT;
            return;
        case ACT_LEFT:
            Set.RotateAct = ACT_FORWARD;
            return;
        }
    case ACT_ROTATE_RIGHT:
        switch(next){
        case ACT_FORWARD:
            //LogLocalPrintf("Rote:R F->R\r\n");
            Set.RotateAct = ACT_RIGHT;
            return;
        case ACT_RIGHT:
            //LogLocalPrintf("Rote:R R->B\r\n");
            Set.RotateAct = ACT_FORWARD;
            return;
        case ACT_BACKWARD:
            //LogLocalPrintf("Rote:R B->L\r\n");
            Set.RotateAct = ACT_RIGHT;
            return;
        case ACT_LEFT:
            //LogLocalPrintf("Rote:R L->B\r\n");
            Set.RotateAct = ACT_BACKWARD;
            return;
        }
        return;
    case ACT_ROUND_LEFT:
    case ACT_ROUND_RIGHT:
        switch(next){
        case ACT_FORWARD:
            Set.RotateAct = ACT_BACKWARD;
            return;
        case ACT_RIGHT:
            Set.RotateAct = ACT_LEFT;
            return;
        case ACT_BACKWARD:
            Set.RotateAct = ACT_FORWARD;
            return;
        case ACT_LEFT:
            Set.RotateAct = ACT_RIGHT;
            return;
        }
        return;
    }
}
rt_inline uint8_t _GetRotateAct(uint8_t act) {
   // LogNoRepeatVar
    //;
    static volatile uint32_t roteTime = 0;
    int8_t next = act & 0x0F;
    static volatile uint8_t preCross = CROSS_OFF, Rotating = False;
    switch(S.CrossType){
    case CROSS_XY:
        roteTime = rt_tick_timeout_reset(60000);
        preCross = CROSS_XY;
        if(Set.RotateCnt > 0){
            if(S.StopAtCross){
                //DebugDriverNoRepeat("Rote:X CNT>0", Set.RotateCnt);
                Rotating = True;
                return Set.RotateAct;
            }else{
                //LogDebugDriver("Rote:X CNT > 0 Stop");
                return _StopAtCross(act);
            }
        }else{
            if(S.StopAtCross){
                Rotating = False;
                changeRotate2NextAct(next);
                //LogDebugDriver("Rote:X-Next %x->%x", next, Set.RotateAct);
                return Set.RotateAct;
            }else{
								//LogDebugDriver("Rote: X %x", Set.RotateAct);
                return Set.RotateAct;
            }
        }
    case CROSS_LR:
    case CROSS_FB:
        roteTime = rt_tick_timeout_reset(60000);
        preCross = S.CrossType;
        if(Rotating){
            //DebugDriverNoRepeat("Rote:S CRX=%d, CNT=%d, RACT=%x", S.CrossType, Set.RotateCnt, Set.RotateAct);
            return Set.RotateAct;
        }
        if(Set.RotateCnt > 0){
            //DebugDriverNoRepeat("Rote:S-CNT>0");
            return _StopAtCross(act);
        }else{
            changeRotate2NextAct(next);
            //DebugDriverNoRepeat("Rote:S-Next%x", Set.RotateAct);
            return Set.RotateAct;
        }
    default:
        if(rt_tick_timeout(roteTime))
        {
            S.Status = STATUS_ERROR_ROTE_OFFLINE;
            return ACT_STOP;
        }
        if(preCross != CROSS_OFF){
            if(Set.RotateCnt > 0){
                Set.RotateCnt--;
            }
            //DebugDriverNoRepeat("Rote:O-CNT-%d", Set.RotateCnt);
            preCross = CROSS_OFF;
        }
        if(Rotating == True){
            //DebugDriverNoRepeat("Rote:O-R");
            return Set.RotateAct;
        }else{
            //DebugDriverNoRepeat("Rote:O-R-N");
            //Set.RotateAct = ACT_NULL;
            return act;
        }
    }
}
rt_inline void _SetDRSpeed(uint8_t act) {
    //LogNoRepeatVar
    //;
    if(Set.RotateAct != ACT_NULL){
        Set.DRRpmDft = Set.RpmNear;
        Set.DRRpmNav = Set.RpmNear;
        //DebugDriverNoRepeat("DRSpeed RotateAct speed", act)
        return;
    }
    if(Set.SpeedAct == ACT_SPEED_LOW){
        Set.DRRpmDft = Set.RpmNear;
        Set.DRRpmNav = Set.RpmNear;
        //DebugDriverNoRepeat("DRSpeed ACT_SPEED_LOW %x", act)
        return;
    }
    switch(act){
    case ACT_FORWARD_LEFT:
    case ACT_FORWARD_RIGHT:
    case ACT_BACKWARD_LEFT:
    case ACT_BACKWARD_RIGHT:
        //DebugDriverNoRepeat("DRSpeed FB-LR %x", act)
        //;
        Set.DRRpmNav = Set.RpmNear;
        Set.DRRpmDft = Set.RpmDft;
        break;
    case ACT_LEFT_FORWARD:
    case ACT_LEFT_BACKWARD:
    case ACT_RIGHT_FORWARD:
    case ACT_RIGHT_BACKWARD:
        //DebugDriverNoRepeat("DRSpeed LR-FB", act)
        //;
        Set.DRRpmNav = Set.RpmNav;
        Set.DRRpmDft = Set.RpmNear;
        break;
    default:
        if(S.Status == STATUS_RUN_OBS_LOW_SPEED){
            Set.DRRpmNav = Set.RpmNear;
            Set.DRRpmDft = Set.RpmNear;
        }else{
            Set.DRRpmNav = Set.RpmNav;
            Set.DRRpmDft = Set.RpmDft;
        }
    }
}
rt_inline uint8_t _calcDirection(uint8_t act) {
//    LogNoRepeatVar
//    ;
    uint8_t pre = act >> 4, next = act & 0x0F;
    if(act == ACT_ROTATE_LEFT){
        return DIRECTION_ROTATE;
    }
    if(act == ACT_ROTATE_RIGHT){
        return DIRECTION_ROTATE;
    }
    if(pre == ACT_NULL){
        switch(next){
        case ACT_LEFT:
//            DebugGuideNoRepeat("calcDirection: no pre next %d DIRECTION_LEFT", next)
//            ;
            return DIRECTION_LEFT;
        case ACT_RIGHT:
//            DebugGuideNoRepeat("calcDirection: no pre next %d DIRECTION_RIGHT", next)
//            ;
            return DIRECTION_RIGHT;
        case ACT_FORWARD:
//            DebugGuideNoRepeat("calcDirection: no pre next %d DIRECTION_FORWARD", next)
//            ;
            return DIRECTION_FORWARD;
        case ACT_BACKWARD:
//            DebugGuideNoRepeat("calcDirection: no pre next %d DIRECTION_BACKWARD", next)
//            ;
            return DIRECTION_BACKWARD;
        }
    }
    switch(S.CrossType){
    case CROSS_LR:
        if((pre == ACT_LEFT) || (next == ACT_LEFT)){
            //DebugGuideNoRepeat("calcDirection: CROSS_LR pre %d next %d DIRECTION_LEFT", pre, next);
            return DIRECTION_LEFT;
        }
        if((pre == ACT_RIGHT) || (next == ACT_RIGHT)){
            //DebugGuideNoRepeat("calcDirection: CROSS_LR pre %d next %d DIRECTION_RIGHT", pre, next);
            return DIRECTION_RIGHT;
        }
//        DebugGuideNoRepeat("calcDirection: CROSS_LR other act %x", act)
//        ;
        break;
    case CROSS_FB:
        if((pre == ACT_FORWARD) || (next == ACT_FORWARD)){
            //DebugGuideNoRepeat("calcDirection: CROSS_LR pre %d next %d DIRECTION_FORWARD", pre, next);
            return DIRECTION_FORWARD;
        }
        if((pre == ACT_BACKWARD) || (next == ACT_BACKWARD)){
            //DebugGuideNoRepeat("calcDirection: CROSS_LR pre %d next %d DIRECTION_BACKWARD", pre, next);
            return DIRECTION_BACKWARD;
        }
//        DebugGuideNoRepeat("calcDirection: CROSS_FB other act %x", act)
//        ;
        break;
    case CROSS_XY:
        switch(next){
        case ACT_LEFT:
//            DebugGuideNoRepeat("calcDirection: CROSS_XY next %d DIRECTION_LEFT", next)
//            ;
            return DIRECTION_LEFT;
        case ACT_RIGHT:
//            DebugGuideNoRepeat("calcDirection: CROSS_XY next %d DIRECTION_RIGHT", next)
//            ;
            return DIRECTION_RIGHT;
        case ACT_FORWARD:
//            DebugGuideNoRepeat("calcDirection: CROSS_XY next %d DIRECTION_FORWARD", next)
//            ;
            return DIRECTION_FORWARD;
        case ACT_BACKWARD:
//            DebugGuideNoRepeat("calcDirection: CROSS_XY next %d DIRECTION_BACKWARD", next)
//            ;
            return DIRECTION_BACKWARD;
        }
//        DebugGuideNoRepeat("calcDirection: CROSS_XY other act %x", act)
//        ;
        break;
    }
    return DIRECTION_NULL;
}