Weber Yang / Mbed 2 deprecated mbed_RS485_CAN_DIO

Delta Battery, CAN_bus, DIO P2P, ROS

Dependencies:   mbed mbed-rtos ros_lib_kinetic

main.cpp@12:ed085acbbde5, 2019-08-22 (annotated)

Committer:
WeberYang
Date:
Thu Aug 22 07:44:05 2019 +0000
Revision:
12:ed085acbbde5
Parent:
11:6d5307ceb569 
Delta Battery Can_bus reader, and DIO P2P, ROS.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WeberYang 2:648583d6e41a 1 /*
WeberYang 3:51194773aa7e 2 0412 combine the sevro motor encoder to publish
WeberYang 2:648583d6e41a 3 */
hardtail 0:6e61e8ec4b42 4 #include "mbed.h"
WeberYang 6:eadfb1b45bda 5 #include "CAN.h"
WeberYang 12:ed085acbbde5 6 #include "cmsis_os.h"
WeberYang 2:648583d6e41a 7 #include <ros.h>
WeberYang 2:648583d6e41a 8 #include <ros/time.h>
WeberYang 8:4974fc24fbd7 9 #include <std_msgs/Int16.h>
WeberYang 2:648583d6e41a 10 #include <std_msgs/String.h>
WeberYang 2:648583d6e41a 11 #include <std_msgs/Float32.h>
WeberYang 2:648583d6e41a 12 #include <sensor_msgs/BatteryState.h>
WeberYang 12:ed085acbbde5 13 #include <sensor_msgs/LaserEcho.h>
WeberYang 2:648583d6e41a 14 #include <geometry_msgs/Twist.h> //set buffer larger than 50byte
WeberYang 2:648583d6e41a 15 #include <math.h>
hardtail 0:6e61e8ec4b42 16 #include <stdio.h>
WeberYang 5:52fb31c1a8c0 17 #include <string>
WeberYang 5:52fb31c1a8c0 18 #include <cstdlib>
hardtail 0:6e61e8ec4b42 19
WeberYang 6:eadfb1b45bda 20 #define CAN_DATA 0x470
WeberYang 6:eadfb1b45bda 21 #define CAN_STATUS 0x471
WeberYang 6:eadfb1b45bda 22
WeberYang 12:ed085acbbde5 23 #define PC_BAUDRATE 115200
WeberYang 12:ed085acbbde5 24
WeberYang 5:52fb31c1a8c0 25 //Serial pc(USBTX,USBRX);
WeberYang 6:eadfb1b45bda 26 Timer t;
WeberYang 6:eadfb1b45bda 27 Serial RS232(PA_9, PA_10);
WeberYang 12:ed085acbbde5 28 DigitalOut Receiver(D7); //RS485_E
WeberYang 6:eadfb1b45bda 29 DigitalOut CAN_T(D14);
WeberYang 6:eadfb1b45bda 30 DigitalOut CAN_R(D15);
WeberYang 8:4974fc24fbd7 31
WeberYang 12:ed085acbbde5 32 DigitalOut DO_0(PC_3);
WeberYang 12:ed085acbbde5 33 DigitalOut DO_1(PC_2);
WeberYang 12:ed085acbbde5 34 //DigitalOut DO_2(PF_1);
WeberYang 12:ed085acbbde5 35 //DigitalOut DO_3(PF_0);
WeberYang 12:ed085acbbde5 36 DigitalOut DO_4(PC_15);
WeberYang 12:ed085acbbde5 37 DigitalOut DO_5(PC_14);
WeberYang 12:ed085acbbde5 38 DigitalOut DO_6(PC_13);
WeberYang 12:ed085acbbde5 39 DigitalOut DO_7(PB_7);
WeberYang 12:ed085acbbde5 40 DigitalOut DO_8(PA_14);
WeberYang 12:ed085acbbde5 41 DigitalOut DO_9(PA_13);
WeberYang 12:ed085acbbde5 42 //DigitalOut DO_10(PF_7);
WeberYang 12:ed085acbbde5 43 //DigitalOut DO_11(PF_6);
WeberYang 12:ed085acbbde5 44 DigitalOut DO_12(PC_12);
WeberYang 12:ed085acbbde5 45 DigitalOut DO_13(PC_10);
WeberYang 12:ed085acbbde5 46 DigitalOut DO_14(PD_2);
WeberYang 12:ed085acbbde5 47 DigitalOut DO_15(PC_11);
WeberYang 12:ed085acbbde5 48 DigitalOut led1(LED1);
WeberYang 8:4974fc24fbd7 49
WeberYang 12:ed085acbbde5 50 //DigitalIn DI_0(PF_5);
WeberYang 12:ed085acbbde5 51 DigitalIn DI_1(PC_4);
WeberYang 12:ed085acbbde5 52 DigitalIn DI_2(PB_13);
WeberYang 12:ed085acbbde5 53 DigitalIn DI_3(PB_14);
WeberYang 12:ed085acbbde5 54 DigitalIn DI_4(PB_15);
WeberYang 12:ed085acbbde5 55 DigitalIn DI_5(PB_1);
WeberYang 12:ed085acbbde5 56 DigitalIn DI_6(PB_2);
WeberYang 12:ed085acbbde5 57 DigitalIn DI_7(PB_11);
WeberYang 12:ed085acbbde5 58 DigitalIn DI_8(PB_12);
WeberYang 12:ed085acbbde5 59 DigitalIn DI_9(PA_11);
WeberYang 12:ed085acbbde5 60 DigitalIn DI_10(PA_12);
WeberYang 12:ed085acbbde5 61 //DigitalIn DI_11(PD_8);
WeberYang 12:ed085acbbde5 62 DigitalIn DI_12(PC_5);
WeberYang 12:ed085acbbde5 63 DigitalIn DI_13(PC_6);
WeberYang 12:ed085acbbde5 64 DigitalIn DI_14(PC_8);
WeberYang 12:ed085acbbde5 65 DigitalIn DI_15(PC_9);
WeberYang 8:4974fc24fbd7 66
WeberYang 8:4974fc24fbd7 67
WeberYang 12:ed085acbbde5 68 //const unsigned int RX_ID = 0x792;
WeberYang 12:ed085acbbde5 69 int id_count[15];
WeberYang 12:ed085acbbde5 70 long Radial_range[15];
WeberYang 12:ed085acbbde5 71 long Radial_speed[15];
WeberYang 12:ed085acbbde5 72 long Radial_Acc[15];
WeberYang 12:ed085acbbde5 73 long Angle[15];
WeberYang 12:ed085acbbde5 74 long Power[15];
WeberYang 12:ed085acbbde5 75 long tmp_dis;
WeberYang 12:ed085acbbde5 76 long dis[7];
WeberYang 12:ed085acbbde5 77 long ldata;
WeberYang 12:ed085acbbde5 78 float fdata1,fdata2;
WeberYang 12:ed085acbbde5 79 float fRange[15],fAngle[15];
WeberYang 2:648583d6e41a 80
WeberYang 12:ed085acbbde5 81 //=========Battery varable================
WeberYang 12:ed085acbbde5 82 uint32_t SOC;
WeberYang 12:ed085acbbde5 83 uint32_t Tempert;
WeberYang 12:ed085acbbde5 84 uint32_t RackVoltage = 0;
WeberYang 12:ed085acbbde5 85 uint32_t Current = 0;
WeberYang 12:ed085acbbde5 86 uint32_t MaxCellV = 0;
WeberYang 12:ed085acbbde5 87 uint32_t MinCellV = 0;
WeberYang 12:ed085acbbde5 88 uint32_t seq;
WeberYang 12:ed085acbbde5 89 //========================================
WeberYang 8:4974fc24fbd7 90
WeberYang 12:ed085acbbde5 91 void CAN_thread(const void *args);
WeberYang 12:ed085acbbde5 92 void DIO_thread(const void *args);
WeberYang 12:ed085acbbde5 93 //CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
WeberYang 12:ed085acbbde5 94 CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
WeberYang 12:ed085acbbde5 95 //CANMsg rxMsg;
WeberYang 12:ed085acbbde5 96 CANMessage rxMsg;
WeberYang 12:ed085acbbde5 97 osThreadDef(CAN_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
WeberYang 12:ed085acbbde5 98 osThreadDef(DIO_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
WeberYang 8:4974fc24fbd7 99
WeberYang 8:4974fc24fbd7 100 //======================================================================
WeberYang 12:ed085acbbde5 101 ros::NodeHandle nh;
WeberYang 2:648583d6e41a 102
WeberYang 12:ed085acbbde5 103 std_msgs::Int16 str_msg1;
WeberYang 12:ed085acbbde5 104 std_msgs::Int16 str_msg2;
WeberYang 12:ed085acbbde5 105 std_msgs::Int16 str_msg3;
WeberYang 12:ed085acbbde5 106 std_msgs::Int16 DI_data;
WeberYang 12:ed085acbbde5 107 std_msgs::Int16 ACT_state;
WeberYang 12:ed085acbbde5 108 std_msgs::Int16 Error_state;
WeberYang 12:ed085acbbde5 109 std_msgs::Int16 DO;
WeberYang 12:ed085acbbde5 110 sensor_msgs::BatteryState BTState;
WeberYang 12:ed085acbbde5 111
WeberYang 12:ed085acbbde5 112 ros::Publisher sonar1("sonar1", &str_msg1);
WeberYang 12:ed085acbbde5 113 ros::Publisher sonar2("sonar2", &str_msg2);
WeberYang 12:ed085acbbde5 114 ros::Publisher sonar3("sonar3", &str_msg3);
WeberYang 12:ed085acbbde5 115 ros::Publisher DI_data_pub("DI_data_pub", &DI_data);
WeberYang 12:ed085acbbde5 116 ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
WeberYang 12:ed085acbbde5 117 ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
WeberYang 12:ed085acbbde5 118 ros::Publisher BT_pub("BatteryState", &BTState);
WeberYang 2:648583d6e41a 119
WeberYang 12:ed085acbbde5 120 void DO_ACT(const std_msgs::Int16 &msg){
WeberYang 12:ed085acbbde5 121
WeberYang 12:ed085acbbde5 122
WeberYang 12:ed085acbbde5 123 DO_0 = (msg.data & 0x001);
WeberYang 12:ed085acbbde5 124 DO_1 = (msg.data & 0x002);
WeberYang 12:ed085acbbde5 125 // DO_2 = (msg.data & 0x004);
WeberYang 12:ed085acbbde5 126 // DO_3 = (msg.data & 0x008);
WeberYang 12:ed085acbbde5 127 DO_4 = (msg.data & 0x010);
WeberYang 12:ed085acbbde5 128 DO_5 = (msg.data & 0x020);
WeberYang 12:ed085acbbde5 129 DO_6 = (msg.data & 0x040);
WeberYang 12:ed085acbbde5 130 DO_7 = (msg.data & 0x080);
WeberYang 12:ed085acbbde5 131 DO_8 = (msg.data & 0x100);
WeberYang 12:ed085acbbde5 132 DO_9 = (msg.data & 0x200);
WeberYang 12:ed085acbbde5 133 // DO_10 = (msg.data & 0x0400);
WeberYang 12:ed085acbbde5 134 DO_12 = (msg.data & 0x1000);
WeberYang 12:ed085acbbde5 135 DO_13 = (msg.data & 0x2000);
WeberYang 12:ed085acbbde5 136 DO_14 = (msg.data & 0x4000);
WeberYang 12:ed085acbbde5 137 DO_15 = (msg.data & 0x8000);
WeberYang 12:ed085acbbde5 138
WeberYang 12:ed085acbbde5 139 led1 = !led1;
WeberYang 12:ed085acbbde5 140 osDelay(1);
hardtail 0:6e61e8ec4b42 141 }
WeberYang 12:ed085acbbde5 142 ros::Subscriber<std_msgs::Int16> DO_data_sub("DO_data_sub", &DO_ACT);
WeberYang 12:ed085acbbde5 143 //======================================================================
WeberYang 12:ed085acbbde5 144 void CAN_thread(const void *args){
WeberYang 12:ed085acbbde5 145 while (true) {
WeberYang 12:ed085acbbde5 146 if (can1.read(rxMsg)){
WeberYang 12:ed085acbbde5 147 seq++;
WeberYang 12:ed085acbbde5 148 SOC = rxMsg.data[0]>>1;
WeberYang 12:ed085acbbde5 149 Tempert = rxMsg.data[1]-50;
WeberYang 12:ed085acbbde5 150 RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2];
WeberYang 12:ed085acbbde5 151 Current = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4];
WeberYang 12:ed085acbbde5 152 MaxCellV = rxMsg.data[6];
WeberYang 12:ed085acbbde5 153 MinCellV = rxMsg.data[7];
WeberYang 6:eadfb1b45bda 154 }
WeberYang 9:e10bd4b460d7 155 else{
WeberYang 12:ed085acbbde5 156 BTState.voltage = 0;
WeberYang 12:ed085acbbde5 157 BTState.current = 0;
WeberYang 12:ed085acbbde5 158 BTState.design_capacity = 80;
WeberYang 12:ed085acbbde5 159 BTState.percentage = 0;
WeberYang 6:eadfb1b45bda 160 }
WeberYang 12:ed085acbbde5 161 BTState.header.stamp = nh.now();
WeberYang 12:ed085acbbde5 162 BTState.header.frame_id = 0;
WeberYang 12:ed085acbbde5 163 BTState.header.seq = seq;
WeberYang 12:ed085acbbde5 164 BTState.voltage = RackVoltage*0.1;
WeberYang 12:ed085acbbde5 165 BTState.current = Current;
WeberYang 12:ed085acbbde5 166 BTState.design_capacity = 50;
WeberYang 12:ed085acbbde5 167 BTState.percentage = SOC;
WeberYang 12:ed085acbbde5 168 osDelay(10);
WeberYang 6:eadfb1b45bda 169 }
WeberYang 12:ed085acbbde5 170 }
WeberYang 2:648583d6e41a 171
WeberYang 12:ed085acbbde5 172 void DIO_thread(const void *args){
WeberYang 12:ed085acbbde5 173 while (true) {
WeberYang 12:ed085acbbde5 174 DI_data.data = 0xffff-0x0001-0x0800 - ((int)DI_1<<1) - (((int)DI_2)<<2) - (((int)DI_3)<<3) - (((int)DI_4)<<4);
WeberYang 12:ed085acbbde5 175 DI_data.data -= (((int)DI_5)<<5);
WeberYang 12:ed085acbbde5 176 DI_data.data -= (((int)DI_6)<<6);
WeberYang 12:ed085acbbde5 177 DI_data.data -= (((int)DI_7)<<7);
WeberYang 12:ed085acbbde5 178 DI_data.data -= (((int)DI_8)<<8);
WeberYang 12:ed085acbbde5 179 DI_data.data -= (((int)DI_9)<<9);
WeberYang 12:ed085acbbde5 180 DI_data.data -= (((int)DI_10)<<10);
WeberYang 12:ed085acbbde5 181 // DI_data.data -= (((int)DI_11)<<11);
WeberYang 12:ed085acbbde5 182 DI_data.data -= (((int)DI_12)<<12);
WeberYang 12:ed085acbbde5 183 DI_data.data -= (((int)DI_13)<<13);
WeberYang 12:ed085acbbde5 184 DI_data.data -= (((int)DI_14)<<14);
WeberYang 12:ed085acbbde5 185 DI_data.data -= (((int)DI_15)<<15);
WeberYang 12:ed085acbbde5 186 osDelay(10);
WeberYang 5:52fb31c1a8c0 187 }
WeberYang 9:e10bd4b460d7 188 }
WeberYang 9:e10bd4b460d7 189 //======================================================================
WeberYang 10:8b7fce3bba86 190
WeberYang 5:52fb31c1a8c0 191 int main() {
WeberYang 6:eadfb1b45bda 192 RS232.baud(PC_BAUDRATE);
WeberYang 12:ed085acbbde5 193 nh.initNode();
WeberYang 12:ed085acbbde5 194 // nh.advertise(sonar1);
WeberYang 12:ed085acbbde5 195 // nh.advertise(sonar2);
WeberYang 12:ed085acbbde5 196 // nh.advertise(ACT_state_pub);
WeberYang 12:ed085acbbde5 197 nh.advertise(DI_data_pub);
WeberYang 12:ed085acbbde5 198 nh.advertise(BT_pub);
WeberYang 12:ed085acbbde5 199 nh.subscribe(DO_data_sub);
WeberYang 12:ed085acbbde5 200 // DI_0.mode(PullUp);
WeberYang 12:ed085acbbde5 201 DI_1.mode(PullUp);
WeberYang 12:ed085acbbde5 202 DI_2.mode(PullUp);
WeberYang 12:ed085acbbde5 203 DI_3.mode(PullUp);
WeberYang 12:ed085acbbde5 204 DI_4.mode(PullUp);
WeberYang 12:ed085acbbde5 205 DI_5.mode(PullUp);
WeberYang 12:ed085acbbde5 206 DI_6.mode(PullUp);
WeberYang 12:ed085acbbde5 207 DI_7.mode(PullUp);
WeberYang 12:ed085acbbde5 208 DI_8.mode(PullUp);
WeberYang 12:ed085acbbde5 209 DI_9.mode(PullUp);
WeberYang 12:ed085acbbde5 210 DI_10.mode(PullUp);
WeberYang 12:ed085acbbde5 211 // DI_11.mode(PullUp);
WeberYang 12:ed085acbbde5 212 DI_12.mode(PullUp);
WeberYang 12:ed085acbbde5 213 DI_13.mode(PullUp);
WeberYang 12:ed085acbbde5 214 DI_14.mode(PullUp);
WeberYang 12:ed085acbbde5 215 DI_15.mode(PullUp);
WeberYang 12:ed085acbbde5 216 wait_ms(10);
WeberYang 12:ed085acbbde5 217 osThreadCreate(osThread(CAN_thread), NULL);
WeberYang 12:ed085acbbde5 218 osThreadCreate(osThread(DIO_thread), NULL);
WeberYang 12:ed085acbbde5 219 // CANMessage rxMsg;
WeberYang 6:eadfb1b45bda 220 CAN_T = 0;
WeberYang 6:eadfb1b45bda 221 CAN_R = 0;
WeberYang 6:eadfb1b45bda 222 wait_ms(50);
WeberYang 12:ed085acbbde5 223 // CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
WeberYang 6:eadfb1b45bda 224 wait_ms(50);
WeberYang 6:eadfb1b45bda 225 can1.frequency(500000);
WeberYang 6:eadfb1b45bda 226 wait_ms(50);
WeberYang 9:e10bd4b460d7 227
WeberYang 6:eadfb1b45bda 228 while(1){
WeberYang 12:ed085acbbde5 229 DI_data_pub.publish( &DI_data);
WeberYang 12:ed085acbbde5 230 // sonar1.publish( &str_msg1);
WeberYang 12:ed085acbbde5 231 // sonar2.publish( &str_msg2);
WeberYang 12:ed085acbbde5 232 BT_pub.publish( &BTState );
WeberYang 12:ed085acbbde5 233 nh.spinOnce();
WeberYang 12:ed085acbbde5 234 osDelay(50);
hardtail 0:6e61e8ec4b42 235 }
hardtail 0:6e61e8ec4b42 236 }