main.cpp

00001 /*
00002 0412 combine the sevro motor encoder to publish
00003 */
00004 #include "mbed.h"
00005 #include "CAN.h"
00006 #include "cmsis_os.h"
00007 #include <ros.h>
00008 #include <ros/time.h>
00009 #include <std_msgs/Int16.h>
00010 #include <std_msgs/String.h>
00011 #include <std_msgs/Float32.h>
00012 #include <sensor_msgs/BatteryState.h>
00013 #include <sensor_msgs/LaserEcho.h>
00014 #include <geometry_msgs/Twist.h> //set buffer larger than 50byte
00015 #include <math.h>
00016 #include <stdio.h>
00017 #include <string> 
00018 #include <cstdlib>
00019 
00020 #define CAN_DATA  0x470
00021 #define CAN_STATUS  0x471
00022 
00023 #define PC_BAUDRATE 115200
00024 
00025 //Serial pc(USBTX,USBRX);
00026 Timer t;
00027 Serial              RS232(PA_9, PA_10);
00028 DigitalOut          Receiver(D7);     //RS485_E
00029 DigitalOut          CAN_T(D14);
00030 DigitalOut          CAN_R(D15);
00031 
00032 DigitalOut          DO_0(PC_3);
00033 DigitalOut          DO_1(PC_2);
00034 //DigitalOut          DO_2(PF_1);
00035 //DigitalOut          DO_3(PF_0);
00036 DigitalOut          DO_4(PC_15);
00037 DigitalOut          DO_5(PC_14);
00038 DigitalOut          DO_6(PC_13);
00039 DigitalOut          DO_7(PB_7);
00040 DigitalOut          DO_8(PA_14);
00041 DigitalOut          DO_9(PA_13);
00042 //DigitalOut          DO_10(PF_7);
00043 //DigitalOut          DO_11(PF_6);
00044 DigitalOut          DO_12(PC_12);
00045 DigitalOut          DO_13(PC_10);
00046 DigitalOut          DO_14(PD_2);
00047 DigitalOut          DO_15(PC_11);
00048 DigitalOut          led1(LED1);
00049 
00050 //DigitalIn           DI_0(PF_5);
00051 DigitalIn           DI_1(PC_4);
00052 DigitalIn           DI_2(PB_13);
00053 DigitalIn           DI_3(PB_14);
00054 DigitalIn           DI_4(PB_15);
00055 DigitalIn           DI_5(PB_1);
00056 DigitalIn           DI_6(PB_2);
00057 DigitalIn           DI_7(PB_11);
00058 DigitalIn           DI_8(PB_12);
00059 DigitalIn           DI_9(PA_11);
00060 DigitalIn           DI_10(PA_12);
00061 //DigitalIn           DI_11(PD_8);
00062 DigitalIn           DI_12(PC_5);
00063 DigitalIn           DI_13(PC_6);
00064 DigitalIn           DI_14(PC_8);
00065 DigitalIn           DI_15(PC_9);
00066 
00067 
00068 //const unsigned int  RX_ID = 0x792;
00069 int id_count[15];
00070 long        Radial_range[15];
00071 long        Radial_speed[15];
00072 long        Radial_Acc[15];
00073 long        Angle[15];
00074 long        Power[15];
00075 long tmp_dis;
00076 long dis[7];
00077 long ldata;
00078 float fdata1,fdata2;
00079 float fRange[15],fAngle[15];
00080 
00081 //=========Battery varable================
00082 uint32_t SOC;
00083 uint32_t Tempert;
00084 uint32_t RackVoltage = 0;
00085 uint32_t Current = 0;
00086 uint32_t MaxCellV = 0;
00087 uint32_t MinCellV = 0;
00088 uint32_t seq;
00089 //========================================
00090 
00091 void CAN_thread(const void *args);
00092 void DIO_thread(const void *args);
00093 //CAN                 can1(PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
00094 CAN     can1(D15,D14);//PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
00095 //CANMsg              rxMsg;
00096 CANMessage          rxMsg;
00097 osThreadDef(CAN_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
00098 osThreadDef(DIO_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
00099 
00100 //======================================================================
00101 ros::NodeHandle  nh;
00102 
00103 std_msgs::Int16 str_msg1;
00104 std_msgs::Int16 str_msg2;
00105 std_msgs::Int16 str_msg3;
00106 std_msgs::Int16 DI_data;
00107 std_msgs::Int16 ACT_state;
00108 std_msgs::Int16 Error_state;
00109 std_msgs::Int16 DO;
00110 sensor_msgs::BatteryState BTState;
00111 
00112 ros::Publisher sonar1("sonar1", &str_msg1);
00113 ros::Publisher sonar2("sonar2", &str_msg2);
00114 ros::Publisher sonar3("sonar3", &str_msg3);
00115 ros::Publisher DI_data_pub("DI_data_pub", &DI_data);
00116 ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
00117 ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
00118 ros::Publisher BT_pub("BatteryState", &BTState);
00119 
00120 void DO_ACT(const std_msgs::Int16 &msg){    
00121         
00122         
00123        DO_0 = (msg.data & 0x001);
00124        DO_1 = (msg.data & 0x002);
00125 //       DO_2 = (msg.data & 0x004);
00126 //       DO_3 = (msg.data & 0x008);
00127        DO_4 = (msg.data & 0x010);
00128        DO_5 = (msg.data & 0x020);
00129        DO_6 = (msg.data & 0x040);
00130        DO_7 = (msg.data & 0x080);
00131        DO_8 = (msg.data & 0x100);
00132        DO_9 = (msg.data & 0x200);
00133 //       DO_10 = (msg.data & 0x0400);
00134        DO_12 = (msg.data & 0x1000);
00135        DO_13 = (msg.data & 0x2000);
00136        DO_14 = (msg.data & 0x4000);
00137        DO_15 = (msg.data & 0x8000);        
00138         
00139         led1 = !led1;        
00140         osDelay(1);
00141 }
00142 ros::Subscriber<std_msgs::Int16> DO_data_sub("DO_data_sub", &DO_ACT);
00143 //======================================================================
00144 void CAN_thread(const void *args){
00145     while (true) {      
00146          if (can1.read(rxMsg)){
00147             seq++;
00148             SOC = rxMsg.data[0]>>1;
00149             Tempert = rxMsg.data[1]-50;
00150             RackVoltage  = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2];
00151             Current      = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4];        
00152             MaxCellV = rxMsg.data[6];
00153             MinCellV = rxMsg.data[7];                                                
00154         }
00155         else{
00156             BTState.voltage = 0;
00157             BTState.current = 0;
00158             BTState.design_capacity = 80;
00159             BTState.percentage = 0;            
00160         }
00161         BTState.header.stamp = nh.now();
00162         BTState.header.frame_id = 0;
00163         BTState.header.seq = seq;
00164         BTState.voltage = RackVoltage*0.1;
00165         BTState.current = Current;
00166         BTState.design_capacity = 50;
00167         BTState.percentage = SOC;                                 
00168         osDelay(10);        
00169     }
00170 }
00171 
00172 void DIO_thread(const void *args){
00173     while (true) {      
00174         DI_data.data = 0xffff-0x0001-0x0800 - ((int)DI_1<<1) - (((int)DI_2)<<2) - (((int)DI_3)<<3) - (((int)DI_4)<<4);
00175         DI_data.data -= (((int)DI_5)<<5);
00176         DI_data.data -= (((int)DI_6)<<6);
00177         DI_data.data -= (((int)DI_7)<<7);
00178         DI_data.data -= (((int)DI_8)<<8);
00179         DI_data.data -= (((int)DI_9)<<9);
00180         DI_data.data -= (((int)DI_10)<<10);
00181 //        DI_data.data -= (((int)DI_11)<<11);
00182         DI_data.data -= (((int)DI_12)<<12);
00183         DI_data.data -= (((int)DI_13)<<13);
00184         DI_data.data -= (((int)DI_14)<<14);
00185         DI_data.data -= (((int)DI_15)<<15);
00186         osDelay(10);        
00187     }
00188 }
00189 //======================================================================
00190 
00191 int main() {
00192     RS232.baud(PC_BAUDRATE);
00193     nh.initNode();
00194 //    nh.advertise(sonar1);
00195 //    nh.advertise(sonar2);
00196 //    nh.advertise(ACT_state_pub);
00197     nh.advertise(DI_data_pub);
00198     nh.advertise(BT_pub);
00199     nh.subscribe(DO_data_sub);
00200 //    DI_0.mode(PullUp);
00201     DI_1.mode(PullUp);
00202     DI_2.mode(PullUp);
00203     DI_3.mode(PullUp);
00204     DI_4.mode(PullUp);
00205     DI_5.mode(PullUp);
00206     DI_6.mode(PullUp);
00207     DI_7.mode(PullUp);
00208     DI_8.mode(PullUp);
00209     DI_9.mode(PullUp);
00210     DI_10.mode(PullUp);
00211 //    DI_11.mode(PullUp);
00212     DI_12.mode(PullUp);
00213     DI_13.mode(PullUp);
00214     DI_14.mode(PullUp);
00215     DI_15.mode(PullUp);
00216     wait_ms(10);
00217     osThreadCreate(osThread(CAN_thread), NULL);
00218     osThreadCreate(osThread(DIO_thread), NULL);
00219 //    CANMessage rxMsg;    
00220     CAN_T = 0;
00221     CAN_R = 0;
00222     wait_ms(50);
00223 //    CAN     can1(D15,D14);//PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
00224     wait_ms(50);
00225     can1.frequency(500000);
00226     wait_ms(50);
00227     
00228     while(1){   
00229         DI_data_pub.publish( &DI_data);
00230 //        sonar1.publish( &str_msg1);
00231 //        sonar2.publish( &str_msg2);
00232         BT_pub.publish( &BTState );        
00233         nh.spinOnce();                         
00234         osDelay(50);          
00235     }
00236 }