File content as of revision 9:f9e9662d26bf:

/*
0412 combine the sevro motor encoder to publish
0523 test fork
*/
#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

//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_8);
DigitalOut          DO_1(PC_9);
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 DO;

void DO_ACT(const std_msgs::Int16 &msg){
    if (msg.data & 0x01){
        DO_0 = 1;
    }
    else{
        DO_0 = 0;
    }

    if (msg.data & 0x02){
        DO_1 = 1;
    }
    else{
        DO_1 = 0;
    }    
}
ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
//======================================================================
uint32_t seq;

#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 115200//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;
double timeout_ticks;

    double w;
    double rate;
    double Dimeter;
    float dx,dy,dr;
int lastButtonState = 1;    
int buttonState;     
int db_conter = 0;
int buffer[9] = {0};
int dataH,datanum;
//=========RS485
char recChar=0;
bool recFlag=false;
char recArr[20];
int index=0;

        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, -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] = 0x01;}else{sendData[7] = 0x00;}
if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
   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()
{
    w = 0.302;//0.2 ;//m
    rate = 20;//50;
    timeout_ticks = 2;
    Dimeter = 0.127;//0.15;
    float right,left;
    float motor_rpm_r, motor_rpm_l;
    double vel_data[2];
    right = ( 1.0 * dx ) + (dr * w /2);
//   left = ( 1.0 * dx ) - (dr * w /2);
//    vel_data[0] = right*rate/Dimeter/60*1000;
//    vel_data[1] =  left*rate/Dimeter/60*1000;
   if (dx!=0)
    {
     if (dx>0)
     { 
       if (dr >=0)
       {
           motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
           motor_rpm_l=300;
       }
       else
       {
           motor_rpm_r=300;
           motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
       }
     }
     else
     {
         if(dr>=0)
         {
          motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
          motor_rpm_l=(-300);
         }
         else
         {
             motor_rpm_r=(-300);
             motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
         }
     }
    }
    else
    {
        if(dr>=0)
        {
            motor_rpm_r=100;
            motor_rpm_l=-100;
        }
        else
        {
            motor_rpm_r=-100;
            motor_rpm_l=100;
        }
    }
    vel_data[0]=motor_rpm_r;
    vel_data[1]=motor_rpm_l; 
    //================================================================ Original Version
    /*if (dr>=0)
    {
        right = ( 1.0 * dx ) + (dr * w /2);
        left = ( 1.0 * dx );
    }
    else
    {
        right = ( 1.0 * dx );
        left = ( 1.0 * dx ) - (dr * w /2);
    }
        vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60;
        vel_data[1] =  left*rate/(Dimeter/2)/(2*3.1416)*60;*/
//===================================================================
    Sendmessage(vel_data[0],vel_data[1]);
    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 messageCb(const geometry_msgs::Twist &msg)
{  
    ticks_since_target = 0;
    dx = msg.linear.x;
    dy = msg.linear.y;
    dr = msg.angular.z;
    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;
    
    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.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.setXAccelOffset(offset.ax);
    mpu.setYAccelOffset(offset.ay);
    mpu.setZAccelOffset(offset.az);
    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    mpu.setXGyroOffset(offset.gx);
    mpu.setYGyroOffset(offset.gy);
    mpu.setZGyroOffset(offset.gz);
    mpu.setDMPEnabled(true);    // Enable DMP
    packetSize = mpu.dmpGetFIFOPacketSize();
    dmpReady = true;    // Enable interrupt.
    
    //pc.printf("Init finish!\n");
 
    return true;
}
//=======================================================
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);
    while(1){   
        seq++;
        t.start();
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

        imu_msg.header.stamp = nh.now();
        imu_msg.header.frame_id = 0;
        imu_msg.header.seq = seq;
        imu_msg.accel.x = ax;
        imu_msg.accel.y = ay;
        imu_msg.accel.z = az;
        imu_msg.gyro.x = gx;
        imu_msg.gyro.y = gy;
        imu_msg.gyro.z = gz;   
//        
        imu_pub.publish( &imu_msg );
        //============DI==================
        int reading = DI_0;
        if (reading == lastButtonState) {
            db_conter = db_conter+1;
        }
        else{
            db_conter = 0;
        }
        
        if (db_conter > 3) {
//            if (reading != buttonState) {
            buttonState = reading;
            DI.data = buttonState; 
//            }
        }

        lastButtonState = reading;
        DI_pub.publish( &DI);

        //=========================================
        if (can1.read(rxMsg) && (t.read_ms() > 5000))  {
//            RS232.printf("  ID      = 0x%.3x\r\n", rxMsg.id);

            if(rxMsg.id == CAN_DATA){
                SOC = rxMsg.data[0]>>1;
                Tempert = rxMsg.data[1]-50;
                RackVoltage  = ((unsigned int)rxMsg.data[3] << 8) + rxMsg.data[2];
                Current      = ((unsigned int)rxMsg.data[5] << 8) + rxMsg.data[4];
                MaxCellV = rxMsg.data[6];
                MinCellV = rxMsg.data[7];
//                RS232.printf("SOC = %d\r\n",SOC);
//                RS232.printf("Tempert = %d\r\n",Tempert);
//                RS232.printf("RackVoltage = %.2f\r\n",RackVoltage*0.1);
//                RS232.printf("Current = %.2f\r\n",Current*0.1);
//                RS232.printf("MaxCellV = %.2f\r\n",MaxCellV*0.02);
//                RS232.printf("MinCellV = %.2f\r\n",MinCellV*0.02);
                
                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
        nh.spinOnce();
        wait_ms(10);         
    }
}