You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
185 lines
5.8 KiB
185 lines
5.8 KiB
11 months ago
|
/*
|
||
|
|
* msp_DC_CAN.c
|
||
|
|
*
|
||
|
|
* Created on: 2024年6月21日
|
||
|
|
* Author: SQ-X
|
||
|
|
*/
|
||
|
|
#include <msp_DC_CAN_SWING_Controller.h>
|
||
|
|
#include "bsp_FDCAN.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
static DHCANopenData0x2E4 dHCANopenData0x2E4;
|
||
|
|
|
||
|
|
DH_CAN_Remote* DH_CAN;
|
||
|
|
|
||
|
|
int32_t * Send_Left_Compensation;
|
||
|
|
int32_t * Send_Right_Compensation;
|
||
|
|
int32_t * Send_BackwardDistance;
|
||
|
|
int32_t * Send_LaneChangeDistance;
|
||
|
|
int32_t * Send_Swing_Speed;
|
||
|
|
int32_t * Send_Swing_Angle;
|
||
|
|
int32_t * Send_Move_Speed;
|
||
|
|
int32_t * Send_Robot_Angle;
|
||
|
|
|
||
|
|
int32_t* SetSpeed[10];
|
||
|
|
|
||
|
|
int32_t* Robot_Move_Speed;
|
||
|
|
FDCANHandler *DH_CAN_Remote_Controller_Handler;
|
||
|
|
HardWareController *hardController;
|
||
|
|
DispacherController *dispacherController;
|
||
|
|
|
||
|
|
void DH_CAN_Remote_Controller_intialize(FDCANHandler *Handler)
|
||
|
|
{
|
||
|
|
|
||
|
|
DH_CAN_Remote_Controller_Handler = Handler;
|
||
|
|
DH_CAN_Remote_Controller_Handler->CAN_Decode = DecodeCAN;
|
||
|
|
|
||
|
|
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
||
|
|
"DH_CAN_Remote_Controller_1E4", 0,DH_CAN_Remote_Controller_1E4);
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
||
|
|
"DH_CAN_Remote_Controller_2E4", 0,DH_CAN_Remote_Controller_2E4);
|
||
|
|
//周期性运行
|
||
|
|
dispacherController = Handler->dispacherController;
|
||
|
|
dispacherController->Add_Dispatcher_List(dispacherController,
|
||
|
|
SendRobtDataToController_0x364);
|
||
|
|
dispacherController->Add_Dispatcher_List(dispacherController,
|
||
|
|
SendRobtDataToController_0x264);
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void DecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
|
||
|
|
{
|
||
|
|
//buffer[0] is CAN ID;
|
||
|
|
|
||
|
|
int DH_40022_Flag = (int) canID;
|
||
|
|
switch (DH_40022_Flag)
|
||
|
|
{
|
||
|
|
case 0x701: //心跳700 + NodeID1
|
||
|
|
//DC_Decoding_Func();
|
||
|
|
break;
|
||
|
|
case 0x702: //心跳700 + NodeID2
|
||
|
|
//DC_Decoding_Func();
|
||
|
|
break;
|
||
|
|
case 0x1E4: //1E4 = 484
|
||
|
|
{
|
||
|
|
DH_CAN->Front_Back_Move_Value = CAN_Buf[0];
|
||
|
|
DH_CAN->Left_Right_Turn_Value = CAN_Buf[1];
|
||
|
|
DH_CAN->Swing_Speed_Value = CAN_Buf[2];
|
||
|
|
DH_CAN->Swing_Angle_Value = CAN_Buf[3];
|
||
|
|
DH_CAN->Left_Compensation_Value = CAN_Buf[4];
|
||
|
|
DH_CAN->Right_Compensation_Value = CAN_Buf[5];
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
||
|
|
"DH_CAN_Remote_Controller_1E4", 1);
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case 0x2E4: //2E4
|
||
|
|
|
||
|
|
{
|
||
|
|
memcpy(&dHCANopenData0x2E4,buffer,8);
|
||
|
|
DH_CAN->Emergency_Stop = dHCANopenData0x2E4.EmergencyStop;
|
||
|
|
DH_CAN->Move_Forward = dHCANopenData0x2E4.Forwards;
|
||
|
|
DH_CAN->Move_Backward = dHCANopenData0x2E4.BackWards;
|
||
|
|
DH_CAN->Move_Turn_Left = dHCANopenData0x2E4.TurnLeft;
|
||
|
|
DH_CAN->Move_Turn_Right = dHCANopenData0x2E4.TurnRight;
|
||
|
|
|
||
|
|
*Robot_Move_Speed = GetSpeed(dHCANopenData0x2E4);
|
||
|
|
|
||
|
|
DH_CAN->Mode_Selection = GetMode(dHCANopenData0x2E4);
|
||
|
|
|
||
|
|
DH_CAN->Work_Start = dHCANopenData0x2E4.TaskStart;
|
||
|
|
|
||
|
|
DH_CAN->Increase = dHCANopenData0x2E4.Incresss;
|
||
|
|
DH_CAN->Decrease = dHCANopenData0x2E4.Decresss;
|
||
|
|
|
||
|
|
DH_CAN->Lane_Change = dHCANopenData0x2E4.LaneChnge;//换道
|
||
|
|
DH_CAN->UpWards = dHCANopenData0x2E4.UpWards;
|
||
|
|
DH_CAN->DownWards = dHCANopenData0x2E4.DownWards;
|
||
|
|
|
||
|
|
DH_CAN->Swing_Left = dHCANopenData0x2E4.SwingLeft;
|
||
|
|
DH_CAN->Swing_Right = dHCANopenData0x2E4.SwingRight;
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
||
|
|
"DH_CAN_Remote_Controller_2E4", 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
uint8_t data_to_Send[8] =
|
||
|
|
{ 1, 2, 3, 4, 5, 6, 7, 8 };
|
||
|
|
|
||
|
|
uint16_t data_temp;
|
||
|
|
void SendRobtDataToController_0x364() //Send 换道距离, 0x364
|
||
|
|
{
|
||
|
|
|
||
|
|
memset(data_to_Send, 0, sizeof(data_to_Send));
|
||
|
|
|
||
|
|
data_temp = (uint16_t)(*Send_Left_Compensation);
|
||
|
|
memcpy(&data_to_Send[0], &data_temp, 2); //左补偿
|
||
|
|
data_temp = (uint16_t) (*Send_Right_Compensation);
|
||
|
|
memcpy(&data_to_Send[2], &data_temp, 2); //右补偿
|
||
|
|
data_temp = (uint16_t) (*Send_BackwardDistance);
|
||
|
|
memcpy(&data_to_Send[4], &data_temp, 2); //后退距离
|
||
|
|
data_temp = (uint16_t) (*Send_LaneChangeDistance);
|
||
|
|
memcpy(&data_to_Send[6], &data_temp, 2); //换道距离
|
||
|
|
|
||
|
|
DH_CAN_Remote_Controller_Handler->CAN_Send(DH_CAN_Remote_Controller_Handler,
|
||
|
|
0X364, 8, data_to_Send);
|
||
|
|
//DH_CAN_Remote_Controller_Handler->CAN_Send(DH_CAN_Remote_Controller_Handler,10,8,data);
|
||
|
|
}
|
||
|
|
void SendRobtDataToController_0x264()
|
||
|
|
{
|
||
|
|
memset(data_to_Send, 0, sizeof(data_to_Send));
|
||
|
|
//data_temp=(uint16_t)GV.DH_CAN.Send_Angle;
|
||
|
|
data_temp = (uint16_t) (*Send_Swing_Speed);
|
||
|
|
memcpy(&data_to_Send[0], &data_temp, 2); //摆速
|
||
|
|
data_temp = (uint16_t) (*Send_Swing_Angle);
|
||
|
|
memcpy(&data_to_Send[2], &data_temp, 2); //摆角
|
||
|
|
data_temp = (uint16_t) (*Send_Move_Speed);
|
||
|
|
memcpy(&data_to_Send[4], &data_temp, 2); //速度
|
||
|
|
data_temp = (uint16_t) (*Send_Robot_Angle);//需要知道是不是这个东西
|
||
|
|
memcpy(&data_to_Send[6], &data_temp, 2); //角度
|
||
|
|
|
||
|
|
//DH_40022_FDCANfunction(0X264, 8,data);
|
||
|
|
DH_CAN_Remote_Controller_Handler->CAN_Send(DH_CAN_Remote_Controller_Handler,
|
||
|
|
0X264, 8, data_to_Send);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
int32_t GetSpeed(DHCANopenData0x2E4 dHCANopenData0x2E4)
|
||
|
|
{
|
||
|
|
if(dHCANopenData0x2E4.Speed1==1) {return *SetSpeed[0];}
|
||
|
|
if(dHCANopenData0x2E4.Speed2==1) {return *SetSpeed[1];}
|
||
|
|
if(dHCANopenData0x2E4.Speed3==1) {return *SetSpeed[2];}
|
||
|
|
if(dHCANopenData0x2E4.Speed4==1) {return *SetSpeed[3];}
|
||
|
|
if(dHCANopenData0x2E4.Speed5==1) {return *SetSpeed[4];}
|
||
|
|
if(dHCANopenData0x2E4.Speed6==1) {return *SetSpeed[5];}
|
||
|
|
if(dHCANopenData0x2E4.Speed7==1) {return *SetSpeed[6];}
|
||
|
|
if(dHCANopenData0x2E4.Speed8==1) {return *SetSpeed[7];}
|
||
|
|
if(dHCANopenData0x2E4.Speed9==1) {return *SetSpeed[8];}
|
||
|
|
if(dHCANopenData0x2E4.Speed10==1) {return *SetSpeed[9];}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
int32_t GetMode(DHCANopenData0x2E4 dHCANopenData0x2E4)
|
||
|
|
{
|
||
|
|
|
||
|
|
if(dHCANopenData0x2E4.Mode0==1) {return 0;}
|
||
|
|
if(dHCANopenData0x2E4.Mode1==1) {return 1;}
|
||
|
|
if(dHCANopenData0x2E4.Mode2==1) {return 2;}
|
||
|
|
if(dHCANopenData0x2E4.Mode3==1) {return 3;}
|
||
|
|
if(dHCANopenData0x2E4.Mode4==1) {return 4;}
|
||
|
|
if(dHCANopenData0x2E4.Mode5==1) {return 5;}
|
||
|
|
if(dHCANopenData0x2E4.Mode6==1) {return 6;}
|
||
|
|
if(dHCANopenData0x2E4.Mode7==1) {return 7;}
|
||
|
|
if(dHCANopenData0x2E4.Mode8==1) {return 8;}
|
||
|
|
if(dHCANopenData0x2E4.Mode9==1) {return 9;}
|
||
|
|
if(dHCANopenData0x2E4.Mode10==1) {return 10;}
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|