File content as of revision 2:648583d6e41a:

/*

*/
#include "MPU6050_6Axis_MotionApps20.h"
#include "mbed.h"
//#include "CANMsg.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Empty.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>

#define Start 0xAA
#define Address 0x7F
#define ReturnType 0x00
#define Clean 0x00
#define Reserve 0x00
#define End 0x55 
#define Motor1 1 
#define Motor2 2 
//Serial              pc(PA_2, PA_3,9600);
Serial              RS485(PA_9, PA_10);
DigitalOut          RS485_E(D7);                     //RS485_E
DigitalOut          myled(LED1);
MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin

tiny_msgs::tinyIMU imu_msg;
std_msgs::String str_msg;
std_msgs::Float32 VelAngular_L;
std_msgs::Float32 VelAngular_R;

ros::NodeHandle  nh;
ros::Publisher imu_pub("tinyImu", &imu_msg);
ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);

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     
 
#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;
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);
void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data);
void SVON(char MotorStation);
void SVOFF(char MotorStation);
int myabs( int a );
void TwistToMotors();        
//===================================================
 
bool Init() {
    //pc.baud(PC_BAUDRATE);
//    pc.printf("Init.\n");

    seq = 0;
    //while(!pc.readable());
    //        pc.getc();
    
        nh.initNode();
    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.setXGyroOffsetUser(offset.gx);
    mpu.setYGyroOffsetUser(offset.gy);
    mpu.setZGyroOffsetUser(offset.gz);
    mpu.setDMPEnabled(true);    // Enable DMP
    packetSize = mpu.dmpGetFIFOPacketSize();
    dmpReady = true;    // Enable interrupt.
    
    //pc.printf("Init finish!\n");
 
    return true;
}
//=======================     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 setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
{
    unsigned char sendData[8];
    int16_t tmpCRC;
    //char MotorAddress;
    char Function;
    //int DataAddress,Data;
    char DataAddressH,DataAddressL;
    char dataH,dataL;
    int i;
    sendData[6] = 0xff;
    sendData[7] = 0xff;
    
    //MotorAddress = address;
    Function = 0x06;
    //DataAddress = data;
    //Data = 0x0001;
    DataAddressH = ((DataAddress>>8)&0xFF);
    DataAddressL = ((DataAddress>>0)&0xFF);
    dataH = ((Data>>8)&0xFF);
    dataL = ((Data>>0)&0xFF);
    
    sendData[0] = MotorAddress;
    sendData[1] = Function;
    sendData[2] = DataAddressH;
    sendData[3] = DataAddressL;
    sendData[4] = dataH;
    sendData[5] = dataL;
    tmpCRC = CRC_Verify(sendData, 6);
    sendData[6] = (tmpCRC & 0xFF);
    sendData[7] = (tmpCRC>>8);
    RS485_E = 1;
    wait_ms(10);
    for (i=0;i<8;i++)
    {
        RS485.printf("%c",sendData[i]);
    }
    wait_ms(10);
    RS485_E = 0;
    //===========================================
}   
void SVON(char MotorStation)
{
    //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
    setAddress(MotorStation,0x0214,0x0100);
} 

void SVOFF(char MotorStation)
{
    //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
    setAddress(MotorStation,0x0214,0x0101);
    wait_ms(10);
} 
int myabs( int a ){
    if ( a < 0 ){
        return -a;
        }
        return a;
        }
void TwistToMotors()
{
    seq = seq + 1;
    //int lower_bound = 100;
    //int upper_bound = 300;
    float right,left;
    float motor_rpm_r, motor_rpm_l;
    //double vel_data[2];
    int16_t Lrpm,Rrpm;
    float vel_data[2];
    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_r = right*rate/(Dimeter/2)*60/(2*3.1416);
    motor_rpm_l =  left*rate/(Dimeter/2)*60/(2*3.1416);
    vel_data[0] = motor_rpm_r;
    vel_data[1] = motor_rpm_l;  

    Lrpm = motor_rpm_l;
    Rrpm = motor_rpm_r;

    setAddress(Motor1,0x0112,Lrpm);
    setAddress(Motor2,0x0112,Rrpm);
    
    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 );
    //}
    //pc.printf("Wr = %.1f\n",vel_data[0]);
    //pc.printf("Wl = %.1f\n",vel_data[1]);
    ticks_since_target += 1;
}
        

void messageCb(const geometry_msgs::Twist &msg)
{  
    ticks_since_target = 0;
    dx = msg.linear.x;
    dy = msg.linear.y;
    dr = msg.angular.z;
    TwistToMotors(); 
}
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");
    }
}

int main() {
    RS485.baud(PC_BAUDRATE);
    MBED_ASSERT(Init() == true);
    
    nh.initNode();
    nh.advertise(imu_pub);
//    nh.advertise(pub_lmotor);
//    nh.advertise(pub_rmotor);
    nh.subscribe(cmd_vel_sub);
    SVON(1);
    SVON(2);
    while(1) 
    { 
        seq++;
        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 );
        nh.spinOnce();
        wait_ms(10);
        //writing current accelerometer and gyro position 
        //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz);    
    }
}