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.
280 lines
6.6 KiB
280 lines
6.6 KiB
2 weeks ago
|
///*
|
||
|
|
// * fsm.c
|
||
|
|
// *
|
||
|
|
// * Created on: Oct 18, 2024
|
||
|
|
// * Author: akeguo
|
||
|
|
// */
|
||
|
|
//
|
||
|
|
//#include <bsp_tempature.h>
|
||
|
|
//#include <logic.h>
|
||
|
|
//#include <math.h>
|
||
|
|
//#include "msp_DAM0404D.h"
|
||
|
|
//#include "BHBF_ROBOT.h"
|
||
|
|
//#include "BSP/bsp_include.h"
|
||
|
|
//#include "msp_DH_Roughening.h"
|
||
|
|
//#include "MSP/msp_PID.h"
|
||
|
|
//#include "MSP/msp_MK32_1.h"
|
||
|
|
//#include "BHBF_ROBOT.h"
|
||
|
|
//
|
||
|
|
//// 需要一个485接口，并且能够实现接收和发送；非周期性发送
|
||
|
|
//// CAN 通讯，然后将数据发送出去
|
||
|
|
//
|
||
|
|
//char job_start_flag = 0;
|
||
|
|
//char job_stop_flag = 0;
|
||
|
|
//char arm_position_reached_flag = 0;
|
||
|
|
//char arm_end_reached_flag = 0;
|
||
|
|
//
|
||
|
|
//
|
||
|
|
//
|
||
|
|
//uint8_t arm_go_to_position[8] =
|
||
|
|
//{ 0X01, 0X01, 0X01, 0X01, 0X01, 0X01, 0X01, 0X01 };
|
||
|
|
//uint8_t arm_spray_gun_on[8] =
|
||
|
|
//{ 0X02, 0X02, 0X01, 0X01, 0X01, 0X01, 0X01, 0X01 };
|
||
|
|
//uint8_t arm_spray_gun_off[8] =
|
||
|
|
//{ 0X03, 0X03, 0X01, 0X01, 0X01, 0X01, 0X01, 0X01 };
|
||
|
|
//uint8_t arm_get_position[8] =
|
||
|
|
//{ 0X04, 0X04, 0X01, 0X01, 0X01, 0X01, 0X01, 0X01 };
|
||
|
|
//
|
||
|
|
//uint8_t arm_get_reached_end[8] =
|
||
|
|
//{ 0X05, 0X05, 0X01, 0X01, 0X01, 0X01, 0X01, 0X01 };
|
||
|
|
//
|
||
|
|
//void klf_DecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length);
|
||
|
|
//void decode_485(uint8_t *buffer, uint16_t length);
|
||
|
|
//
|
||
|
|
//struct UARTHandler *klf_handler;
|
||
|
|
//DispacherController *klf_dispacherController;
|
||
|
|
//
|
||
|
|
//FDCANHandler *klf_can_controller;
|
||
|
|
//DispacherController *klf_can_DispacherController;
|
||
|
|
//
|
||
|
|
//void fke_lai_fen_modbus_intialize(struct UARTHandler *Handler)
|
||
|
|
//{
|
||
|
|
//
|
||
|
|
// klf_handler = Handler;
|
||
|
|
// klf_handler->UART_Decode = decode_485;
|
||
|
|
// klf_handler->Wait_time = 6; //等待10ms 最低不要低于4；
|
||
|
|
// klf_dispacherController = Handler->dispacherController;
|
||
|
|
// klf_dispacherController->Dispacher_Enable = 0;
|
||
|
|
// klf_dispacherController->DispacherCallTime = 2;
|
||
|
|
//
|
||
|
|
// HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
||
|
|
// "KeLaiFen485", 0, KeLaiFen485);
|
||
|
|
// //uartHandler->Insert_HardWare_Entry_UART
|
||
|
|
// LOG("steering_engine_intialize");
|
||
|
|
//}
|
||
|
|
//
|
||
|
|
//uint8_t test_Order[8] =
|
||
|
|
//{ 0X01, 0X03, 0X07, 0XD0, 0X00, 0X02, 0XC4, 0X86 };
|
||
|
|
//void klf_test()
|
||
|
|
//{
|
||
|
|
//
|
||
|
|
// memcpy(klf_handler->Tx_Buf, test_Order, 8);
|
||
|
|
// klf_handler->TxCount = 8;
|
||
|
|
// //立刻发送
|
||
|
|
// klf_handler->UART_Tx(klf_handler);
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
//
|
||
|
|
//void klf_send(char *buf, char length)
|
||
|
|
//{
|
||
|
|
//
|
||
|
|
// memcpy(klf_handler->Tx_Buf, buf, length);
|
||
|
|
// klf_handler->TxCount = length;
|
||
|
|
// //立刻发送
|
||
|
|
// klf_handler->UART_Tx(klf_handler);
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
//
|
||
|
|
//void decode_485(uint8_t *buffer, uint16_t length)
|
||
|
|
//{
|
||
|
|
// klf_test();
|
||
|
|
//
|
||
|
|
// //klf_handler->AddSendList(klf_handler,test_Order,8,2000,decode_485);
|
||
|
|
// //延时立刻发送
|
||
|
|
// //klf_handler->AddSendList(klf_handler,buffer,length,2000,decode_485);
|
||
|
|
// //dLT_Log_UART_Handler->AddSendList(dLT_Log_UART_Handler,DltLogData,Size,100,NULL);
|
||
|
|
//// memcpy(klf_handler->Tx_Buf, buffer, length);
|
||
|
|
//// klf_handler->TxCount = length;
|
||
|
|
//// klf_handler->UART_Tx(klf_handler);
|
||
|
|
//
|
||
|
|
// /* CRC 校验 */
|
||
|
|
// uint16_t crc_check = ((buffer[length - 1] << 8) | buffer[length - 2]);
|
||
|
|
// /* CRC 校验正确 */
|
||
|
|
// if (crc_check == MB_CRC16(buffer, length - 2))
|
||
|
|
// {
|
||
|
|
//
|
||
|
|
// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"KeLaiFen485",1);
|
||
|
|
//
|
||
|
|
// if(1)
|
||
|
|
// {
|
||
|
|
// arm_position_reached_flag = 1;
|
||
|
|
//
|
||
|
|
//
|
||
|
|
// }else if(1)
|
||
|
|
// {
|
||
|
|
// arm_end_reached_flag = 1;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
//
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
// LOGFF(DL_ERROR,"force_sensor decoding failed");
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
//
|
||
|
|
//void klf_can_controller_intialize(FDCANHandler *Handler)
|
||
|
|
//{
|
||
|
|
// //初始化
|
||
|
|
//
|
||
|
|
// klf_can_controller = Handler;
|
||
|
|
// klf_can_controller->CAN_Decode = klf_DecodeCAN;
|
||
|
|
//
|
||
|
|
// HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
|
||
|
|
// "KeLaiFenCAN", 0, KeLaiFenCAN);
|
||
|
|
//
|
||
|
|
// klf_can_DispacherController = Handler->dispacherController;
|
||
|
|
// klf_can_DispacherController->DispacherCallTime = 2;
|
||
|
|
// klf_can_DispacherController->Dispacher_Enable = 0;
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
//
|
||
|
|
//void klf_DecodeCAN(uint32_t canID, uint8_t *buffer, uint32_t length)
|
||
|
|
//{
|
||
|
|
// if(1)
|
||
|
|
// {
|
||
|
|
// job_start_flag=1;
|
||
|
|
// }
|
||
|
|
// else if(1)
|
||
|
|
// {
|
||
|
|
// job_stop_flag=1;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
//
|
||
|
|
// HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
|
||
|
|
// "KeLaiFenCAN", 1);
|
||
|
|
//
|
||
|
|
//// memcpy(klf_can_controller->Tx_Buf, buffer, length);
|
||
|
|
//// klf_can_controller->SendLength = length;
|
||
|
|
//// klf_can_controller->SendFrameID = canID;
|
||
|
|
//// //立刻发送
|
||
|
|
//// klf_can_controller->CAN_Send_Data(klf_can_controller);
|
||
|
|
////
|
||
|
|
//// //延时发送
|
||
|
|
//// klf_can_controller->AddCANSendList(klf_can_controller, canID, length,
|
||
|
|
//// klf_can_controller->Tx_Buf, 2, klf_DecodeCAN); //
|
||
|
|
////
|
||
|
|
//// job_start_flag = 1;
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
//
|
||
|
|
//enum CLF_State
|
||
|
|
//{
|
||
|
|
// JobStart = 1,
|
||
|
|
// FirstDelay,
|
||
|
|
// SprayOn,
|
||
|
|
// SprayOff,
|
||
|
|
// ReadArmPosition,
|
||
|
|
// ReadArmReachedEnd,
|
||
|
|
// SecondDelay,
|
||
|
|
// JobEnd,
|
||
|
|
// NoMove
|
||
|
|
//};
|
||
|
|
////定义枚举类型 enum SEASONS
|
||
|
|
//enum CLF_State clf_state=NoMove; //定义了一个枚举类型变量season(类型enum SEASONS)
|
||
|
|
//
|
||
|
|
//void clf_loop()
|
||
|
|
//{
|
||
|
|
// switch (clf_state)
|
||
|
|
// {
|
||
|
|
// case JobStart:
|
||
|
|
// {
|
||
|
|
// klf_send(arm_go_to_position, sizeof(arm_go_to_position)); //机器人运动到位置
|
||
|
|
//
|
||
|
|
// //清零
|
||
|
|
// timer_handler_1.start_timer = 1;
|
||
|
|
// clf_state = FirstDelay;
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case FirstDelay:
|
||
|
|
// {
|
||
|
|
// if (CompareTimer(40, &timer_handler_1))//计时结束
|
||
|
|
// {
|
||
|
|
// clf_state = SprayOn;
|
||
|
|
// }
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case SprayOn:
|
||
|
|
// {
|
||
|
|
// klf_send(arm_spray_gun_on, sizeof(arm_spray_gun_on)); //发送启动喷枪指令
|
||
|
|
// clf_state = ReadArmPosition;
|
||
|
|
// timer_handler_1.start_timer = 1;
|
||
|
|
// timer_handler_2.start_timer = 1;
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case ReadArmPosition: //周期性获取，担心数据过快
|
||
|
|
// {
|
||
|
|
// if (CompareTimer(100, &timer_handler_2))
|
||
|
|
// {
|
||
|
|
// klf_send(arm_get_position, sizeof(arm_get_position)); //发送启动喷枪指令
|
||
|
|
// timer_handler_2.start_timer = 1;
|
||
|
|
// if (arm_position_reached_flag == 1)
|
||
|
|
// {
|
||
|
|
// clf_state = SprayOff;
|
||
|
|
// arm_position_reached_flag =0 ;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case SprayOff:
|
||
|
|
// {
|
||
|
|
// klf_send(arm_spray_gun_off, sizeof(arm_spray_gun_off)); //发送启动喷枪指令
|
||
|
|
//
|
||
|
|
// clf_state = ReadArmReachedEnd;
|
||
|
|
//
|
||
|
|
// timer_handler_2.start_timer = 1;
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case ReadArmReachedEnd:
|
||
|
|
// {
|
||
|
|
// if (CompareTimer(20, &timer_handler_2))
|
||
|
|
// {
|
||
|
|
// klf_send(arm_get_reached_end, sizeof(arm_get_reached_end));
|
||
|
|
// timer_handler_2.start_timer = 1;
|
||
|
|
// if (arm_end_reached_flag == 1)
|
||
|
|
// {
|
||
|
|
// clf_state = SecondDelay;
|
||
|
|
// timer_handler_1.start_timer = 1;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// }
|
||
|
|
// break;
|
||
|
|
// }case SecondDelay:
|
||
|
|
// {
|
||
|
|
// if (!CompareTimer(1000, &timer_handler_1))//计时中
|
||
|
|
// {
|
||
|
|
// if (job_stop_flag == 1)
|
||
|
|
// {
|
||
|
|
// clf_state = JobEnd;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// }else//计时结束
|
||
|
|
// {
|
||
|
|
// clf_state = JobStart;
|
||
|
|
// }
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
// case JobEnd:
|
||
|
|
// {
|
||
|
|
//
|
||
|
|
// break;
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
//}
|
||
|
|
//
|