YouRenV1.0YZ/Drivers/bsp/Motion_model.c

#include "Motion_model.h"
 int16_t Speed_left,Speed_right,Speed_left_flag,Speed_right_flag=0;
uint16_t Max_Speed = 500;
uint8_t Stop_Flag,Drive_Flag=0;
    _Motion_Data Motion_Data={0};                //  左右驱动器数据


 int16_t JDZ_4_20(int16_t In_Put,int32_t AIMAX,int32_t AIMIN,int32_t SDMAX,int32_t SDMIN  )
 {
     uint64_t TEP,TED,OUT=0;
     TED=(In_Put-AIMIN)*(SDMAX-SDMIN) ;
     TEP=TED/(AIMAX-AIMIN);
     OUT= TEP+(SDMIN);
     return OUT;
 }

/*
*********************************************************************************************************
*
*	模块名称 : Motion_model模块
*   匹配左右

*********************************************************************************************************
*/

 
 
 
void Sbus_Motion_Re(void)
{
    
    memset(&Motion_Data,0,sizeof(Motion_Data));
    int16_t RockX=0, RockY=0;                    //  X Y  变化量
    uint8_t RockYStopFlag=0, RockXStopFlag=0;    //  X Y  停止标志位
    int32_t speed=0;
    

//    
    RockY = SBUS_CH.CH2;  // Y 轴
    RockX = SBUS_CH.CH1;  // X 轴

//        if(((SBUS_ROCK_LEFTDEAD <= RockX)&&(RockX <= SBUS_ROCK_RIGHTDEAD)) || (RockX == 0)){
//        //摇杆X轴中位死区
//        RockXStopFlag=1;
//    }
//    if(((SBUS_ROCK_DOWNDEAD <= RockY)&&(RockY <= SBUS_ROCK_UPDEAD)) || (RockY == 0)){
//        //摇杆Y轴中位死区
//        RockYStopFlag=1;
//    }
//    if(RockYStopFlag && RockXStopFlag){//停止
//        Motion_Data.SetSpeed[Motion_Left] = 0;
//        Motion_Data.SetSpeed[Motion_Right] = 0;
//        Motion_Data.MotionFlag[Motion_Left] = Con_Mot_Stop;
//        Motion_Data.MotionFlag[Motion_Right] =Con_Mot_Stop;
//    }else
//    {
//        Motion_Data.MotionFlag[Motion_Left] = Con_Mot_Speed;
//        Motion_Data.MotionFlag[Motion_Right] =Con_Mot_Speed;
//        if(!RockYStopFlag && RockXStopFlag){   //X轴无动作
//            if(RockY > SBUS_ROCK_UPDEAD){      // 前进
//                if(RockY >= SBUS_ROCK_Y_MAX)   // 越界
//                {
//                    Motion_Data.SetSpeed[Motion_Left] = Max_Speed;
//                    Motion_Data.SetSpeed[Motion_Right] =(~Max_Speed)+1;  //Max_Speed;//
//                }
//                else //界内
//                {    //计算匹配两侧速度
//                    Motion_Data.SetSpeed[Motion_Left] = (Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD));
//                    Motion_Data.SetSpeed[Motion_Right] = (~(Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)))+1;//(Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD));
//                } 
//            }else if((0<RockY)&&(RockY < SBUS_ROCK_DOWNDEAD)){//后退
//                if(RockY <= SBUS_ROCK_Y_MIN){
//                    Motion_Data.SetSpeed[Motion_Left] = (~Max_Speed)+1;
//                    Motion_Data.SetSpeed[Motion_Right] = Max_Speed;//(~Max_Speed)+1;//
//                }
//                else{
//                    Motion_Data.SetSpeed[Motion_Left] = (~(Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)))+1;
//                    Motion_Data.SetSpeed[Motion_Right] = (Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN));//(~(Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)))+1;//
//                }
//            }
//            else{}
//            
//        }else if(RockYStopFlag&&!RockXStopFlag){//Y轴无动作
//            if((0<RockX)&&(RockX < SBUS_ROCK_LEFTDEAD)){//原地左转
//                if(RockX <= SBUS_ROCK_X_MIN){
//                    Motion_Data.SetSpeed[Motion_Left] = (~(Max_Speed/2))+1;
//                    Motion_Data.SetSpeed[Motion_Right] =(~(Max_Speed/2))+1;// (Max_Speed/2);//
//                }
//                else{
//                    Motion_Data.SetSpeed[Motion_Left] = (~(Max_Speed/2*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)))+1;
//                    Motion_Data.SetSpeed[Motion_Right] = (~(Max_Speed/2*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)))+1;//(Max_Speed/2*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN));//
//                }
//            }
//            else if(RockX > SBUS_ROCK_RIGHTDEAD){//原地右转
//                if(RockX >= SBUS_ROCK_X_MAX){
//                    Motion_Data.SetSpeed[Motion_Left] = (Max_Speed/2);
//                    Motion_Data.SetSpeed[Motion_Right] = (Max_Speed/2);//(~(Max_Speed/2))+1;//
//                }
//                else{
//                    Motion_Data.SetSpeed[Motion_Left] = (Max_Speed/2*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD));
//                    Motion_Data.SetSpeed[Motion_Right]=(Max_Speed/2*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD));//(~(Max_Speed/2*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)))+1; //
//                }
//            }
//            else{}
//        }else{//摇杆XY轴都有动作
//            if((SBUS_ROCK_UPDEAD<RockY) && ((0<RockX)&&(RockX<SBUS_ROCK_LEFTDEAD))){//左前方 2
//                if((RockY-SBUS_ROCK_UPDEAD) >= (SBUS_ROCK_LEFTDEAD-RockX)){//左前偏上
//                    speed = Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD);//计算中心速度
//                    Motion_Data.SetSpeed[Motion_Left] = (speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN));
//                    Motion_Data.SetSpeed[Motion_Right] =(~(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4))+1;//(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4); //
//                }else{//左前偏下   
//                    speed = Max_Speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN);
//                    Motion_Data.SetSpeed[Motion_Left] = (~(speed/2-speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/2))+1;
//                    Motion_Data.SetSpeed[Motion_Right] =(~(speed/2+speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/4))+1;//(speed/2+speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/4); //
//                }
//            }
//            else if((SBUS_ROCK_UPDEAD<RockY) && (SBUS_ROCK_RIGHTDEAD<RockX)){//右前方 1
//                if((RockY-SBUS_ROCK_UPDEAD) >= (RockX-SBUS_ROCK_RIGHTDEAD)){//右前偏上
//                    speed = Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD);
//                    Motion_Data.SetSpeed[Motion_Left] = (speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)/4);
//                    Motion_Data.SetSpeed[Motion_Right] =(~(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)))+1;//(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)); //
//                } 
//                else{//右前偏下   
//                    speed = Max_Speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD);
//                    Motion_Data.SetSpeed[Motion_Left] = (speed/2+speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/4);
//                    Motion_Data.SetSpeed[Motion_Right] =(speed/2-speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/2);//(~(speed/2-speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/2))+1; //
//                }
//            }
//            else if(((0<RockY)&&(RockY<SBUS_ROCK_DOWNDEAD)) && ((0<RockX)&&(RockX<SBUS_ROCK_LEFTDEAD))){//左后方 3
//                if((SBUS_ROCK_DOWNDEAD-RockY) >= (SBUS_ROCK_LEFTDEAD-RockX)){//左后偏下  
//                    speed = Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN);
//                    Motion_Data.SetSpeed[Motion_Left] = (~(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)))+1;
//                    Motion_Data.SetSpeed[Motion_Right]= (speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4);//(~(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4))+1;//
//                }
//                else{//左后偏上
//                    speed = Max_Speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN);
//                    Motion_Data.SetSpeed[Motion_Left] = (speed/2-speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/2);
//                    Motion_Data.SetSpeed[Motion_Right] =(speed/2+speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/4);//(~(speed/2+speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/4))+1; //
//                }
//            }
//            else if(((0<RockY)&&(RockY<SBUS_ROCK_DOWNDEAD)) && (RockX>SBUS_ROCK_RIGHTDEAD)){//右后方 4
//                if((SBUS_ROCK_DOWNDEAD-RockY) >= (RockX-SBUS_ROCK_RIGHTDEAD)){//右后偏下
//                    speed = Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN);
//                    Motion_Data.SetSpeed[Motion_Left] = (~(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)/4))+1;
//                    Motion_Data.SetSpeed[Motion_Right]= (speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD));//(~(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)))+1;//
//                }
//                else{//右后偏上
//                    speed = Max_Speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD);
//                    Motion_Data.SetSpeed[Motion_Left] = (~(speed/2+speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/4))+1;
//                    Motion_Data.SetSpeed[Motion_Right] =(~(speed/2-speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/2))+1;//(speed/2-speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/2); //
//                }
//            }
//        }
//        //可整体限制
//        Motion_Data.SetSpeed[Motion_Left]=Motion_Data.SetSpeed[Motion_Left]*1.0;
//        Motion_Data.SetSpeed[Motion_Right]=Motion_Data.SetSpeed[Motion_Right]*1.0; 
//    }

    if(((SBUS_ROCK_LEFTDEAD <= RockX)&&(RockX <= SBUS_ROCK_RIGHTDEAD)) || (RockX == 0)){
        //摇杆X轴中位死区
        RockXStopFlag=1;
    }
    if(((SBUS_ROCK_DOWNDEAD <= RockY)&&(RockY <= SBUS_ROCK_UPDEAD)) || (RockY == 0)){
        //摇杆Y轴中位死区
        RockYStopFlag=1;
    }
    if(RockYStopFlag && RockXStopFlag){//停止
        Motion_Data.SetSpeed[Motion_Left] = 0;
        Motion_Data.SetSpeed[Motion_Right] = 0;
        Motion_Data.MotionFlag[Motion_Left] = Con_Mot_Stop;
        Motion_Data.MotionFlag[Motion_Right] =Con_Mot_Stop;
    }else
    {
        Motion_Data.MotionFlag[Motion_Left] = Con_Mot_Speed;
        Motion_Data.MotionFlag[Motion_Right] =Con_Mot_Speed;
        if(!RockYStopFlag && RockXStopFlag){   //X轴无动作
            if(RockY > SBUS_ROCK_UPDEAD){      // 前进
                if(RockY >= SBUS_ROCK_Y_MAX)   // 越界
                {
                    Motion_Data.SetSpeed[Motion_Left] = Max_Speed;
                    Motion_Data.SetSpeed[Motion_Right] =(~Max_Speed)+1;  //Max_Speed;//
                }
                else //界内
                {    //计算匹配两侧速度
                    Motion_Data.SetSpeed[Motion_Left] = (Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD));
                    Motion_Data.SetSpeed[Motion_Right] = (~(Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)))+1;//(Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD));
                } 
            }else if((0<RockY)&&(RockY < SBUS_ROCK_DOWNDEAD)){//后退
                if(RockY <= SBUS_ROCK_Y_MIN){
                    Motion_Data.SetSpeed[Motion_Left] = (~Max_Speed)+1;
                    Motion_Data.SetSpeed[Motion_Right] = Max_Speed;//(~Max_Speed)+1;//
                }
                else{
                    Motion_Data.SetSpeed[Motion_Left] = (~(Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)))+1;
                    Motion_Data.SetSpeed[Motion_Right] = (Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN));//(~(Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)))+1;//
                }
            }
            else{}
            
        }else if(RockYStopFlag&&!RockXStopFlag){//Y轴无动作
            if((0<RockX)&&(RockX < SBUS_ROCK_LEFTDEAD)){//原地左转
                if(RockX <= SBUS_ROCK_X_MIN){
                    Motion_Data.SetSpeed[Motion_Left] = (~(Max_Speed/2))+1;
                    Motion_Data.SetSpeed[Motion_Right] =(~(Max_Speed/2))+1;// (Max_Speed/2);//
                }
                else{
                    Motion_Data.SetSpeed[Motion_Left] = (~(Max_Speed/2*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)))+1;
                    Motion_Data.SetSpeed[Motion_Right] = (~(Max_Speed/2*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)))+1;//(Max_Speed/2*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN));//
                }
            }
            else if(RockX > SBUS_ROCK_RIGHTDEAD){//原地右转
                if(RockX >= SBUS_ROCK_X_MAX){
                    Motion_Data.SetSpeed[Motion_Left] = (Max_Speed/2);
                    Motion_Data.SetSpeed[Motion_Right] = (Max_Speed/2);//(~(Max_Speed/2))+1;//
                }
                else{
                    Motion_Data.SetSpeed[Motion_Left] = (Max_Speed/2*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD));
                    Motion_Data.SetSpeed[Motion_Right]=(Max_Speed/2*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD));//(~(Max_Speed/2*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)))+1; //
                }
            }
            else{}
        }else{//摇杆XY轴都有动作
            if((SBUS_ROCK_UPDEAD<RockY) && ((0<RockX)&&(RockX<SBUS_ROCK_LEFTDEAD))){//左前方 2
                if((RockY-SBUS_ROCK_UPDEAD) >= (SBUS_ROCK_LEFTDEAD-RockX)){//左前偏上
                    speed = Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD);//计算中心速度
                    Motion_Data.SetSpeed[Motion_Left] = (speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4);
                    Motion_Data.SetSpeed[Motion_Right] =(~(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)))+1;//(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4); //
                }else{//左前偏下   
                    speed = Max_Speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN);
                    Motion_Data.SetSpeed[Motion_Left] = (speed/2+speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/4);
                    Motion_Data.SetSpeed[Motion_Right] =(speed/2-speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/2);//(speed/2+speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/4); //
                }
            }
            else if((SBUS_ROCK_UPDEAD<RockY) && (SBUS_ROCK_RIGHTDEAD<RockX)){//右前方 1
                if((RockY-SBUS_ROCK_UPDEAD) >= (RockX-SBUS_ROCK_RIGHTDEAD)){//右前偏上
                    speed = Max_Speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD);
                    Motion_Data.SetSpeed[Motion_Left] = (speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD));
                    Motion_Data.SetSpeed[Motion_Right] =(~(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)/4))+1;//(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)); //
                } 
                else{//右前偏下   
                    speed = Max_Speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD);
                    Motion_Data.SetSpeed[Motion_Left] = (~(speed/2-speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/2))+1;
                    Motion_Data.SetSpeed[Motion_Right] =(~(speed/2+speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/4))+1;//(~(speed/2-speed*(RockY-SBUS_ROCK_UPDEAD)/(SBUS_ROCK_Y_MAX-SBUS_ROCK_UPDEAD)/2))+1; //
                }
            }
            else if(((0<RockY)&&(RockY<SBUS_ROCK_DOWNDEAD)) && ((0<RockX)&&(RockX<SBUS_ROCK_LEFTDEAD))){//左后方 3
                if((SBUS_ROCK_DOWNDEAD-RockY) >= (SBUS_ROCK_LEFTDEAD-RockX)){//左后偏下  
                    speed = Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN);
                    Motion_Data.SetSpeed[Motion_Left] = (~(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4))+1;
                    Motion_Data.SetSpeed[Motion_Right]= (speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN));//(~(speed-speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN)/4))+1;//

                }
                else{//左后偏上
                    speed = Max_Speed*(SBUS_ROCK_LEFTDEAD-RockX)/(SBUS_ROCK_LEFTDEAD-SBUS_ROCK_X_MIN);
                    Motion_Data.SetSpeed[Motion_Left] = (~(speed/2+speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/4))+1;
                    Motion_Data.SetSpeed[Motion_Right] =(~(speed/2-speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/2))+1;//(~(speed/2+speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/4))+1; //

                }
            }
            else if(((0<RockY)&&(RockY<SBUS_ROCK_DOWNDEAD)) && (RockX>SBUS_ROCK_RIGHTDEAD)){//右后方 4
                if((SBUS_ROCK_DOWNDEAD-RockY) >= (RockX-SBUS_ROCK_RIGHTDEAD)){//右后偏下
                    speed = Max_Speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN);
                    Motion_Data.SetSpeed[Motion_Left] = (~(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)))+1;
                    Motion_Data.SetSpeed[Motion_Right]= (speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)/4);//(~(speed-speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD)))+1;//
   
                }
                else{//右后偏上
                    speed = Max_Speed*(RockX-SBUS_ROCK_RIGHTDEAD)/(SBUS_ROCK_X_MAX-SBUS_ROCK_RIGHTDEAD);
                    Motion_Data.SetSpeed[Motion_Left] = (speed/2-speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/2);
                    Motion_Data.SetSpeed[Motion_Right] =(speed/2+speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/4);//(speed/2-speed*(SBUS_ROCK_DOWNDEAD-RockY)/(SBUS_ROCK_DOWNDEAD-SBUS_ROCK_Y_MIN)/2); //
                }
            }
        }
        //可整体限制
        Motion_Data.SetSpeed[Motion_Left]=Motion_Data.SetSpeed[Motion_Left]*1.0;
        Motion_Data.SetSpeed[Motion_Right]=Motion_Data.SetSpeed[Motion_Right]*1.0; 
    }



    Speed_left = Motion_Data.SetSpeed[Motion_Left];
    Speed_right = Motion_Data.SetSpeed[Motion_Right];
    Speed_left_flag = Motion_Data.MotionFlag[Motion_Left];
    Speed_right_flag = Motion_Data.MotionFlag[Motion_Right];

}

/*****************************************运动发送*********************************************/
void Can_Drive_Send(int16_t L_Flag,int16_t L_Speed,int16_t R_Flag,int16_t R_Speed)
{
    uint8_t Can_Walk_Left[8]={0x2B,0x00,0x20,0x00,0x10,0x00,0x00,0x00};
    uint8_t Can_Walk_Right[8]={0x2B,0x00,0x20,0x00,0x10,0x00,0x00,0x00};
    //can_send_msg(CAN_HandleTypeDef *hcan,uint8_t IDE,uint32_t id, uint8_t *msg, uint8_t len )
    
    if((L_Flag==Con_Mot_Stop)||(Speed_right_flag==Con_Mot_Stop))
    {
        switch (Stop_Flag)
        {
            case 0:
                Can_Walk_Left[0]=0x2F;
                Can_Walk_Right[0]=0x2F;
                Can_Walk_Left[1]=0x00;
                Can_Walk_Right[1]=0x00;
                Can_Walk_Left[4]=Con_Mot_Stop; 
                Can_Walk_Right[4]=Con_Mot_Stop;
                can_send_msg(&hcan2,0,Left_Driver, Can_Walk_Left,8 );
                can_send_msg(&hcan2,0,Right_Driver, Can_Walk_Right,8 );
                Stop_Flag=1;
            break;
            case 1:
                Can_Walk_Left[0]=0x2B;
                Can_Walk_Right[0]=0x2B;
                Can_Walk_Left[1]=0x01;
                Can_Walk_Right[1]=0x01;
                Can_Walk_Left[4]=0x00;
                Can_Walk_Right[4]=0x00;
                Can_Walk_Left[5]=0x00;
                Can_Walk_Right[5]=0x00;
                can_send_msg(&hcan2,0,Left_Driver, Can_Walk_Left,8 );
                can_send_msg(&hcan2,0,Right_Driver, Can_Walk_Right,8 );
                Stop_Flag=0;
            break;
        }
    }else
    {
        switch (Drive_Flag)
        {
            case 0:
                Can_Walk_Left[0]=0x2F;
                Can_Walk_Right[0]=0x2F;
                Can_Walk_Left[1]=0x00;
                Can_Walk_Right[1]=0x00;
                Can_Walk_Left[4]=Speed_left_flag;
                Can_Walk_Right[4]=Speed_right_flag;
                can_send_msg(&hcan2,0,Left_Driver, Can_Walk_Left,8 );
                can_send_msg(&hcan2,0,Right_Driver, Can_Walk_Right,8 );
                Drive_Flag=1;
            break;
            case 1:
                Can_Walk_Left[0]=0x2B;
                Can_Walk_Right[0]=0x2B;
                Can_Walk_Left[1]=0x01;
                Can_Walk_Right[1]=0x01; 
                Can_Walk_Left[4]=L_Speed & 0xFF;
                Can_Walk_Right[4]=R_Speed & 0xFF;
                Can_Walk_Left[5]=(L_Speed>>8) & 0xFF;
                Can_Walk_Right[5]=(R_Speed>>8) & 0xFF;
                can_send_msg(&hcan2,0,Left_Driver, Can_Walk_Left,8 );
                can_send_msg(&hcan2,0,Right_Driver, Can_Walk_Right,8 );
                Drive_Flag=0;
            break;
        }
    }
    
}

void Can_Drive_Stop(void)
{
    uint8_t Can_Walk_Left[8]={0x2B,0x00,0x20,0x00,0x10,0x00,0x00,0x00};
    uint8_t Can_Walk_Right[8]={0x2B,0x00,0x20,0x00,0x10,0x00,0x00,0x00};   
    
    switch (Stop_Flag)
    {
        case 0:
        Can_Walk_Left[0]=0x2F;
        Can_Walk_Right[0]=0x2F;
        Can_Walk_Left[1]=0x00;
        Can_Walk_Right[1]=0x00;
        Can_Walk_Left[4]=0x10;
        Can_Walk_Right[4]=0x10;
        can_send_msg(&hcan2,0,Left_Driver, Can_Walk_Left,8 );
        can_send_msg(&hcan2,0,Right_Driver, Can_Walk_Right,8 );
        Stop_Flag=1;
        break;
        case 1:
        Can_Walk_Left[0]=0x2B;
        Can_Walk_Right[0]=0x2B;
        Can_Walk_Left[1]=0x01;
        Can_Walk_Right[1]=0x01;
        Can_Walk_Left[4]=0x00;
        Can_Walk_Right[4]=0x00;
        Can_Walk_Left[5]=0x00;
        Can_Walk_Right[5]=0x00;
       can_send_msg(&hcan2,0,Left_Driver, Can_Walk_Left,8 );
        can_send_msg(&hcan2,0,Right_Driver, Can_Walk_Right,8 );
        Stop_Flag=0;
        break;
    }
}


void KY_CAN_Send(int16_t L_Flag,int16_t L_Speed,int16_t R_Flag,int16_t R_Speed)
{
    uint8_t Can_Walk_Left[8]={0x23,0x00,0x20,0x01,0x00,0x00,0x00,0x00};//左电机 01 速度报文
    uint8_t Can_Walk_Right[8]={0x23,0x00,0x20,0x02,0x00,0x00,0x00,0x00};//右电机 02 速度报文
    
    uint8_t Can_DisEnable[8]={0x2C,0x0C,0x20,0x00,0x00,0x00,0x00,0x00};//失能
    uint8_t Can_Enable[8]={0x2C,0x0D,0x20,0x00,0x00,0x00,0x00,0x00};//使能
    
    if((L_Flag==Con_Mot_Stop)||(R_Flag==Con_Mot_Stop))
    {
        switch (Stop_Flag)
        {
            case 0:
                can_send_msg(&hcan2,0,KY_Driver, Can_DisEnable,8 );
                Stop_Flag=1;
            break;
            case 1:
                Can_Walk_Left[4]=0x00;
                Can_Walk_Right[4]=0x00;
                Can_Walk_Left[5]=0x00;
                Can_Walk_Right[5]=0x00;
                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Left,8 );
                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Right,8 );
                Stop_Flag=0;
            break;
//            case 2:
//                Can_Walk_Left[4]=0x00;
//                Can_Walk_Right[4]=0x00;
//                Can_Walk_Left[5]=0x00;
//                Can_Walk_Right[5]=0x00;
//                //can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Left,8 );
//                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Right,8 );
//                Stop_Flag=0;
//            break;
            
        }
    }else
    {
        switch (Drive_Flag)
        {
            case 0:
                can_send_msg(&hcan2,0,KY_Driver, Can_Enable,8 );
                Drive_Flag=1;
            break;
            case 1:
                Can_Walk_Left[4]=L_Speed & 0xFF;
                Can_Walk_Right[4]=R_Speed & 0xFF;
                Can_Walk_Left[5]=(L_Speed>>8) & 0xFF;
                Can_Walk_Right[5]=(R_Speed>>8) & 0xFF;
                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Left,8 );
                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Right,8 );
                Drive_Flag=0;
            break;
//            case 2:
//                Can_Walk_Left[4]=L_Speed & 0xFF;
//                Can_Walk_Right[4]=R_Speed & 0xFF;
//                Can_Walk_Left[5]=(L_Speed>>8) & 0xFF;
//                Can_Walk_Right[5]=(R_Speed>>8) & 0xFF;
//               // can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Left,8 );
//                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Right,8 );
//                Drive_Flag=0;
//            break;
        }
    }
    
    
}

void KY_Can_Stop(void)
{
    uint8_t Can_Walk_Left[8]={0x23,0x00,0x20,0x01,0x00,0x00,0x00,0x00};//左电机 01 速度报文
    uint8_t Can_Walk_Right[8]={0x23,0x00,0x20,0x02,0x00,0x00,0x00,0x00};//右电机 02 速度报文
    
    uint8_t Can_DisEnable[8]={0x2C,0x0C,0x20,0x00,0x00,0x00,0x00,0x00};//失能
    
        switch (Stop_Flag)
        {
            case 0:
                can_send_msg(&hcan2,0,KY_Driver, Can_DisEnable,8 );
                Stop_Flag=1;
            break;
            case 1:
                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Left,8 );
                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Right,8 );
                Stop_Flag=0;
            break;
//            case 2:
//               // can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Left,8 );
//                can_send_msg(&hcan2,0,KY_Driver, Can_Walk_Right,8 );
//                Stop_Flag=0;
//            break;
        }
}