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);          
    }
}