ELIDRISSI_Ousama

imu_ultrasounds

Nov 4th, 2017
150
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <ros.h>
  2. #include <ros/time.h>
  3. #include <sensor_msgs/Range.h>
  4. #include <geometry_msgs/Quaternion.h>
  5. #include "I2Cdev.h"
  6. #include "MPU6050_6Axis_MotionApps20.h"
  7.  
  8. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  9.     #include "Wire.h"
  10. #endif
  11.  
  12. MPU6050 mpu;
  13.  
  14. long duration;float d;int i=0;
  15. int T[]={23,25,27,29,31,33,35,37,39,41};
  16. int E[]={22,24,26,28,30,32,34,36,38,40};
  17.  
  18. ros::NodeHandle  nh;
  19.  
  20. sensor_msgs::Range range_msg1;
  21. sensor_msgs::Range range_msg2;
  22. sensor_msgs::Range range_msg3;
  23. ros::Publisher pub_range1("/ultrasound1", &range_msg1);
  24. ros::Publisher pub_range2("/ultrasound2", &range_msg2);
  25. ros::Publisher pub_range3("/ultrasound3", &range_msg3);
  26.  
  27. geometry_msgs::Quaternion orientation;
  28. ros::Publisher imu_ori("imu_orientation",&orientation);
  29.  
  30. bool dmpReady = false;  // set true if DMP init was successful
  31. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  32. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  33. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  34. uint16_t fifoCount;     // count of all bytes currently in FIFO
  35. uint8_t fifoBuffer[64]; // FIFO storage buffer
  36.  
  37. Quaternion q;           // [w, x, y, z]         quaternion container
  38. VectorInt16 aa;         // [x, y, z]            accel sensor measurements
  39. VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
  40. VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
  41. VectorFloat gravity;    // [x, y, z]            gravity vector
  42. float euler[3];         // [psi, theta, phi]    Euler angle container
  43. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  44.  
  45. volatile bool mpuInterrupt = false;
  46. void dmpDataReady() {
  47.     mpuInterrupt = true;
  48. }
  49.  
  50. float getRange(int T,int E)
  51. {
  52.   digitalWrite(T,HIGH);
  53.   delayMicroseconds(10);
  54.   digitalWrite(T,LOW);
  55.   duration=pulseIn(E,HIGH);
  56.   d=duration/58;
  57.   delayMicroseconds(5);
  58.   return d/100;
  59. }
  60.  
  61. void setup()
  62. {
  63.   #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  64.         Wire.begin();        
  65.     #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  66.         Fastwire::setup(400, true);
  67.     #endif
  68.   nh.initNode();
  69.   nh.advertise(imu_ori);  
  70.   nh.advertise(pub_range1);
  71.   nh.advertise(pub_range2);
  72.   nh.advertise(pub_range3);
  73.  
  74.   range_msg1.radiation_type = sensor_msgs::Range::ULTRASOUND;
  75.   range_msg1.header.frame_id =  "/ultrasound1";
  76.   range_msg1.field_of_view = 1.570796;
  77.   range_msg1.min_range = 0.02;
  78.   range_msg1.max_range = 4.0;
  79.  
  80.   range_msg2.radiation_type = sensor_msgs::Range::ULTRASOUND;
  81.   range_msg2.header.frame_id =  "/ultrasound2";
  82.   range_msg2.field_of_view = 1.570796;
  83.   range_msg2.min_range = 0.02;
  84.   range_msg2.max_range = 4.0;
  85.  
  86.   range_msg3.radiation_type = sensor_msgs::Range::ULTRASOUND;
  87.   range_msg3.header.frame_id =  "/ultrasound3";
  88.   range_msg3.field_of_view = 1.570796;
  89.   range_msg3.min_range = 0.02;
  90.   range_msg3.max_range = 4.0;
  91.  
  92.   for(i=0;i<3;i++){
  93.     pinMode(T[i], OUTPUT);
  94.     digitalWrite(T[i], LOW);
  95.     pinMode(E[i], INPUT);
  96.   }
  97.  
  98.   mpu.initialize();
  99.   devStatus = mpu.dmpInitialize();    
  100.   if (devStatus == 0) {    
  101.     mpu.setDMPEnabled(true);
  102.     attachInterrupt(0, dmpDataReady, RISING);
  103.     mpuIntStatus = mpu.getIntStatus();
  104.     dmpReady = true;
  105.     packetSize = mpu.dmpGetFIFOPacketSize();
  106.   }
  107. }
  108.  
  109. long range_time;
  110.  
  111. void loop()
  112. {
  113.   if ( millis() >= range_time ){
  114.    
  115.     range_msg1.range = getRange(T[0],E[0]);
  116.     range_msg1.header.stamp = nh.now();
  117.     pub_range1.publish(&range_msg1);
  118.     nh.spinOnce();
  119.  
  120.     range_msg2.range = getRange(T[1],E[1]);
  121.     range_msg2.header.stamp = nh.now();
  122.     pub_range2.publish(&range_msg2);
  123.     nh.spinOnce();
  124.  
  125.     range_msg3.range = getRange(T[2],E[2]);
  126.     range_msg3.header.stamp = nh.now();
  127.     pub_range3.publish(&range_msg3);
  128.     nh.spinOnce();            
  129.      
  130.     range_time =  millis() + 50;
  131.   }
  132.  
  133.   if (!dmpReady) return;
  134.    
  135.   while (!mpuInterrupt && fifoCount < packetSize) {}
  136.  
  137.   mpuInterrupt = false;
  138.   mpuIntStatus = mpu.getIntStatus();
  139.   fifoCount = mpu.getFIFOCount();
  140.  
  141.   if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  142.   {
  143.     mpu.resetFIFO();
  144.   }
  145.   else if (mpuIntStatus & 0x01)
  146.   {
  147.         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  148.        
  149.         mpu.getFIFOBytes(fifoBuffer, packetSize);
  150.        
  151.         fifoCount -= packetSize;            
  152.            
  153.             // display quaternion values in easy matrix form: w x y z
  154.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  155. //            mpu.dmpGetGravity(&gravity, &q);
  156. //            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  157. //            mpu.dmpGetAccel(&aa, fifoBuffer);
  158. //            mpu.dmpGetGravity(&gravity, &q);
  159. //            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  160. //            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
  161.                        
  162.             orientation.x = q.x;
  163.             orientation.y = q.y;
  164.             orientation.z = q.z;
  165.             orientation.w = q.w;            
  166.             imu_ori.publish(&orientation);
  167.             nh.spinOnce();
  168.    
  169.             delay(100);        
  170.     }
  171.  
  172.   //nh.spinOnce();
  173. }
RAW Paste Data