![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Delta Battery, CAN_bus, DIO P2P, ROS
Dependencies: mbed mbed-rtos ros_lib_kinetic
main.cpp
- Committer:
- WeberYang
- Date:
- 2019-08-22
- Revision:
- 12:ed085acbbde5
- Parent:
- 11:6d5307ceb569
File content as of revision 12:ed085acbbde5:
/* 0412 combine the sevro motor encoder to publish */ #include "mbed.h" #include "CAN.h" #include "cmsis_os.h" #include <ros.h> #include <ros/time.h> #include <std_msgs/Int16.h> #include <std_msgs/String.h> #include <std_msgs/Float32.h> #include <sensor_msgs/BatteryState.h> #include <sensor_msgs/LaserEcho.h> #include <geometry_msgs/Twist.h> //set buffer larger than 50byte #include <math.h> #include <stdio.h> #include <string> #include <cstdlib> #define CAN_DATA 0x470 #define CAN_STATUS 0x471 #define PC_BAUDRATE 115200 //Serial pc(USBTX,USBRX); Timer t; Serial RS232(PA_9, PA_10); DigitalOut Receiver(D7); //RS485_E DigitalOut CAN_T(D14); DigitalOut CAN_R(D15); DigitalOut DO_0(PC_3); DigitalOut DO_1(PC_2); //DigitalOut DO_2(PF_1); //DigitalOut DO_3(PF_0); DigitalOut DO_4(PC_15); DigitalOut DO_5(PC_14); DigitalOut DO_6(PC_13); DigitalOut DO_7(PB_7); DigitalOut DO_8(PA_14); DigitalOut DO_9(PA_13); //DigitalOut DO_10(PF_7); //DigitalOut DO_11(PF_6); DigitalOut DO_12(PC_12); DigitalOut DO_13(PC_10); DigitalOut DO_14(PD_2); DigitalOut DO_15(PC_11); DigitalOut led1(LED1); //DigitalIn DI_0(PF_5); DigitalIn DI_1(PC_4); DigitalIn DI_2(PB_13); DigitalIn DI_3(PB_14); DigitalIn DI_4(PB_15); DigitalIn DI_5(PB_1); DigitalIn DI_6(PB_2); DigitalIn DI_7(PB_11); DigitalIn DI_8(PB_12); DigitalIn DI_9(PA_11); DigitalIn DI_10(PA_12); //DigitalIn DI_11(PD_8); DigitalIn DI_12(PC_5); DigitalIn DI_13(PC_6); DigitalIn DI_14(PC_8); DigitalIn DI_15(PC_9); //const unsigned int RX_ID = 0x792; int id_count[15]; long Radial_range[15]; long Radial_speed[15]; long Radial_Acc[15]; long Angle[15]; long Power[15]; long tmp_dis; long dis[7]; long ldata; float fdata1,fdata2; float fRange[15],fAngle[15]; //=========Battery varable================ uint32_t SOC; uint32_t Tempert; uint32_t RackVoltage = 0; uint32_t Current = 0; uint32_t MaxCellV = 0; uint32_t MinCellV = 0; uint32_t seq; //======================================== void CAN_thread(const void *args); void DIO_thread(const void *args); //CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name //CANMsg rxMsg; CANMessage rxMsg; osThreadDef(CAN_thread, osPriorityNormal, DEFAULT_STACK_SIZE); osThreadDef(DIO_thread, osPriorityNormal, DEFAULT_STACK_SIZE); //====================================================================== ros::NodeHandle nh; std_msgs::Int16 str_msg1; std_msgs::Int16 str_msg2; std_msgs::Int16 str_msg3; std_msgs::Int16 DI_data; std_msgs::Int16 ACT_state; std_msgs::Int16 Error_state; std_msgs::Int16 DO; sensor_msgs::BatteryState BTState; ros::Publisher sonar1("sonar1", &str_msg1); ros::Publisher sonar2("sonar2", &str_msg2); ros::Publisher sonar3("sonar3", &str_msg3); ros::Publisher DI_data_pub("DI_data_pub", &DI_data); ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state); ros::Publisher Error_state_pub("Error_state_pub", &Error_state); ros::Publisher BT_pub("BatteryState", &BTState); void DO_ACT(const std_msgs::Int16 &msg){ DO_0 = (msg.data & 0x001); DO_1 = (msg.data & 0x002); // DO_2 = (msg.data & 0x004); // DO_3 = (msg.data & 0x008); DO_4 = (msg.data & 0x010); DO_5 = (msg.data & 0x020); DO_6 = (msg.data & 0x040); DO_7 = (msg.data & 0x080); DO_8 = (msg.data & 0x100); DO_9 = (msg.data & 0x200); // DO_10 = (msg.data & 0x0400); DO_12 = (msg.data & 0x1000); DO_13 = (msg.data & 0x2000); DO_14 = (msg.data & 0x4000); DO_15 = (msg.data & 0x8000); led1 = !led1; osDelay(1); } ros::Subscriber<std_msgs::Int16> DO_data_sub("DO_data_sub", &DO_ACT); //====================================================================== void CAN_thread(const void *args){ while (true) { if (can1.read(rxMsg)){ seq++; SOC = rxMsg.data[0]>>1; Tempert = rxMsg.data[1]-50; RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2]; Current = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4]; MaxCellV = rxMsg.data[6]; MinCellV = rxMsg.data[7]; } else{ BTState.voltage = 0; BTState.current = 0; BTState.design_capacity = 80; BTState.percentage = 0; } BTState.header.stamp = nh.now(); BTState.header.frame_id = 0; BTState.header.seq = seq; BTState.voltage = RackVoltage*0.1; BTState.current = Current; BTState.design_capacity = 50; BTState.percentage = SOC; osDelay(10); } } void DIO_thread(const void *args){ while (true) { DI_data.data = 0xffff-0x0001-0x0800 - ((int)DI_1<<1) - (((int)DI_2)<<2) - (((int)DI_3)<<3) - (((int)DI_4)<<4); DI_data.data -= (((int)DI_5)<<5); DI_data.data -= (((int)DI_6)<<6); DI_data.data -= (((int)DI_7)<<7); DI_data.data -= (((int)DI_8)<<8); DI_data.data -= (((int)DI_9)<<9); DI_data.data -= (((int)DI_10)<<10); // DI_data.data -= (((int)DI_11)<<11); DI_data.data -= (((int)DI_12)<<12); DI_data.data -= (((int)DI_13)<<13); DI_data.data -= (((int)DI_14)<<14); DI_data.data -= (((int)DI_15)<<15); osDelay(10); } } //====================================================================== int main() { RS232.baud(PC_BAUDRATE); nh.initNode(); // nh.advertise(sonar1); // nh.advertise(sonar2); // nh.advertise(ACT_state_pub); nh.advertise(DI_data_pub); nh.advertise(BT_pub); nh.subscribe(DO_data_sub); // DI_0.mode(PullUp); DI_1.mode(PullUp); DI_2.mode(PullUp); DI_3.mode(PullUp); DI_4.mode(PullUp); DI_5.mode(PullUp); DI_6.mode(PullUp); DI_7.mode(PullUp); DI_8.mode(PullUp); DI_9.mode(PullUp); DI_10.mode(PullUp); // DI_11.mode(PullUp); DI_12.mode(PullUp); DI_13.mode(PullUp); DI_14.mode(PullUp); DI_15.mode(PullUp); wait_ms(10); osThreadCreate(osThread(CAN_thread), NULL); osThreadCreate(osThread(DIO_thread), NULL); // CANMessage rxMsg; CAN_T = 0; CAN_R = 0; wait_ms(50); // CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name wait_ms(50); can1.frequency(500000); wait_ms(50); while(1){ DI_data_pub.publish( &DI_data); // sonar1.publish( &str_msg1); // sonar2.publish( &str_msg2); BT_pub.publish( &BTState ); nh.spinOnce(); osDelay(50); } }