File content as of revision 11:6d5307ceb569:

/*
0412 combine the sevro motor encoder to publish
*/
#include "MPU6050_6Axis_MotionApps20.h"
#include "mbed.h"
#include "CAN.h"
#include "I2Cdev.h"
#include "MPU6050.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 <geometry_msgs/Twist.h> //set buffer larger than 50byte
#include <math.h>
#include <stdio.h>
#include <tiny_msgs/tinyVector.h>
#include <tiny_msgs/tinyIMU.h>
#include <string> 
#include <cstdlib>

#define Start 0xAA
#define Address 0x7F
#define ReturnType 0x00
#define Clean 0x00
#define Reserve 0x00
#define End 0x55 
#define Motor1 1 
#define Motor2 2 
#define LENG 31   //0x42 + 31 bytes equal to 32 bytes

#define Write 0x06
#define Read 0x03
#define DI1 0x0214      //0214H means digital input DI1 for sevro on
#define APR 0x0066      //0066H means encoder abs rev
#define SP1 0x0112

#define CAN_DATA  0x470
#define CAN_STATUS  0x471

#define IDLE            0
#define ACT_MG_ON       1
#define ACT_MG_OFF      2
#define Check_BMS_ON    3
#define Check_BMS_OFF   4
#define WAIT_BAT        5
//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_5);
DigitalOut          DO_1(PC_6);
DigitalOut          DO_2(PC_8);
DigitalOut          DO_3(PC_9);
DigitalOut          DO_4(PA_12);
DigitalIn           DI_0(PB_13);

//CAN                 can1(PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
//CANMsg              rxMsg;
//CANMessage          rxMsg;
Ticker CheckDataR;

MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin

ros::NodeHandle  nh;
//======================================================================
tiny_msgs::tinyIMU imu_msg;
ros::Publisher imu_pub("tinyImu", &imu_msg);
//======================================================================

//======================================================================
std_msgs::Float32 VelAngular_L;
ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
//======================================================================

//======================================================================
std_msgs::Float32 VelAngular_R;
ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
//======================================================================

//======================================================================
sensor_msgs::BatteryState BTState;
ros::Publisher BT_pub("BatteryState", &BTState);
//======================================================================

//======================================================================
std_msgs::Int16 DI;
ros::Publisher DI_pub("DI_pub", &DI);
//======================================================================

//======================================================================
std_msgs::Int16 ACT_state;
ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
//======================================================================

//======================================================================
std_msgs::Int16 Error_state;
ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
//======================================================================

uint32_t seq;

//========define ACT_state return code============================================
#define Satndby         0
#define Busy            1
#define Sensor_error    2
#define BMS_error       3
//========================================================
#define IMU_FIFO_RATE_DIVIDER 0x09
#define IMU_SAMPLE_RATE_DIVIDER 4
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
 
#define PC_BAUDRATE 38400     
 
#define DEG_TO_RAD(x) ( x * 0.01745329 )
#define RAD_TO_DEG(x) ( x * 57.29578 )

const int FIFO_BUFFER_SIZE = 128;
uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
uint16_t fifoCount;
uint16_t packetSize;
bool dmpReady;
uint8_t mpuIntStatus;
const int snprintf_buffer_size = 100;
char snprintf_buffer[snprintf_buffer_size];
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
int16_t ax, ay, az;
int16_t gx, gy, gz;
float Lrpm,Rrpm;
float ticks_since_target;
float motor_rpm_r, motor_rpm_l;
float timeout_ticks;
int counter;
    double w;
    double rate;
    double Dimeter;
    float dx,dy,dr;
int lastsensorState = 1;    
int sensorState;     
int db_conter = 0;
int buffer[9] = {0};
int dataH,datanum;
int motor_seq,motor_old_seq;
int state_code;
int error_code;
//=========RS485
char recChar=0;
bool recFlag=false;
char recArr[20];
int index=0;
int BMS_state;
        uint32_t SOC;
        uint32_t Tempert;
        uint32_t RackVoltage = 0;
        uint32_t Current = 0;
        uint32_t MaxCellV = 0;
        uint32_t MinCellV = 0;

struct Offset {
    int16_t ax, ay, az;
    int16_t gx, gy, gz;
}offset = {150+600, -350+300, 1000, -110-100, 5, 0};//{150, -350, 1000, -110, 5, 0};    // Measured values
 
struct MPU6050_DmpData {
    Quaternion q;
    VectorFloat gravity;    // g
    float roll, pitch, yaw;     // rad
}dmpData;
 
long map(long x, long in_min, long in_max, long out_min, long out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
//==========define sub function========================
bool Init();
void dmpDataUpdate();
unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
int myabs( int a );
void TwistToMotors();   
//===================================================
 

//=======================     motor      =================================================
unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
{
    unsigned int i, j;
    //#define wPolynom 0xA001
    unsigned int wCrc = 0xffff;
    unsigned int wPolynom = 0xA001;
    /*---------------------------------------------------------------------------------*/
    for (i = 0; i < iBufLen; i++)
    {
        wCrc ^= cBuffer[i];
        for (j = 0; j < 8; j++)
        {
            if (wCrc &0x0001)
            {
                wCrc = (wCrc >> 1) ^ wPolynom;
            }
            else
            { 
                wCrc = wCrc >> 1;
            }
        }
    }
    return wCrc;
}
void Sendmessage(float Rrpm,float Lrpm)
{
//    RS232.printf("Wr = %.1f\n",Rrpm);
//    RS232.printf("Wl = %.1f\n",Lrpm);
unsigned char sendData[16];
unsigned int tmpCRC;
int motor1,motor2;
   
    sendData[0] = Start;
    sendData[1] = Address;
    sendData[2] = ReturnType;
    sendData[3] = Clean;
    sendData[4] = Reserve;
    sendData[5]  = 0x01;//motor1Sevro ON
    sendData[6] = 0x01;//motor2Sevro ON
if (Rrpm>0){sendData[7] = 0x00;}else{sendData[7] = 0x01;}
if (Lrpm>0){sendData[8] = 0x01;}else{sendData[8] = 0x00;}
   motor1 =  abs(Rrpm);
   motor2 =  abs(Lrpm); 
    
    sendData[9] = (motor1>>8);//motor1speedH
    sendData[10] = (motor1 & 0xFF);//motor1speedL
    sendData[11] = (motor2>>8);//motor2speedH
    sendData[12] = (motor2 & 0xFF);//motor2speedL
    sendData[13] = End;
    tmpCRC = CRC_Verify(sendData, 14);
    sendData[14] = (tmpCRC & 0xFF);
    sendData[15] = (tmpCRC>>8);
    int i;
    for (i=0;i<16;i++)
    {
        RS232.printf("%c",sendData[i]);
    }
    RS232.printf("\r\n");
}
void TwistToMotors()
{
    float right,left;
//    double vel_data[2];
    float vel_data[2];
    float motor_rpm_vx, motor_rpm_theta;
    motor_old_seq = motor_seq;
    w = 0.302;//0.2 ;//m
    rate = 20;//50;
    timeout_ticks = 2;
    Dimeter = 0.127;//0.15;
    
    // prevent agv receive weird 1.0 command from cmd_vel
    if (dr == 1.0){
        dr = 0.001;
    }
    right = ( 1.0 * dx ) + (dr * w /2);
    left = ( 1.0 * dx ) - (dr * w /2);
    motor_rpm_vx = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
    if((motor_rpm_vx !=0) && (myabs(motor_rpm_vx)<100)){
        if(motor_rpm_vx >0){
            motor_rpm_vx = 100;
        }
        else{
            motor_rpm_vx = -100;
        }
    }
    motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);
    motor_rpm_r = motor_rpm_vx+ motor_rpm_theta;
    motor_rpm_l = motor_rpm_vx- motor_rpm_theta;
    if (myabs(motor_rpm_r)<100|| myabs(motor_rpm_l)<100){
        if( dx==0){
            if(dr>0){
                motor_rpm_r=100;
                motor_rpm_l=-100;
            }else if (dr<0){
                motor_rpm_r=-100;
                motor_rpm_l=100; 
            }else{
                motor_rpm_r=0;
                motor_rpm_l=0;
            }
        }
        else if(dx>0){
            if (myabs(motor_rpm_r)<100){
                motor_rpm_r =100;
            }
            if (myabs(motor_rpm_l)<100){
                motor_rpm_l =100;
            }
        }
        else{
            if(myabs(motor_rpm_r)<100){
                motor_rpm_r =-100;
            }
            if(myabs(motor_rpm_l)<100){
                motor_rpm_l =-100;
            }
        }
    }
        vel_data[0] = motor_rpm_r;
        vel_data[1] = motor_rpm_l;
        
//===================================================================
    //Sendmessage(vel_data[0],vel_data[1]);
       
    //Sendmessage(motor_rpm_l,motor_rpm_r);
       
    VelAngular_R.data = vel_data[0];
    VelAngular_L.data = vel_data[1];
    //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
    //}
    //else{
    pub_rmotor.publish( &VelAngular_R );
    pub_lmotor.publish( &VelAngular_L );
    //}
    //RS232.printf("Wr = %.1f\n",vel_data[0]);
    //RS232.printf("Wl = %.1f\n",vel_data[1]);
    ticks_since_target += 1;

}
int myabs( int a ){
    if ( a < 0 ){
        return -a;
        }
        return a;
        }

int str2int(const char* str, int star, int end)
{
    int i;
    int ret = 0;
    for (i = star; i < end+1; i++)
    {
        ret = ret *10 + (str[i] - '0');
    }
    return ret;     
}       

void update_state(int code1,int code2){

}
//======================================================================
std_msgs::Int16 DO;
//DO_0 MAG_1
//DO_1,MAG_2
//DO_2,MAG_3
//DO_3,BMS
//DO_4,MainRelay
int State;
void DO_ACT(const std_msgs::Int16 &msg){
    //0xFF for action procedure
    if (msg.data == 0x21){
        error_code = 99;
        State = WAIT_BAT;//ACT_MG_ON;
    }
    
    if (msg.data == 0x20){
        error_code = 99;
        State = ACT_MG_OFF;
    }
    
    if (msg.data == 0x00){
        DO_0 = 0;
        DO_1 = 0;
        DO_2 = 0;
        DO_3 = 0;
        DO_4 = 0;
    }
    if (msg.data == 0x40){    
            //BMS trigger
            DO_3 = 1;            
            wait(3);
            DO_3 = 0;
    }
        if (msg.data == 0x50){    
            //Main Relay off
            DO_4 = 0;            
    }
        if (msg.data == 0x51){    
            //Main Relay on
            DO_4 = 1;            
    }
    if (msg.data == 0x31){    
            //Lock triggrt
            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 0;
            DO_1 = 1;
            DO_2 = 0;
                        
            wait_ms(500);

            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;
    }
    
    if (msg.data == 0x30){    
            //unLock triggrt
            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 1;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 1;
            DO_1 = 0;
            DO_2 = 1;

            wait_ms(500);
          
            DO_0 = 1;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;
    }

}
ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
//======================================================================
//======================================================================================
void messageCb(const geometry_msgs::Twist &msg)
{  
//    RS232.printf("messageCb");
    ticks_since_target = 0;
    dx = msg.linear.x;
    dy = msg.linear.y;
    dr = msg.angular.z;
//    RS232.printf("dx = %d,dy = %d,dr = %d\r\n",dx,dy,dr);
    TwistToMotors(); 
    //ReadENC(Motor1);
}
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
 //======================================================================================
void dmpDataUpdate() {
    // Check that this interrupt has enabled.
    if (dmpReady == false) return;
    
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    
    // Check that this interrupt is a FIFO buffer overflow interrupt.
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();
        //pc.printf("FIFO overflow!\n");
        return;
        
    // Check that this interrupt is a Data Ready interrupt.
    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
    #ifdef OUTPUT_QUATERNION
        mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Quaternion : w=%f, x=%f, y=%f, z=%f\n", dmpData.q.w, dmpData.q.x, dmpData.q.y, dmpData.q.z ) < 0 ) return;
        pc.printf(snprintf_buffer);
    #endif
        
    #ifdef OUTPUT_EULER
        float euler[3];
        mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
        mpu.dmpGetEuler(euler, &dmpData.q);
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Euler : psi=%fdeg, theta=%fdeg, phi=%fdeg\n", RAD_TO_DEG(euler[0]), RAD_TO_DEG(euler[1]), RAD_TO_DEG(euler[2]) ) < 0 ) return;
        pc.printf(snprintf_buffer);
    #endif
        
    #ifdef OUTPUT_ROLL_PITCH_YAW
        mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
        mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
        float rollPitchYaw[3];
        mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
        dmpData.roll = rollPitchYaw[2];
        dmpData.pitch = rollPitchYaw[1];
        dmpData.yaw = rollPitchYaw[0];
        
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(dmpData.roll), RAD_TO_DEG(dmpData.pitch), RAD_TO_DEG(dmpData.yaw) ) < 0 ) return;
        pc.printf(snprintf_buffer);
                 
#ifdef servotest
        int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
        if(servoPulse > 1450) servoPulse = 1450;
        if(servoPulse < 500) servoPulse = 500;
        sv.pulsewidth_us(servoPulse);
#endif
    #endif
        
    #ifdef OUTPUT_FOR_TEAPOT
        teapotPacket[2] = fifoBuffer[0];
        teapotPacket[3] = fifoBuffer[1];
        teapotPacket[4] = fifoBuffer[4];        
        teapotPacket[5] = fifoBuffer[5];
        teapotPacket[6] = fifoBuffer[8];
        teapotPacket[7] = fifoBuffer[9];
        teapotPacket[8] = fifoBuffer[12];
        teapotPacket[9] = fifoBuffer[13];
        for (uint8_t i = 0; i < 14; i++) {
            pc.putc(teapotPacket[i]);
        }
    #endif
        
    #ifdef OUTPUT_TEMPERATURE
        float temp = mpu.getTemperature() / 340.0 + 36.53;
        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
        pc.printf(snprintf_buffer);
    #endif
        
//        pc.printf("\n");
    }
}
//=======================================================
bool Init() {
    DO_0 = 0;
    DO_1 = 0;
    DO_2 = 0;
    DO_3 = 0;
    DO_4 = 1;//for manual turn on the LiBattery, and turn on the main relay to open Load
    seq = 0;
    nh.initNode();
//    nh.advertise(imu_pub);
    nh.advertise(pub_lmotor);
    nh.advertise(pub_rmotor);
    nh.advertise(BT_pub);
    nh.advertise(DI_pub);
    nh.advertise(ACT_state_pub);
    nh.advertise(Error_state_pub);
    nh.subscribe(cmd_vel_sub);
    nh.subscribe(ACT_sub);
    
    //==========================
    /*
    mpu.initialize();
    if (mpu.testConnection()) {
//        pc.printf("MPU6050 test connection passed.\n");
    } else {
//        pc.printf("MPU6050 test connection failed.\n");
        return false;
    }
    if (mpu.dmpInitialize() == 0) {
//        pc.printf("succeed in MPU6050 DMP Initializing.\n");
    } else {
//        pc.printf("failed in MPU6050 DMP Initializing.\n");
        return false;
    }

    //mpu.setXAccelOffsetTC(offset.ax);
//    mpu.setYAccelOffsetTC(offset.ay);
//    mpu.setZAccelOffset(offset.az);
    mpu.setXAccelOffset(1000); //2000 -3134
    mpu.setYAccelOffset(0);
    mpu.setZAccelOffset(0);
    mpu.setXGyroOffset(800);//offset.gx);
    mpu.setYGyroOffset(0);//offset.gy);
    mpu.setZGyroOffset(0);//offset.gz);
    
    
    
    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);//2000);
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    mpu.setDMPEnabled(true);    // Enable DMP
    
    packetSize = mpu.dmpGetFIFOPacketSize();
    dmpReady = true;    // Enable interrupt.
        */
    //pc.printf("Init finish!\n");
 
    return true;
}
int MG_ACT(int state)
{
//    int ret;
    if (state == 1){    //MAG_ON
//        if (sensorState == 0){     //Battery on-position
            //Lock triggrt
            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 0;
            DO_1 = 1;
            DO_2 = 0;
                        
            wait_ms(500);

            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;
            
            // if Battery is off, then do turn on
//            if (BMS_state == 0){
            //BMS ON
            DO_3 = 1;
            wait(4);
            DO_3 = 0;
//            }
            return 1;
//        }
    }
    else if (state == 2){//MAG_OFF
            // if Battery is on, then do trun off
//            if (BMS_state == 1){
            //BMS ON
            DO_3 = 1;
            wait(4);
            DO_3 = 0;
//            }

            //unLock triggrt
            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 1;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 1;
            DO_1 = 0;
            DO_2 = 1;

            wait_ms(500);
          
            DO_0 = 1;
            DO_1 = 0;
            DO_2 = 0;

            DO_0 = 0;
            DO_1 = 0;
            DO_2 = 0;

            return 1;
    }
    return 0;
}
//=======================================================
int main() {
    RS232.baud(PC_BAUDRATE);
    MBED_ASSERT(Init() == true);
    CANMessage rxMsg;
    DI_0.mode(PullUp);
    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);
    
    //Lock triggrt
    wait_ms(500);

    DO_0 = 0;
    DO_1 = 0;
    DO_2 = 0;

    DO_0 = 0;
    DO_1 = 1;
    DO_2 = 0;
                        
    wait_ms(500);

    DO_0 = 0;
    DO_1 = 0;
    DO_2 = 0;
    
    wait_ms(500);
    
    while(1){   
        seq++;
        motor_seq = seq;
        t.start();
        //================================
        //========define ACT_state return code============================================
        //#define Satndby         0x00
        //#define Busy            0x01
        //#define Sensor_error    0x10
        //========================================================
        counter++;
        state_code = State;
        switch (State){
            int ret;
            case IDLE:
                counter = 0;
            break;
            
            case WAIT_BAT:
            if (sensorState == 0){
                State = ACT_MG_ON;
                counter = 0;
            }
            if (counter>1000){
                State = IDLE;
                error_code = Sensor_error;
            }
            break;
            
            case ACT_MG_ON:
                ret = MG_ACT(1);
                if (ret){
//                     DO_4 = 0; 
                     State = Check_BMS_ON;
                     counter = 0;
                }
                if (counter>10){
                    State = IDLE;
                    error_code = Sensor_error;
                    DO_0 = 0;
                    DO_1 = 0;
                    DO_2 = 0;
                    DO_3 = 0;
                    DO_4 = 0;
                }
            break;
            
            case Check_BMS_ON:
                if (BMS_state == 1){
                    // turn on parrel wire to share the UPS current
                    DO_4 = 1;
                    State = IDLE;
                    error_code = 0;
                }
                if (counter>100){
                    State = IDLE;
                    error_code = BMS_error;
                    DO_0 = 0;
                    DO_1 = 0;
                    DO_2 = 0;
                    DO_3 = 0;
                    DO_4 = 0;
                }   
            break;
            
            case ACT_MG_OFF:
                // turn off parrel wire to avoid voltage feedback to UPS
                DO_4 = 0;
                
                ret = MG_ACT(2);
                if (ret){ 
                     State = Check_BMS_OFF;
                     counter = 0;
                }
                if (counter>10){
                    State = IDLE;
                    error_code = Sensor_error;
                }                
            break;
            
            case Check_BMS_OFF:
                if (BMS_state == 0){
                    State = IDLE;
                    error_code = 0;
                }
                if (counter>100){
                    State = IDLE;
                    error_code = BMS_error;
                }   
            break;
            }
            
        ACT_state.data = state_code;
        ACT_state_pub.publish( &ACT_state);
        wait_ms(10);
        Error_state.data = error_code;
        Error_state_pub.publish( &Error_state);
        wait_ms(10);
        //============DI==================
        int reading = DI_0;
        if (reading == lastsensorState) {
            db_conter = db_conter+1;
        }
        else{
            db_conter = 0;
        }        
        if (db_conter > 3) {
            sensorState = reading;
            DI.data = sensorState; 
        }

        lastsensorState = reading;
        DI_pub.publish( &DI);
        wait_ms(10);
        //=========================================
        if (can1.read(rxMsg) && (t.read_ms() > 5000))  {
            if(rxMsg.id == CAN_DATA){
                BMS_state = 1;
                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];                
                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 = 80;
                BTState.percentage = SOC;
                BT_pub.publish( &BTState );
                t.reset();   
            } // if
        } // if
        else{
            BMS_state = 0;
        }
        
        
        if (motor_seq  > motor_old_seq + 5){
        motor_rpm_r = 0;
        motor_rpm_l = 0;
        }       
        
        Sendmessage(motor_rpm_l,motor_rpm_r);
        wait_ms(70);
        //wait_ms(100);     
        nh.spinOnce();    
    }
}