#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 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= (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= (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= (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(((0SBUS_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 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= (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= (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= (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(((0SBUS_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; } }