Weber Yang / Mbed 2 deprecated AGV_0411

For nucleoF103 change sda,scl to PB_7,PB_6 Finish Delta Servo control with RS485, and IMU6050

Dependencies:   MPU6050-DMP mbed ros_lib_kinetic

Fork of MPU6050_DMP_test_for1549 by takaaki mastuzawa

main.cpp@2:648583d6e41a, 2018-04-12 (annotated)

Committer:
WeberYang
Date:
Thu Apr 12 01:15:57 2018 +0000
Revision:
2:648583d6e41a
Parent:
0:6e61e8ec4b42 
beta1.0 finish Delta servo motor control and IMU-6050

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WeberYang 2:648583d6e41a 1 /*
WeberYang 2:648583d6e41a 2
WeberYang 2:648583d6e41a 3 */
hardtail 0:6e61e8ec4b42 4 #include "MPU6050_6Axis_MotionApps20.h"
hardtail 0:6e61e8ec4b42 5 #include "mbed.h"
WeberYang 2:648583d6e41a 6 //#include "CANMsg.h"
WeberYang 2:648583d6e41a 7 #include "I2Cdev.h"
WeberYang 2:648583d6e41a 8 #include "MPU6050.h"
WeberYang 2:648583d6e41a 9 #include <ros.h>
WeberYang 2:648583d6e41a 10 #include <ros/time.h>
WeberYang 2:648583d6e41a 11 #include <std_msgs/Empty.h>
WeberYang 2:648583d6e41a 12 #include <std_msgs/String.h>
WeberYang 2:648583d6e41a 13 #include <std_msgs/Float32.h>
WeberYang 2:648583d6e41a 14 #include <sensor_msgs/BatteryState.h>
WeberYang 2:648583d6e41a 15 #include <geometry_msgs/Twist.h> //set buffer larger than 50byte
WeberYang 2:648583d6e41a 16 #include <math.h>
hardtail 0:6e61e8ec4b42 17 #include <stdio.h>
WeberYang 2:648583d6e41a 18 #include <tiny_msgs/tinyVector.h>
WeberYang 2:648583d6e41a 19 #include <tiny_msgs/tinyIMU.h>
hardtail 0:6e61e8ec4b42 20
WeberYang 2:648583d6e41a 21 #define Start 0xAA
WeberYang 2:648583d6e41a 22 #define Address 0x7F
WeberYang 2:648583d6e41a 23 #define ReturnType 0x00
WeberYang 2:648583d6e41a 24 #define Clean 0x00
WeberYang 2:648583d6e41a 25 #define Reserve 0x00
WeberYang 2:648583d6e41a 26 #define End 0x55
WeberYang 2:648583d6e41a 27 #define Motor1 1
WeberYang 2:648583d6e41a 28 #define Motor2 2
WeberYang 2:648583d6e41a 29 //Serial pc(PA_2, PA_3,9600);
WeberYang 2:648583d6e41a 30 Serial RS485(PA_9, PA_10);
WeberYang 2:648583d6e41a 31 DigitalOut RS485_E(D7); //RS485_E
WeberYang 2:648583d6e41a 32 DigitalOut myled(LED1);
WeberYang 2:648583d6e41a 33 MPU6050 mpu;//(PB_7,PB_6); // sda, scl pin
hardtail 0:6e61e8ec4b42 34
WeberYang 2:648583d6e41a 35 tiny_msgs::tinyIMU imu_msg;
WeberYang 2:648583d6e41a 36 std_msgs::String str_msg;
WeberYang 2:648583d6e41a 37 std_msgs::Float32 VelAngular_L;
WeberYang 2:648583d6e41a 38 std_msgs::Float32 VelAngular_R;
WeberYang 2:648583d6e41a 39
WeberYang 2:648583d6e41a 40 ros::NodeHandle nh;
WeberYang 2:648583d6e41a 41 ros::Publisher imu_pub("tinyImu", &imu_msg);
WeberYang 2:648583d6e41a 42 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
WeberYang 2:648583d6e41a 43 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
WeberYang 2:648583d6e41a 44
WeberYang 2:648583d6e41a 45 uint32_t seq;
WeberYang 2:648583d6e41a 46
hardtail 0:6e61e8ec4b42 47 #define IMU_FIFO_RATE_DIVIDER 0x09
hardtail 0:6e61e8ec4b42 48 #define IMU_SAMPLE_RATE_DIVIDER 4
hardtail 0:6e61e8ec4b42 49 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
hardtail 0:6e61e8ec4b42 50 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
hardtail 0:6e61e8ec4b42 51
WeberYang 2:648583d6e41a 52 #define PC_BAUDRATE 115200
hardtail 0:6e61e8ec4b42 53
hardtail 0:6e61e8ec4b42 54 #define DEG_TO_RAD(x) ( x * 0.01745329 )
hardtail 0:6e61e8ec4b42 55 #define RAD_TO_DEG(x) ( x * 57.29578 )
WeberYang 2:648583d6e41a 56
hardtail 0:6e61e8ec4b42 57 const int FIFO_BUFFER_SIZE = 128;
hardtail 0:6e61e8ec4b42 58 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
hardtail 0:6e61e8ec4b42 59 uint16_t fifoCount;
hardtail 0:6e61e8ec4b42 60 uint16_t packetSize;
hardtail 0:6e61e8ec4b42 61 bool dmpReady;
hardtail 0:6e61e8ec4b42 62 uint8_t mpuIntStatus;
hardtail 0:6e61e8ec4b42 63 const int snprintf_buffer_size = 100;
hardtail 0:6e61e8ec4b42 64 char snprintf_buffer[snprintf_buffer_size];
hardtail 0:6e61e8ec4b42 65 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
WeberYang 2:648583d6e41a 66 int16_t ax, ay, az;
WeberYang 2:648583d6e41a 67 int16_t gx, gy, gz;
WeberYang 2:648583d6e41a 68 float Lrpm,Rrpm;
WeberYang 2:648583d6e41a 69 float ticks_since_target;
WeberYang 2:648583d6e41a 70 double timeout_ticks;
WeberYang 2:648583d6e41a 71
WeberYang 2:648583d6e41a 72 double w;
WeberYang 2:648583d6e41a 73 double rate;
WeberYang 2:648583d6e41a 74 double Dimeter;
WeberYang 2:648583d6e41a 75 float dx,dy,dr;
hardtail 0:6e61e8ec4b42 76 struct Offset {
hardtail 0:6e61e8ec4b42 77 int16_t ax, ay, az;
hardtail 0:6e61e8ec4b42 78 int16_t gx, gy, gz;
hardtail 0:6e61e8ec4b42 79 }offset = {150, -350, 1000, -110, 5, 0}; // Measured values
hardtail 0:6e61e8ec4b42 80
hardtail 0:6e61e8ec4b42 81 struct MPU6050_DmpData {
hardtail 0:6e61e8ec4b42 82 Quaternion q;
hardtail 0:6e61e8ec4b42 83 VectorFloat gravity; // g
hardtail 0:6e61e8ec4b42 84 float roll, pitch, yaw; // rad
hardtail 0:6e61e8ec4b42 85 }dmpData;
hardtail 0:6e61e8ec4b42 86
hardtail 0:6e61e8ec4b42 87 long map(long x, long in_min, long in_max, long out_min, long out_max) {
hardtail 0:6e61e8ec4b42 88 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
hardtail 0:6e61e8ec4b42 89 }
WeberYang 2:648583d6e41a 90 //==========define sub function========================
hardtail 0:6e61e8ec4b42 91 bool Init();
hardtail 0:6e61e8ec4b42 92 void dmpDataUpdate();
WeberYang 2:648583d6e41a 93 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
WeberYang 2:648583d6e41a 94 void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data);
WeberYang 2:648583d6e41a 95 void SVON(char MotorStation);
WeberYang 2:648583d6e41a 96 void SVOFF(char MotorStation);
WeberYang 2:648583d6e41a 97 int myabs( int a );
WeberYang 2:648583d6e41a 98 void TwistToMotors();
WeberYang 2:648583d6e41a 99 //===================================================
hardtail 0:6e61e8ec4b42 100
hardtail 0:6e61e8ec4b42 101 bool Init() {
hardtail 0:6e61e8ec4b42 102 //pc.baud(PC_BAUDRATE);
WeberYang 2:648583d6e41a 103 // pc.printf("Init.\n");
WeberYang 2:648583d6e41a 104
WeberYang 2:648583d6e41a 105 seq = 0;
WeberYang 2:648583d6e41a 106 //while(!pc.readable());
WeberYang 2:648583d6e41a 107 // pc.getc();
hardtail 0:6e61e8ec4b42 108
WeberYang 2:648583d6e41a 109 nh.initNode();
hardtail 0:6e61e8ec4b42 110 mpu.initialize();
hardtail 0:6e61e8ec4b42 111 if (mpu.testConnection()) {
WeberYang 2:648583d6e41a 112 // pc.printf("MPU6050 test connection passed.\n");
hardtail 0:6e61e8ec4b42 113 } else {
WeberYang 2:648583d6e41a 114 // pc.printf("MPU6050 test connection failed.\n");
hardtail 0:6e61e8ec4b42 115 return false;
hardtail 0:6e61e8ec4b42 116 }
hardtail 0:6e61e8ec4b42 117 if (mpu.dmpInitialize() == 0) {
WeberYang 2:648583d6e41a 118 // pc.printf("succeed in MPU6050 DMP Initializing.\n");
hardtail 0:6e61e8ec4b42 119 } else {
WeberYang 2:648583d6e41a 120 // pc.printf("failed in MPU6050 DMP Initializing.\n");
hardtail 0:6e61e8ec4b42 121 return false;
hardtail 0:6e61e8ec4b42 122 }
hardtail 0:6e61e8ec4b42 123 mpu.setXAccelOffset(offset.ax);
hardtail 0:6e61e8ec4b42 124 mpu.setYAccelOffset(offset.ay);
hardtail 0:6e61e8ec4b42 125 mpu.setZAccelOffset(offset.az);
hardtail 0:6e61e8ec4b42 126 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
hardtail 0:6e61e8ec4b42 127 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
hardtail 0:6e61e8ec4b42 128 mpu.setXGyroOffsetUser(offset.gx);
hardtail 0:6e61e8ec4b42 129 mpu.setYGyroOffsetUser(offset.gy);
hardtail 0:6e61e8ec4b42 130 mpu.setZGyroOffsetUser(offset.gz);
hardtail 0:6e61e8ec4b42 131 mpu.setDMPEnabled(true); // Enable DMP
hardtail 0:6e61e8ec4b42 132 packetSize = mpu.dmpGetFIFOPacketSize();
hardtail 0:6e61e8ec4b42 133 dmpReady = true; // Enable interrupt.
hardtail 0:6e61e8ec4b42 134
WeberYang 2:648583d6e41a 135 //pc.printf("Init finish!\n");
hardtail 0:6e61e8ec4b42 136
hardtail 0:6e61e8ec4b42 137 return true;
hardtail 0:6e61e8ec4b42 138 }
WeberYang 2:648583d6e41a 139 //======================= motor =================================================
WeberYang 2:648583d6e41a 140 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
WeberYang 2:648583d6e41a 141 {
WeberYang 2:648583d6e41a 142 unsigned int i, j;
WeberYang 2:648583d6e41a 143 //#define wPolynom 0xA001
WeberYang 2:648583d6e41a 144 unsigned int wCrc = 0xffff;
WeberYang 2:648583d6e41a 145 unsigned int wPolynom = 0xA001;
WeberYang 2:648583d6e41a 146 /*---------------------------------------------------------------------------------*/
WeberYang 2:648583d6e41a 147 for (i = 0; i < iBufLen; i++)
WeberYang 2:648583d6e41a 148 {
WeberYang 2:648583d6e41a 149 wCrc ^= cBuffer[i];
WeberYang 2:648583d6e41a 150 for (j = 0; j < 8; j++)
WeberYang 2:648583d6e41a 151 {
WeberYang 2:648583d6e41a 152 if (wCrc &0x0001)
WeberYang 2:648583d6e41a 153 {
WeberYang 2:648583d6e41a 154 wCrc = (wCrc >> 1) ^ wPolynom;
WeberYang 2:648583d6e41a 155 }
WeberYang 2:648583d6e41a 156 else
WeberYang 2:648583d6e41a 157 {
WeberYang 2:648583d6e41a 158 wCrc = wCrc >> 1;
WeberYang 2:648583d6e41a 159 }
WeberYang 2:648583d6e41a 160 }
WeberYang 2:648583d6e41a 161 }
WeberYang 2:648583d6e41a 162 return wCrc;
WeberYang 2:648583d6e41a 163 }
WeberYang 2:648583d6e41a 164
WeberYang 2:648583d6e41a 165 void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
WeberYang 2:648583d6e41a 166 {
WeberYang 2:648583d6e41a 167 unsigned char sendData[8];
WeberYang 2:648583d6e41a 168 int16_t tmpCRC;
WeberYang 2:648583d6e41a 169 //char MotorAddress;
WeberYang 2:648583d6e41a 170 char Function;
WeberYang 2:648583d6e41a 171 //int DataAddress,Data;
WeberYang 2:648583d6e41a 172 char DataAddressH,DataAddressL;
WeberYang 2:648583d6e41a 173 char dataH,dataL;
WeberYang 2:648583d6e41a 174 int i;
WeberYang 2:648583d6e41a 175 sendData[6] = 0xff;
WeberYang 2:648583d6e41a 176 sendData[7] = 0xff;
WeberYang 2:648583d6e41a 177
WeberYang 2:648583d6e41a 178 //MotorAddress = address;
WeberYang 2:648583d6e41a 179 Function = 0x06;
WeberYang 2:648583d6e41a 180 //DataAddress = data;
WeberYang 2:648583d6e41a 181 //Data = 0x0001;
WeberYang 2:648583d6e41a 182 DataAddressH = ((DataAddress>>8)&0xFF);
WeberYang 2:648583d6e41a 183 DataAddressL = ((DataAddress>>0)&0xFF);
WeberYang 2:648583d6e41a 184 dataH = ((Data>>8)&0xFF);
WeberYang 2:648583d6e41a 185 dataL = ((Data>>0)&0xFF);
WeberYang 2:648583d6e41a 186
WeberYang 2:648583d6e41a 187 sendData[0] = MotorAddress;
WeberYang 2:648583d6e41a 188 sendData[1] = Function;
WeberYang 2:648583d6e41a 189 sendData[2] = DataAddressH;
WeberYang 2:648583d6e41a 190 sendData[3] = DataAddressL;
WeberYang 2:648583d6e41a 191 sendData[4] = dataH;
WeberYang 2:648583d6e41a 192 sendData[5] = dataL;
WeberYang 2:648583d6e41a 193 tmpCRC = CRC_Verify(sendData, 6);
WeberYang 2:648583d6e41a 194 sendData[6] = (tmpCRC & 0xFF);
WeberYang 2:648583d6e41a 195 sendData[7] = (tmpCRC>>8);
WeberYang 2:648583d6e41a 196 RS485_E = 1;
WeberYang 2:648583d6e41a 197 wait_ms(10);
WeberYang 2:648583d6e41a 198 for (i=0;i<8;i++)
WeberYang 2:648583d6e41a 199 {
WeberYang 2:648583d6e41a 200 RS485.printf("%c",sendData[i]);
WeberYang 2:648583d6e41a 201 }
WeberYang 2:648583d6e41a 202 wait_ms(10);
WeberYang 2:648583d6e41a 203 RS485_E = 0;
WeberYang 2:648583d6e41a 204 //===========================================
WeberYang 2:648583d6e41a 205 }
WeberYang 2:648583d6e41a 206 void SVON(char MotorStation)
WeberYang 2:648583d6e41a 207 {
WeberYang 2:648583d6e41a 208 //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
WeberYang 2:648583d6e41a 209 setAddress(MotorStation,0x0214,0x0100);
WeberYang 2:648583d6e41a 210 }
WeberYang 2:648583d6e41a 211
WeberYang 2:648583d6e41a 212 void SVOFF(char MotorStation)
WeberYang 2:648583d6e41a 213 {
WeberYang 2:648583d6e41a 214 //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
WeberYang 2:648583d6e41a 215 setAddress(MotorStation,0x0214,0x0101);
WeberYang 2:648583d6e41a 216 wait_ms(10);
WeberYang 2:648583d6e41a 217 }
WeberYang 2:648583d6e41a 218 int myabs( int a ){
WeberYang 2:648583d6e41a 219 if ( a < 0 ){
WeberYang 2:648583d6e41a 220 return -a;
WeberYang 2:648583d6e41a 221 }
WeberYang 2:648583d6e41a 222 return a;
WeberYang 2:648583d6e41a 223 }
WeberYang 2:648583d6e41a 224 void TwistToMotors()
WeberYang 2:648583d6e41a 225 {
WeberYang 2:648583d6e41a 226 seq = seq + 1;
WeberYang 2:648583d6e41a 227 //int lower_bound = 100;
WeberYang 2:648583d6e41a 228 //int upper_bound = 300;
WeberYang 2:648583d6e41a 229 float right,left;
WeberYang 2:648583d6e41a 230 float motor_rpm_r, motor_rpm_l;
WeberYang 2:648583d6e41a 231 //double vel_data[2];
WeberYang 2:648583d6e41a 232 int16_t Lrpm,Rrpm;
WeberYang 2:648583d6e41a 233 float vel_data[2];
WeberYang 2:648583d6e41a 234 w = 0.302;//0.2 ;//m
WeberYang 2:648583d6e41a 235 rate = 20;//50;
WeberYang 2:648583d6e41a 236 timeout_ticks = 2;
WeberYang 2:648583d6e41a 237 Dimeter = 0.127;//0.15;
WeberYang 2:648583d6e41a 238
WeberYang 2:648583d6e41a 239 // prevent agv receive weird 1.0 command from cmd_vel
WeberYang 2:648583d6e41a 240 if (dr == 1.0){
WeberYang 2:648583d6e41a 241
WeberYang 2:648583d6e41a 242 dr = 0.001;
WeberYang 2:648583d6e41a 243 }
WeberYang 2:648583d6e41a 244
WeberYang 2:648583d6e41a 245 //
WeberYang 2:648583d6e41a 246 right = ( 1.0 * dx ) + (dr * w /2);
WeberYang 2:648583d6e41a 247 left = ( 1.0 * dx ) - (dr * w /2);
WeberYang 2:648583d6e41a 248 motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 2:648583d6e41a 249 motor_rpm_l = left*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 2:648583d6e41a 250 vel_data[0] = motor_rpm_r;
WeberYang 2:648583d6e41a 251 vel_data[1] = motor_rpm_l;
WeberYang 2:648583d6e41a 252
WeberYang 2:648583d6e41a 253 Lrpm = motor_rpm_l;
WeberYang 2:648583d6e41a 254 Rrpm = motor_rpm_r;
WeberYang 2:648583d6e41a 255
WeberYang 2:648583d6e41a 256 setAddress(Motor1,0x0112,Lrpm);
WeberYang 2:648583d6e41a 257 setAddress(Motor2,0x0112,Rrpm);
WeberYang 2:648583d6e41a 258
WeberYang 2:648583d6e41a 259 VelAngular_R.data = vel_data[0];
WeberYang 2:648583d6e41a 260 VelAngular_L.data = vel_data[1];
WeberYang 2:648583d6e41a 261 //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
WeberYang 2:648583d6e41a 262 //}
WeberYang 2:648583d6e41a 263 //else{
WeberYang 2:648583d6e41a 264 // pub_rmotor.publish( &VelAngular_R );
WeberYang 2:648583d6e41a 265 // pub_lmotor.publish( &VelAngular_L );
WeberYang 2:648583d6e41a 266 //}
WeberYang 2:648583d6e41a 267 //pc.printf("Wr = %.1f\n",vel_data[0]);
WeberYang 2:648583d6e41a 268 //pc.printf("Wl = %.1f\n",vel_data[1]);
WeberYang 2:648583d6e41a 269 ticks_since_target += 1;
WeberYang 2:648583d6e41a 270 }
WeberYang 2:648583d6e41a 271
WeberYang 2:648583d6e41a 272
WeberYang 2:648583d6e41a 273 void messageCb(const geometry_msgs::Twist &msg)
WeberYang 2:648583d6e41a 274 {
WeberYang 2:648583d6e41a 275 ticks_since_target = 0;
WeberYang 2:648583d6e41a 276 dx = msg.linear.x;
WeberYang 2:648583d6e41a 277 dy = msg.linear.y;
WeberYang 2:648583d6e41a 278 dr = msg.angular.z;
WeberYang 2:648583d6e41a 279 TwistToMotors();
WeberYang 2:648583d6e41a 280 }
WeberYang 2:648583d6e41a 281 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
WeberYang 2:648583d6e41a 282 //======================================================================================
WeberYang 2:648583d6e41a 283 //
hardtail 0:6e61e8ec4b42 284 void dmpDataUpdate() {
hardtail 0:6e61e8ec4b42 285 // Check that this interrupt has enabled.
hardtail 0:6e61e8ec4b42 286 if (dmpReady == false) return;
hardtail 0:6e61e8ec4b42 287
hardtail 0:6e61e8ec4b42 288 mpuIntStatus = mpu.getIntStatus();
hardtail 0:6e61e8ec4b42 289 fifoCount = mpu.getFIFOCount();
hardtail 0:6e61e8ec4b42 290
hardtail 0:6e61e8ec4b42 291 // Check that this interrupt is a FIFO buffer overflow interrupt.
hardtail 0:6e61e8ec4b42 292 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
hardtail 0:6e61e8ec4b42 293 mpu.resetFIFO();
WeberYang 2:648583d6e41a 294 //pc.printf("FIFO overflow!\n");
hardtail 0:6e61e8ec4b42 295 return;
hardtail 0:6e61e8ec4b42 296
hardtail 0:6e61e8ec4b42 297 // Check that this interrupt is a Data Ready interrupt.
hardtail 0:6e61e8ec4b42 298 } else if (mpuIntStatus & 0x02) {
hardtail 0:6e61e8ec4b42 299 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
hardtail 0:6e61e8ec4b42 300
hardtail 0:6e61e8ec4b42 301 mpu.getFIFOBytes(fifoBuffer, packetSize);
hardtail 0:6e61e8ec4b42 302
hardtail 0:6e61e8ec4b42 303 #ifdef OUTPUT_QUATERNION
hardtail 0:6e61e8ec4b42 304 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 305 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;
WeberYang 2:648583d6e41a 306 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 307 #endif
hardtail 0:6e61e8ec4b42 308
hardtail 0:6e61e8ec4b42 309 #ifdef OUTPUT_EULER
hardtail 0:6e61e8ec4b42 310 float euler[3];
hardtail 0:6e61e8ec4b42 311 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 312 mpu.dmpGetEuler(euler, &dmpData.q);
hardtail 0:6e61e8ec4b42 313 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;
WeberYang 2:648583d6e41a 314 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 315 #endif
hardtail 0:6e61e8ec4b42 316
hardtail 0:6e61e8ec4b42 317 #ifdef OUTPUT_ROLL_PITCH_YAW
hardtail 0:6e61e8ec4b42 318 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 319 mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
hardtail 0:6e61e8ec4b42 320 float rollPitchYaw[3];
hardtail 0:6e61e8ec4b42 321 mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
hardtail 0:6e61e8ec4b42 322 dmpData.roll = rollPitchYaw[2];
hardtail 0:6e61e8ec4b42 323 dmpData.pitch = rollPitchYaw[1];
hardtail 0:6e61e8ec4b42 324 dmpData.yaw = rollPitchYaw[0];
hardtail 0:6e61e8ec4b42 325
hardtail 0:6e61e8ec4b42 326 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;
WeberYang 2:648583d6e41a 327 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 328
hardtail 0:6e61e8ec4b42 329 #ifdef servotest
hardtail 0:6e61e8ec4b42 330 int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
hardtail 0:6e61e8ec4b42 331 if(servoPulse > 1450) servoPulse = 1450;
hardtail 0:6e61e8ec4b42 332 if(servoPulse < 500) servoPulse = 500;
hardtail 0:6e61e8ec4b42 333 sv.pulsewidth_us(servoPulse);
hardtail 0:6e61e8ec4b42 334 #endif
hardtail 0:6e61e8ec4b42 335 #endif
hardtail 0:6e61e8ec4b42 336
hardtail 0:6e61e8ec4b42 337 #ifdef OUTPUT_FOR_TEAPOT
hardtail 0:6e61e8ec4b42 338 teapotPacket[2] = fifoBuffer[0];
hardtail 0:6e61e8ec4b42 339 teapotPacket[3] = fifoBuffer[1];
hardtail 0:6e61e8ec4b42 340 teapotPacket[4] = fifoBuffer[4];
hardtail 0:6e61e8ec4b42 341 teapotPacket[5] = fifoBuffer[5];
hardtail 0:6e61e8ec4b42 342 teapotPacket[6] = fifoBuffer[8];
hardtail 0:6e61e8ec4b42 343 teapotPacket[7] = fifoBuffer[9];
hardtail 0:6e61e8ec4b42 344 teapotPacket[8] = fifoBuffer[12];
hardtail 0:6e61e8ec4b42 345 teapotPacket[9] = fifoBuffer[13];
hardtail 0:6e61e8ec4b42 346 for (uint8_t i = 0; i < 14; i++) {
hardtail 0:6e61e8ec4b42 347 pc.putc(teapotPacket[i]);
hardtail 0:6e61e8ec4b42 348 }
hardtail 0:6e61e8ec4b42 349 #endif
hardtail 0:6e61e8ec4b42 350
hardtail 0:6e61e8ec4b42 351 #ifdef OUTPUT_TEMPERATURE
hardtail 0:6e61e8ec4b42 352 float temp = mpu.getTemperature() / 340.0 + 36.53;
hardtail 0:6e61e8ec4b42 353 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
WeberYang 2:648583d6e41a 354 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 355 #endif
hardtail 0:6e61e8ec4b42 356
WeberYang 2:648583d6e41a 357 // pc.printf("\n");
WeberYang 2:648583d6e41a 358 }
WeberYang 2:648583d6e41a 359 }
WeberYang 2:648583d6e41a 360
WeberYang 2:648583d6e41a 361 int main() {
WeberYang 2:648583d6e41a 362 RS485.baud(PC_BAUDRATE);
WeberYang 2:648583d6e41a 363 MBED_ASSERT(Init() == true);
WeberYang 2:648583d6e41a 364
WeberYang 2:648583d6e41a 365 nh.initNode();
WeberYang 2:648583d6e41a 366 nh.advertise(imu_pub);
WeberYang 2:648583d6e41a 367 // nh.advertise(pub_lmotor);
WeberYang 2:648583d6e41a 368 // nh.advertise(pub_rmotor);
WeberYang 2:648583d6e41a 369 nh.subscribe(cmd_vel_sub);
WeberYang 2:648583d6e41a 370 SVON(1);
WeberYang 2:648583d6e41a 371 SVON(2);
WeberYang 2:648583d6e41a 372 while(1)
WeberYang 2:648583d6e41a 373 {
WeberYang 2:648583d6e41a 374 seq++;
WeberYang 2:648583d6e41a 375 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
WeberYang 2:648583d6e41a 376
WeberYang 2:648583d6e41a 377 imu_msg.header.stamp = nh.now();
WeberYang 2:648583d6e41a 378 imu_msg.header.frame_id = 0;
WeberYang 2:648583d6e41a 379 imu_msg.header.seq = seq;
WeberYang 2:648583d6e41a 380 imu_msg.accel.x = ax;
WeberYang 2:648583d6e41a 381 imu_msg.accel.y = ay;
WeberYang 2:648583d6e41a 382 imu_msg.accel.z = az;
WeberYang 2:648583d6e41a 383 imu_msg.gyro.x = gx;
WeberYang 2:648583d6e41a 384 imu_msg.gyro.y = gy;
WeberYang 2:648583d6e41a 385 imu_msg.gyro.z = gz;
WeberYang 2:648583d6e41a 386 imu_pub.publish( &imu_msg );
WeberYang 2:648583d6e41a 387 nh.spinOnce();
WeberYang 2:648583d6e41a 388 wait_ms(10);
WeberYang 2:648583d6e41a 389 //writing current accelerometer and gyro position
WeberYang 2:648583d6e41a 390 //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz);
hardtail 0:6e61e8ec4b42 391 }
hardtail 0:6e61e8ec4b42 392 }