main.cpp

00001 /*
00002 
00003 */
00004 #include "MPU6050_6Axis_MotionApps20.h"
00005 #include "mbed.h"
00006 //#include "CANMsg.h"
00007 #include "I2Cdev.h"
00008 #include "MPU6050.h"
00009 #include <ros.h>
00010 #include <ros/time.h>
00011 #include <std_msgs/Empty.h>
00012 #include <std_msgs/String.h>
00013 #include <std_msgs/Float32.h>
00014 #include <sensor_msgs/BatteryState.h>
00015 #include <geometry_msgs/Twist.h> //set buffer larger than 50byte
00016 #include <math.h>
00017 #include <stdio.h>
00018 #include <tiny_msgs/tinyVector.h>
00019 #include <tiny_msgs/tinyIMU.h>
00020 
00021 #define Start 0xAA
00022 #define Address 0x7F
00023 #define ReturnType 0x00
00024 #define Clean 0x00
00025 #define Reserve 0x00
00026 #define End 0x55 
00027 #define Motor1 1 
00028 #define Motor2 2 
00029 //Serial              pc(PA_2, PA_3,9600);
00030 Serial              RS485(PA_9, PA_10);
00031 DigitalOut          RS485_E(D7);                     //RS485_E
00032 DigitalOut          myled(LED1);
00033 MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin
00034 
00035 tiny_msgs::tinyIMU imu_msg;
00036 std_msgs::String str_msg;
00037 std_msgs::Float32 VelAngular_L;
00038 std_msgs::Float32 VelAngular_R;
00039 
00040 ros::NodeHandle  nh;
00041 ros::Publisher imu_pub("tinyImu", &imu_msg);
00042 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
00043 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
00044 
00045 uint32_t seq;
00046 
00047 #define IMU_FIFO_RATE_DIVIDER 0x09
00048 #define IMU_SAMPLE_RATE_DIVIDER 4
00049 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
00050 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
00051  
00052 #define PC_BAUDRATE 115200     
00053  
00054 #define DEG_TO_RAD(x) ( x * 0.01745329 )
00055 #define RAD_TO_DEG(x) ( x * 57.29578 )
00056 
00057 const int FIFO_BUFFER_SIZE = 128;
00058 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
00059 uint16_t fifoCount;
00060 uint16_t packetSize;
00061 bool dmpReady;
00062 uint8_t mpuIntStatus;
00063 const int snprintf_buffer_size = 100;
00064 char snprintf_buffer[snprintf_buffer_size];
00065 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
00066 int16_t ax, ay, az;
00067 int16_t gx, gy, gz;
00068   float Lrpm,Rrpm;
00069     float ticks_since_target;
00070     double timeout_ticks;
00071 
00072     double w;
00073     double rate;
00074     double Dimeter;
00075     float dx,dy,dr;
00076 struct Offset {
00077     int16_t ax, ay, az;
00078     int16_t gx, gy, gz;
00079 }offset = {150, -350, 1000, -110, 5, 0};    // Measured values
00080  
00081 struct MPU6050_DmpData {
00082     Quaternion q;
00083     VectorFloat gravity;    // g
00084     float roll, pitch, yaw;     // rad
00085 }dmpData;
00086  
00087 long map(long x, long in_min, long in_max, long out_min, long out_max) {
00088   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00089 }
00090 //==========define sub function========================
00091 bool Init();
00092 void dmpDataUpdate();
00093 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
00094 void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data);
00095 void SVON(char MotorStation);
00096 void SVOFF(char MotorStation);
00097 int myabs( int a );
00098 void TwistToMotors();        
00099 //===================================================
00100  
00101 bool Init() {
00102     //pc.baud(PC_BAUDRATE);
00103 //    pc.printf("Init.\n");
00104 
00105     seq = 0;
00106     //while(!pc.readable());
00107     //        pc.getc();
00108     
00109         nh.initNode();
00110     mpu.initialize();
00111     if (mpu.testConnection()) {
00112 //        pc.printf("MPU6050 test connection passed.\n");
00113     } else {
00114 //        pc.printf("MPU6050 test connection failed.\n");
00115         return false;
00116     }
00117     if (mpu.dmpInitialize() == 0) {
00118 //        pc.printf("succeed in MPU6050 DMP Initializing.\n");
00119     } else {
00120 //        pc.printf("failed in MPU6050 DMP Initializing.\n");
00121         return false;
00122     }
00123     mpu.setXAccelOffset(offset.ax);
00124     mpu.setYAccelOffset(offset.ay);
00125     mpu.setZAccelOffset(offset.az);
00126     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00127     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00128     mpu.setXGyroOffsetUser(offset.gx);
00129     mpu.setYGyroOffsetUser(offset.gy);
00130     mpu.setZGyroOffsetUser(offset.gz);
00131     mpu.setDMPEnabled(true);    // Enable DMP
00132     packetSize = mpu.dmpGetFIFOPacketSize();
00133     dmpReady = true;    // Enable interrupt.
00134     
00135     //pc.printf("Init finish!\n");
00136  
00137     return true;
00138 }
00139 //=======================     motor      =================================================
00140 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
00141 {
00142     unsigned int i, j;
00143     //#define wPolynom 0xA001
00144     unsigned int wCrc = 0xffff;
00145     unsigned int wPolynom = 0xA001;
00146     /*---------------------------------------------------------------------------------*/
00147     for (i = 0; i < iBufLen; i++)
00148     {
00149         wCrc ^= cBuffer[i];
00150         for (j = 0; j < 8; j++)
00151         {
00152             if (wCrc &0x0001)
00153             {
00154                 wCrc = (wCrc >> 1) ^ wPolynom;
00155             }
00156             else
00157             { 
00158                 wCrc = wCrc >> 1;
00159             }
00160         }
00161     }
00162     return wCrc;
00163 }
00164    
00165 void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
00166 {
00167     unsigned char sendData[8];
00168     int16_t tmpCRC;
00169     //char MotorAddress;
00170     char Function;
00171     //int DataAddress,Data;
00172     char DataAddressH,DataAddressL;
00173     char dataH,dataL;
00174     int i;
00175     sendData[6] = 0xff;
00176     sendData[7] = 0xff;
00177     
00178     //MotorAddress = address;
00179     Function = 0x06;
00180     //DataAddress = data;
00181     //Data = 0x0001;
00182     DataAddressH = ((DataAddress>>8)&0xFF);
00183     DataAddressL = ((DataAddress>>0)&0xFF);
00184     dataH = ((Data>>8)&0xFF);
00185     dataL = ((Data>>0)&0xFF);
00186     
00187     sendData[0] = MotorAddress;
00188     sendData[1] = Function;
00189     sendData[2] = DataAddressH;
00190     sendData[3] = DataAddressL;
00191     sendData[4] = dataH;
00192     sendData[5] = dataL;
00193     tmpCRC = CRC_Verify(sendData, 6);
00194     sendData[6] = (tmpCRC & 0xFF);
00195     sendData[7] = (tmpCRC>>8);
00196     RS485_E = 1;
00197     wait_ms(10);
00198     for (i=0;i<8;i++)
00199     {
00200         RS485.printf("%c",sendData[i]);
00201     }
00202     wait_ms(10);
00203     RS485_E = 0;
00204     //===========================================
00205 }   
00206 void SVON(char MotorStation)
00207 {
00208     //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
00209     setAddress(MotorStation,0x0214,0x0100);
00210 } 
00211 
00212 void SVOFF(char MotorStation)
00213 {
00214     //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
00215     setAddress(MotorStation,0x0214,0x0101);
00216     wait_ms(10);
00217 } 
00218 int myabs( int a ){
00219     if ( a < 0 ){
00220         return -a;
00221         }
00222         return a;
00223         }
00224 void TwistToMotors()
00225 {
00226     seq = seq + 1;
00227     //int lower_bound = 100;
00228     //int upper_bound = 300;
00229     float right,left;
00230     float motor_rpm_r, motor_rpm_l;
00231     //double vel_data[2];
00232     int16_t Lrpm,Rrpm;
00233     float vel_data[2];
00234     w = 0.302;//0.2 ;//m
00235     rate = 20;//50;
00236     timeout_ticks = 2;
00237     Dimeter = 0.127;//0.15;
00238 
00239     // prevent agv receive weird 1.0 command from cmd_vel
00240     if (dr == 1.0){
00241         
00242         dr = 0.001;
00243         }
00244         
00245         //
00246     right = ( 1.0 * dx ) + (dr * w /2);
00247     left = ( 1.0 * dx ) - (dr * w /2);
00248     motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
00249     motor_rpm_l =  left*rate/(Dimeter/2)*60/(2*3.1416);
00250     vel_data[0] = motor_rpm_r;
00251     vel_data[1] = motor_rpm_l;  
00252 
00253     Lrpm = motor_rpm_l;
00254     Rrpm = motor_rpm_r;
00255 
00256     setAddress(Motor1,0x0112,Lrpm);
00257     setAddress(Motor2,0x0112,Rrpm);
00258     
00259     VelAngular_R.data = vel_data[0];
00260     VelAngular_L.data = vel_data[1];
00261     //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
00262     //}
00263     //else{
00264 //    pub_rmotor.publish( &VelAngular_R );
00265 //    pub_lmotor.publish( &VelAngular_L );
00266     //}
00267     //pc.printf("Wr = %.1f\n",vel_data[0]);
00268     //pc.printf("Wl = %.1f\n",vel_data[1]);
00269     ticks_since_target += 1;
00270 }
00271         
00272 
00273 void messageCb(const geometry_msgs::Twist &msg)
00274 {  
00275     ticks_since_target = 0;
00276     dx = msg.linear.x;
00277     dy = msg.linear.y;
00278     dr = msg.angular.z;
00279     TwistToMotors(); 
00280 }
00281 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
00282  //======================================================================================
00283  //
00284 void dmpDataUpdate() {
00285     // Check that this interrupt has enabled.
00286     if (dmpReady == false) return;
00287     
00288     mpuIntStatus = mpu.getIntStatus();
00289     fifoCount = mpu.getFIFOCount();
00290     
00291     // Check that this interrupt is a FIFO buffer overflow interrupt.
00292     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00293         mpu.resetFIFO();
00294         //pc.printf("FIFO overflow!\n");
00295         return;
00296         
00297     // Check that this interrupt is a Data Ready interrupt.
00298     } else if (mpuIntStatus & 0x02) {
00299         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00300         
00301         mpu.getFIFOBytes(fifoBuffer, packetSize);
00302         
00303     #ifdef OUTPUT_QUATERNION
00304         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00305         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;
00306         pc.printf(snprintf_buffer);
00307     #endif
00308         
00309     #ifdef OUTPUT_EULER
00310         float euler[3];
00311         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00312         mpu.dmpGetEuler(euler, &dmpData.q);
00313         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;
00314         pc.printf(snprintf_buffer);
00315     #endif
00316         
00317     #ifdef OUTPUT_ROLL_PITCH_YAW
00318         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00319         mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
00320         float rollPitchYaw[3];
00321         mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
00322         dmpData.roll = rollPitchYaw[2];
00323         dmpData.pitch = rollPitchYaw[1];
00324         dmpData.yaw = rollPitchYaw[0];
00325         
00326         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;
00327         pc.printf(snprintf_buffer);
00328                  
00329 #ifdef servotest
00330         int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
00331         if(servoPulse > 1450) servoPulse = 1450;
00332         if(servoPulse < 500) servoPulse = 500;
00333         sv.pulsewidth_us(servoPulse);
00334 #endif
00335     #endif
00336         
00337     #ifdef OUTPUT_FOR_TEAPOT
00338         teapotPacket[2] = fifoBuffer[0];
00339         teapotPacket[3] = fifoBuffer[1];
00340         teapotPacket[4] = fifoBuffer[4];        
00341         teapotPacket[5] = fifoBuffer[5];
00342         teapotPacket[6] = fifoBuffer[8];
00343         teapotPacket[7] = fifoBuffer[9];
00344         teapotPacket[8] = fifoBuffer[12];
00345         teapotPacket[9] = fifoBuffer[13];
00346         for (uint8_t i = 0; i < 14; i++) {
00347             pc.putc(teapotPacket[i]);
00348         }
00349     #endif
00350         
00351     #ifdef OUTPUT_TEMPERATURE
00352         float temp = mpu.getTemperature() / 340.0 + 36.53;
00353         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
00354         pc.printf(snprintf_buffer);
00355     #endif
00356         
00357 //        pc.printf("\n");
00358     }
00359 }
00360 
00361 int main() {
00362     RS485.baud(PC_BAUDRATE);
00363     MBED_ASSERT(Init() == true);
00364     
00365     nh.initNode();
00366     nh.advertise(imu_pub);
00367 //    nh.advertise(pub_lmotor);
00368 //    nh.advertise(pub_rmotor);
00369     nh.subscribe(cmd_vel_sub);
00370     SVON(1);
00371     SVON(2);
00372     while(1) 
00373     { 
00374         seq++;
00375         mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00376 
00377         imu_msg.header.stamp = nh.now();
00378         imu_msg.header.frame_id = 0;
00379         imu_msg.header.seq = seq;
00380         imu_msg.accel.x = ax;
00381         imu_msg.accel.y = ay;
00382         imu_msg.accel.z = az;
00383         imu_msg.gyro.x = gx;
00384         imu_msg.gyro.y = gy;
00385         imu_msg.gyro.z = gz;       
00386         imu_pub.publish( &imu_msg );
00387         nh.spinOnce();
00388         wait_ms(10);
00389         //writing current accelerometer and gyro position 
00390         //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz);    
00391     }
00392 }