main.cpp

00001 /*
00002 0412 combine the sevro motor encoder to publish
00003 0523 test fork
00004 */
00005 #include "MPU6050_6Axis_MotionApps20.h"
00006 #include "mbed.h"
00007 #include "CAN.h"
00008 #include "I2Cdev.h"
00009 #include "MPU6050.h"
00010 
00011 #include <ros.h>
00012 #include <ros/time.h>
00013 #include <std_msgs/Int16.h>
00014 #include <std_msgs/String.h>
00015 #include <std_msgs/Float32.h>
00016 #include <sensor_msgs/BatteryState.h>
00017 #include <geometry_msgs/Twist.h> //set buffer larger than 50byte
00018 #include <math.h>
00019 #include <stdio.h>
00020 #include <tiny_msgs/tinyVector.h>
00021 #include <tiny_msgs/tinyIMU.h>
00022 #include <string> 
00023 #include <cstdlib>
00024 
00025 #define Start 0xAA
00026 #define Address 0x7F
00027 #define ReturnType 0x00
00028 #define Clean 0x00
00029 #define Reserve 0x00
00030 #define End 0x55 
00031 #define Motor1 1 
00032 #define Motor2 2 
00033 #define LENG 31   //0x42 + 31 bytes equal to 32 bytes
00034 
00035 #define Write 0x06
00036 #define Read 0x03
00037 #define DI1 0x0214      //0214H means digital input DI1 for sevro on
00038 #define APR 0x0066      //0066H means encoder abs rev
00039 #define SP1 0x0112
00040 
00041 #define CAN_DATA  0x470
00042 #define CAN_STATUS  0x471
00043 
00044 //Serial pc(USBTX,USBRX);
00045 Timer t;
00046 Serial              RS232(PA_9, PA_10);
00047 DigitalOut          Receiver(D7);                     //RS485_E
00048 DigitalOut          CAN_T(D14);
00049 DigitalOut          CAN_R(D15);
00050 DigitalOut          DO_0(PC_8);
00051 DigitalOut          DO_1(PC_9);
00052 DigitalIn           DI_0(PB_13);
00053 
00054 //CAN                 can1(PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
00055 //CANMsg              rxMsg;
00056 //CANMessage          rxMsg;
00057 Ticker CheckDataR;
00058 
00059 MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin
00060 
00061 ros::NodeHandle  nh;
00062 //======================================================================
00063 tiny_msgs::tinyIMU imu_msg;
00064 ros::Publisher imu_pub("tinyImu", &imu_msg);
00065 //======================================================================
00066 
00067 //======================================================================
00068 std_msgs::Float32 VelAngular_L;
00069 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
00070 //======================================================================
00071 
00072 //======================================================================
00073 std_msgs::Float32 VelAngular_R;
00074 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
00075 //======================================================================
00076 
00077 //======================================================================
00078 sensor_msgs::BatteryState BTState;
00079 ros::Publisher BT_pub("BatteryState", &BTState);
00080 //======================================================================
00081 
00082 //======================================================================
00083 std_msgs::Int16 DI;
00084 ros::Publisher DI_pub("DI_pub", &DI);
00085 //======================================================================
00086 
00087 //======================================================================
00088 std_msgs::Int16 DO;
00089 
00090 void DO_ACT(const std_msgs::Int16 &msg){
00091     if (msg.data & 0x01){
00092         DO_0 = 1;
00093     }
00094     else{
00095         DO_0 = 0;
00096     }
00097 
00098     if (msg.data & 0x02){
00099         DO_1 = 1;
00100     }
00101     else{
00102         DO_1 = 0;
00103     }    
00104 }
00105 ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
00106 //======================================================================
00107 uint32_t seq;
00108 
00109 #define IMU_FIFO_RATE_DIVIDER 0x09
00110 #define IMU_SAMPLE_RATE_DIVIDER 4
00111 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
00112 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
00113  
00114 #define PC_BAUDRATE 115200//38400     
00115  
00116 #define DEG_TO_RAD(x) ( x * 0.01745329 )
00117 #define RAD_TO_DEG(x) ( x * 57.29578 )
00118 
00119 const int FIFO_BUFFER_SIZE = 128;
00120 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
00121 uint16_t fifoCount;
00122 uint16_t packetSize;
00123 bool dmpReady;
00124 uint8_t mpuIntStatus;
00125 const int snprintf_buffer_size = 100;
00126 char snprintf_buffer[snprintf_buffer_size];
00127 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
00128 int16_t ax, ay, az;
00129 int16_t gx, gy, gz;
00130 float Lrpm,Rrpm;
00131 float ticks_since_target;
00132 double timeout_ticks;
00133 
00134     double w;
00135     double rate;
00136     double Dimeter;
00137     float dx,dy,dr;
00138 int lastButtonState = 1;    
00139 int buttonState;     
00140 int db_conter = 0;
00141 int buffer[9] = {0};
00142 int dataH,datanum;
00143 //=========RS485
00144 char recChar=0;
00145 bool recFlag=false;
00146 char recArr[20];
00147 int index=0;
00148 
00149         uint32_t SOC;
00150         uint32_t Tempert;
00151         uint32_t RackVoltage = 0;
00152         uint32_t Current = 0;
00153         uint32_t MaxCellV = 0;
00154         uint32_t MinCellV = 0;
00155 
00156 struct Offset {
00157     int16_t ax, ay, az;
00158     int16_t gx, gy, gz;
00159 }offset = {150, -350, 1000, -110, 5, 0};    // Measured values
00160  
00161 struct MPU6050_DmpData {
00162     Quaternion q;
00163     VectorFloat gravity;    // g
00164     float roll, pitch, yaw;     // rad
00165 }dmpData;
00166  
00167 long map(long x, long in_min, long in_max, long out_min, long out_max) {
00168   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00169 }
00170 //==========define sub function========================
00171 bool Init();
00172 void dmpDataUpdate();
00173 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
00174 int myabs( int a );
00175 void TwistToMotors();   
00176 //===================================================
00177  
00178 
00179 //=======================     motor      =================================================
00180 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
00181 {
00182     unsigned int i, j;
00183     //#define wPolynom 0xA001
00184     unsigned int wCrc = 0xffff;
00185     unsigned int wPolynom = 0xA001;
00186     /*---------------------------------------------------------------------------------*/
00187     for (i = 0; i < iBufLen; i++)
00188     {
00189         wCrc ^= cBuffer[i];
00190         for (j = 0; j < 8; j++)
00191         {
00192             if (wCrc &0x0001)
00193             {
00194                 wCrc = (wCrc >> 1) ^ wPolynom;
00195             }
00196             else
00197             { 
00198                 wCrc = wCrc >> 1;
00199             }
00200         }
00201     }
00202     return wCrc;
00203 }
00204 void Sendmessage(float Rrpm,float Lrpm)
00205 {
00206     //RS232.printf("Wr = %.1f\n",Rrpm);
00207     //RS232.printf("Wl = %.1f\n",Lrpm);
00208 unsigned char sendData[16];
00209 unsigned int tmpCRC;
00210 int motor1,motor2;
00211    
00212     sendData[0] = Start;
00213     sendData[1] = Address;
00214     sendData[2] = ReturnType;
00215     sendData[3] = Clean;
00216     sendData[4] = Reserve;
00217     sendData[5]  = 0x01;//motor1Sevro ON
00218     sendData[6] = 0x01;//motor2Sevro ON
00219 if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
00220 if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
00221    motor1 =  abs(Rrpm);
00222    motor2 =  abs(Lrpm); 
00223     
00224     sendData[9] = (motor1>>8);//motor1speedH
00225     sendData[10] = (motor1 & 0xFF);//motor1speedL
00226     sendData[11] = (motor2>>8);//motor2speedH
00227     sendData[12] = (motor2 & 0xFF);//motor2speedL
00228     sendData[13] = End;
00229     tmpCRC = CRC_Verify(sendData, 14);
00230     sendData[14] = (tmpCRC & 0xFF);
00231     sendData[15] = (tmpCRC>>8);
00232     int i;
00233     for (i=0;i<16;i++)
00234     {
00235         RS232.printf("%c",sendData[i]);
00236     }
00237     RS232.printf("\r\n");
00238 }
00239 void TwistToMotors()
00240 {
00241     w = 0.302;//0.2 ;//m
00242     rate = 20;//50;
00243     timeout_ticks = 2;
00244     Dimeter = 0.127;//0.15;
00245     float right,left;
00246     float motor_rpm_r, motor_rpm_l;
00247     double vel_data[2];
00248     right = ( 1.0 * dx ) + (dr * w /2);
00249 //   left = ( 1.0 * dx ) - (dr * w /2);
00250 //    vel_data[0] = right*rate/Dimeter/60*1000;
00251 //    vel_data[1] =  left*rate/Dimeter/60*1000;
00252    if (dx!=0)
00253     {
00254      if (dx>0)
00255      { 
00256        if (dr >=0)
00257        {
00258            motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00259            motor_rpm_l=300;
00260        }
00261        else
00262        {
00263            motor_rpm_r=300;
00264            motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00265        }
00266      }
00267      else
00268      {
00269          if(dr>=0)
00270          {
00271           motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00272           motor_rpm_l=(-300);
00273          }
00274          else
00275          {
00276              motor_rpm_r=(-300);
00277              motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
00278          }
00279      }
00280     }
00281     else
00282     {
00283         if(dr>=0)
00284         {
00285             motor_rpm_r=100;
00286             motor_rpm_l=-100;
00287         }
00288         else
00289         {
00290             motor_rpm_r=-100;
00291             motor_rpm_l=100;
00292         }
00293     }
00294     vel_data[0]=motor_rpm_r;
00295     vel_data[1]=motor_rpm_l; 
00296     //================================================================ Original Version
00297     /*if (dr>=0)
00298     {
00299         right = ( 1.0 * dx ) + (dr * w /2);
00300         left = ( 1.0 * dx );
00301     }
00302     else
00303     {
00304         right = ( 1.0 * dx );
00305         left = ( 1.0 * dx ) - (dr * w /2);
00306     }
00307         vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60;
00308         vel_data[1] =  left*rate/(Dimeter/2)/(2*3.1416)*60;*/
00309 //===================================================================
00310     Sendmessage(vel_data[0],vel_data[1]);
00311     VelAngular_R.data = vel_data[0];
00312     VelAngular_L.data = vel_data[1];
00313     //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
00314     //}
00315     //else{
00316     pub_rmotor.publish( &VelAngular_R );
00317     pub_lmotor.publish( &VelAngular_L );
00318     //}
00319     //RS232.printf("Wr = %.1f\n",vel_data[0]);
00320     //RS232.printf("Wl = %.1f\n",vel_data[1]);
00321     ticks_since_target += 1;
00322 
00323 }
00324 
00325 int myabs( int a ){
00326     if ( a < 0 ){
00327         return -a;
00328         }
00329         return a;
00330         }
00331 
00332 int str2int(const char* str, int star, int end)
00333 {
00334     int i;
00335     int ret = 0;
00336     for (i = star; i < end+1; i++)
00337     {
00338         ret = ret *10 + (str[i] - '0');
00339     }
00340     return ret;     
00341 }       
00342 //======================================================================================
00343 void messageCb(const geometry_msgs::Twist &msg)
00344 {  
00345     ticks_since_target = 0;
00346     dx = msg.linear.x;
00347     dy = msg.linear.y;
00348     dr = msg.angular.z;
00349     TwistToMotors(); 
00350     //ReadENC(Motor1);
00351 }
00352 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
00353  //======================================================================================
00354 void dmpDataUpdate() {
00355     // Check that this interrupt has enabled.
00356     if (dmpReady == false) return;
00357     
00358     mpuIntStatus = mpu.getIntStatus();
00359     fifoCount = mpu.getFIFOCount();
00360     
00361     // Check that this interrupt is a FIFO buffer overflow interrupt.
00362     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00363         mpu.resetFIFO();
00364         //pc.printf("FIFO overflow!\n");
00365         return;
00366         
00367     // Check that this interrupt is a Data Ready interrupt.
00368     } else if (mpuIntStatus & 0x02) {
00369         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00370         
00371         mpu.getFIFOBytes(fifoBuffer, packetSize);
00372         
00373     #ifdef OUTPUT_QUATERNION
00374         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00375         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;
00376         pc.printf(snprintf_buffer);
00377     #endif
00378         
00379     #ifdef OUTPUT_EULER
00380         float euler[3];
00381         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00382         mpu.dmpGetEuler(euler, &dmpData.q);
00383         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;
00384         pc.printf(snprintf_buffer);
00385     #endif
00386         
00387     #ifdef OUTPUT_ROLL_PITCH_YAW
00388         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00389         mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
00390         float rollPitchYaw[3];
00391         mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
00392         dmpData.roll = rollPitchYaw[2];
00393         dmpData.pitch = rollPitchYaw[1];
00394         dmpData.yaw = rollPitchYaw[0];
00395         
00396         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;
00397         pc.printf(snprintf_buffer);
00398                  
00399 #ifdef servotest
00400         int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
00401         if(servoPulse > 1450) servoPulse = 1450;
00402         if(servoPulse < 500) servoPulse = 500;
00403         sv.pulsewidth_us(servoPulse);
00404 #endif
00405     #endif
00406         
00407     #ifdef OUTPUT_FOR_TEAPOT
00408         teapotPacket[2] = fifoBuffer[0];
00409         teapotPacket[3] = fifoBuffer[1];
00410         teapotPacket[4] = fifoBuffer[4];        
00411         teapotPacket[5] = fifoBuffer[5];
00412         teapotPacket[6] = fifoBuffer[8];
00413         teapotPacket[7] = fifoBuffer[9];
00414         teapotPacket[8] = fifoBuffer[12];
00415         teapotPacket[9] = fifoBuffer[13];
00416         for (uint8_t i = 0; i < 14; i++) {
00417             pc.putc(teapotPacket[i]);
00418         }
00419     #endif
00420         
00421     #ifdef OUTPUT_TEMPERATURE
00422         float temp = mpu.getTemperature() / 340.0 + 36.53;
00423         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
00424         pc.printf(snprintf_buffer);
00425     #endif
00426         
00427 //        pc.printf("\n");
00428     }
00429 }
00430 //=======================================================
00431 bool Init() {
00432     DO_0 = 0;
00433     DO_1 = 0;
00434     
00435     seq = 0;
00436     nh.initNode();
00437     nh.advertise(imu_pub);
00438     nh.advertise(pub_lmotor);
00439     nh.advertise(pub_rmotor);
00440     nh.advertise(BT_pub);
00441     nh.advertise(DI_pub);
00442     nh.subscribe(cmd_vel_sub);
00443     nh.subscribe(ACT_sub);
00444     
00445     mpu.initialize();
00446     if (mpu.testConnection()) {
00447 //        pc.printf("MPU6050 test connection passed.\n");
00448     } else {
00449 //        pc.printf("MPU6050 test connection failed.\n");
00450         return false;
00451     }
00452     if (mpu.dmpInitialize() == 0) {
00453 //        pc.printf("succeed in MPU6050 DMP Initializing.\n");
00454     } else {
00455 //        pc.printf("failed in MPU6050 DMP Initializing.\n");
00456         return false;
00457     }
00458     mpu.setXAccelOffset(offset.ax);
00459     mpu.setYAccelOffset(offset.ay);
00460     mpu.setZAccelOffset(offset.az);
00461     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00462     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00463     mpu.setXGyroOffset(offset.gx);
00464     mpu.setYGyroOffset(offset.gy);
00465     mpu.setZGyroOffset(offset.gz);
00466     mpu.setDMPEnabled(true);    // Enable DMP
00467     packetSize = mpu.dmpGetFIFOPacketSize();
00468     dmpReady = true;    // Enable interrupt.
00469     
00470     //pc.printf("Init finish!\n");
00471  
00472     return true;
00473 }
00474 //=======================================================
00475 int main() {
00476     RS232.baud(PC_BAUDRATE);
00477     MBED_ASSERT(Init() == true);
00478     CANMessage rxMsg;
00479     DI_0.mode(PullUp);
00480     CAN_T = 0;
00481     CAN_R = 0;
00482     wait_ms(50);
00483     CAN     can1(D15,D14);//PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
00484     wait_ms(50);
00485     can1.frequency(500000);
00486     wait_ms(50);
00487     while(1){   
00488         seq++;
00489         t.start();
00490         mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00491 
00492         imu_msg.header.stamp = nh.now();
00493         imu_msg.header.frame_id = 0;
00494         imu_msg.header.seq = seq;
00495         imu_msg.accel.x = ax;
00496         imu_msg.accel.y = ay;
00497         imu_msg.accel.z = az;
00498         imu_msg.gyro.x = gx;
00499         imu_msg.gyro.y = gy;
00500         imu_msg.gyro.z = gz;   
00501 //        
00502         imu_pub.publish( &imu_msg );
00503         //============DI==================
00504         int reading = DI_0;
00505         if (reading == lastButtonState) {
00506             db_conter = db_conter+1;
00507         }
00508         else{
00509             db_conter = 0;
00510         }
00511         
00512         if (db_conter > 3) {
00513 //            if (reading != buttonState) {
00514             buttonState = reading;
00515             DI.data = buttonState; 
00516 //            }
00517         }
00518 
00519         lastButtonState = reading;
00520         DI_pub.publish( &DI);
00521 
00522         //=========================================
00523         if (can1.read(rxMsg) && (t.read_ms() > 5000))  {
00524 //            RS232.printf("  ID      = 0x%.3x\r\n", rxMsg.id);
00525 
00526             if(rxMsg.id == CAN_DATA){
00527                 SOC = rxMsg.data[0]>>1;
00528                 Tempert = rxMsg.data[1]-50;
00529                 RackVoltage  = ((unsigned int)rxMsg.data[3] << 8) + rxMsg.data[2];
00530                 Current      = ((unsigned int)rxMsg.data[5] << 8) + rxMsg.data[4];
00531                 MaxCellV = rxMsg.data[6];
00532                 MinCellV = rxMsg.data[7];
00533 //                RS232.printf("SOC = %d\r\n",SOC);
00534 //                RS232.printf("Tempert = %d\r\n",Tempert);
00535 //                RS232.printf("RackVoltage = %.2f\r\n",RackVoltage*0.1);
00536 //                RS232.printf("Current = %.2f\r\n",Current*0.1);
00537 //                RS232.printf("MaxCellV = %.2f\r\n",MaxCellV*0.02);
00538 //                RS232.printf("MinCellV = %.2f\r\n",MinCellV*0.02);
00539                 
00540                 BTState.header.stamp = nh.now();
00541                 BTState.header.frame_id = 0;
00542                 BTState.header.seq = seq;
00543                 BTState.voltage = RackVoltage*0.1;
00544                 BTState.current =  Current;
00545                 BTState.design_capacity = 80;
00546                 BTState.percentage = SOC;
00547                 BT_pub.publish( &BTState );
00548                 t.reset();   
00549             } // if
00550         } // if
00551         nh.spinOnce();
00552         wait_ms(10);         
00553     }
00554 }