start to work from here...
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_0411 by
main.cpp
- Committer:
- WeberYang
- Date:
- 2018-04-12
- Revision:
- 2:648583d6e41a
- Parent:
- 0:6e61e8ec4b42
- Child:
- 3:51194773aa7e
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); } }