![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
Diff: main.cpp
- Revision:
- 2:648583d6e41a
- Parent:
- 0:6e61e8ec4b42
--- a/main.cpp Wed Oct 25 08:17:05 2017 +0000 +++ b/main.cpp Thu Apr 12 01:15:57 2018 +0000 @@ -1,64 +1,59 @@ -/* Demo code for MPU6050 DMP - * I thank Ian Hua. - * Copyright (c) 2015 Match - * - * THE PROGRAM IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE PROGRAM OR THE USE OR OTHER DEALINGS IN - * THE PROGRAM. - */ - - -// Define Necessary. -//#define OUTPUT_QUATERNION -//#define OUTPUT_EULER -#define OUTPUT_ROLL_PITCH_YAW -//#define OUTPUT_FOR_TEAPOT -//#define OUTPUT_TEMPERATURE - +/* + +*/ #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 servotest -#ifdef servotest -PwmOut sv(P0_0); -#endif +#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 -// FIFO rate = 200Hz / (1 + this value) -// For example, 0x01 is 100Hz, 0x03 is 50Hz. -// 0x00 to 0x09 +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 - -// Sample rate = 1kHz / (1 + this valye) -// For example, 4 is 200Hz. #define IMU_SAMPLE_RATE_DIVIDER 4 - -// measuring range of gyroscope (±n deg/s) -// But other value doesn't yet support. #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 - -// measuring range of acceleration sensor (±n g) -// But other value doesn't yet support. #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 -#define PC_BAUDRATE 921600 - - -#include "USBSerial.h" +#define PC_BAUDRATE 115200 #define DEG_TO_RAD(x) ( x * 0.01745329 ) #define RAD_TO_DEG(x) ( x * 57.29578 ) - - -//RawSerial pc(USBTX, USBRX); -USBSerial pc; -MPU6050 mpu; // sda, scl pin -InterruptIn INT0(P0_9); // INT0 pin - + const int FIFO_BUFFER_SIZE = 128; uint8_t fifoBuffer[FIFO_BUFFER_SIZE]; uint16_t fifoCount; @@ -68,7 +63,16 @@ 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; @@ -83,40 +87,37 @@ 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(); - - -int main() { - MBED_ASSERT(Init() == true); - - while(1) { - - } -} - +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); - - while(!pc.readable()); - pc.getc(); +// pc.printf("Init.\n"); + + seq = 0; + //while(!pc.readable()); + // pc.getc(); - INT0.mode(PullDown); - INT0.fall(dmpDataUpdate); - + nh.initNode(); mpu.initialize(); if (mpu.testConnection()) { - pc.puts("MPU6050 test connection passed.\n"); +// pc.printf("MPU6050 test connection passed.\n"); } else { - pc.puts("MPU6050 test connection failed.\n"); +// pc.printf("MPU6050 test connection failed.\n"); return false; } if (mpu.dmpInitialize() == 0) { - pc.puts("succeed in MPU6050 DMP Initializing.\n"); +// pc.printf("succeed in MPU6050 DMP Initializing.\n"); } else { - pc.puts("failed in MPU6050 DMP Initializing.\n"); +// pc.printf("failed in MPU6050 DMP Initializing.\n"); return false; } mpu.setXAccelOffset(offset.ax); @@ -131,16 +132,155 @@ packetSize = mpu.dmpGetFIFOPacketSize(); dmpReady = true; // Enable interrupt. - pc.puts("Init finish!\n"); - - #ifdef servotest - sv.pulsewidth_us(1450); - #endif + //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; @@ -151,7 +291,7 @@ // Check that this interrupt is a FIFO buffer overflow interrupt. if ((mpuIntStatus & 0x10) || fifoCount == 1024) { mpu.resetFIFO(); - //pc.puts("FIFO overflow!\n"); + //pc.printf("FIFO overflow!\n"); return; // Check that this interrupt is a Data Ready interrupt. @@ -163,7 +303,7 @@ #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.puts(snprintf_buffer); + pc.printf(snprintf_buffer); #endif #ifdef OUTPUT_EULER @@ -171,7 +311,7 @@ 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.puts(snprintf_buffer); + pc.printf(snprintf_buffer); #endif #ifdef OUTPUT_ROLL_PITCH_YAW @@ -184,7 +324,7 @@ 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.puts(snprintf_buffer); + pc.printf(snprintf_buffer); #ifdef servotest int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450); @@ -211,9 +351,42 @@ #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.puts(snprintf_buffer); + pc.printf(snprintf_buffer); #endif - pc.puts("\n"); +// 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); } } \ No newline at end of file