start to work from here...
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_0411 by
main.cpp
- Committer:
- WeberYang
- Date:
- 2018-05-15
- Revision:
- 8:4974fc24fbd7
- Parent:
- 7:1a234f02746f
- Child:
- 9:e10bd4b460d7
File content as of revision 8:4974fc24fbd7:
/* 0412 combine the sevro motor encoder to publish */ #include "MPU6050_6Axis_MotionApps20.h" #include "mbed.h" #include "CAN.h" #include "I2Cdev.h" #include "MPU6050.h" #include <ros.h> #include <ros/time.h> #include <std_msgs/Int16.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> #include <string> #include <cstdlib> #define Start 0xAA #define Address 0x7F #define ReturnType 0x00 #define Clean 0x00 #define Reserve 0x00 #define End 0x55 #define Motor1 1 #define Motor2 2 #define LENG 31 //0x42 + 31 bytes equal to 32 bytes #define Write 0x06 #define Read 0x03 #define DI1 0x0214 //0214H means digital input DI1 for sevro on #define APR 0x0066 //0066H means encoder abs rev #define SP1 0x0112 #define CAN_DATA 0x470 #define CAN_STATUS 0x471 //Serial pc(USBTX,USBRX); Timer t; Serial RS232(PA_9, PA_10); DigitalOut Receiver(D7); //RS485_E DigitalOut CAN_T(D14); DigitalOut CAN_R(D15); DigitalOut DO_0(PC_8); DigitalOut DO_1(PC_9); DigitalIn DI_0(PB_13); //CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name //CANMsg rxMsg; //CANMessage rxMsg; Ticker CheckDataR; MPU6050 mpu;//(PB_7,PB_6); // sda, scl pin ros::NodeHandle nh; //====================================================================== tiny_msgs::tinyIMU imu_msg; ros::Publisher imu_pub("tinyImu", &imu_msg); //====================================================================== //====================================================================== std_msgs::Float32 VelAngular_L; ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L); //====================================================================== //====================================================================== std_msgs::Float32 VelAngular_R; ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R); //====================================================================== //====================================================================== sensor_msgs::BatteryState BTState; ros::Publisher BT_pub("BatteryState", &BTState); //====================================================================== //====================================================================== std_msgs::Int16 DI; ros::Publisher DI_pub("DI_pub", &DI); //====================================================================== //====================================================================== std_msgs::Int16 DO; void DO_ACT(const std_msgs::Int16 &msg){ if (msg.data & 0x01){ DO_0 = 1; } else{ DO_0 = 0; } if (msg.data & 0x02){ DO_1 = 1; } else{ DO_1 = 0; } } ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT); //====================================================================== 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//38400 #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; int lastButtonState = 1; int buttonState; int db_conter = 0; int buffer[9] = {0}; int dataH,datanum; //=========RS485 char recChar=0; bool recFlag=false; char recArr[20]; int index=0; uint32_t SOC; uint32_t Tempert; uint32_t RackVoltage = 0; uint32_t Current = 0; uint32_t MaxCellV = 0; uint32_t MinCellV = 0; 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); int myabs( int a ); void TwistToMotors(); //=================================================== //======================= 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 Sendmessage(float Rrpm,float Lrpm) { //RS232.printf("Wr = %.1f\n",Rrpm); //RS232.printf("Wl = %.1f\n",Lrpm); unsigned char sendData[16]; unsigned int tmpCRC; int motor1,motor2; sendData[0] = Start; sendData[1] = Address; sendData[2] = ReturnType; sendData[3] = Clean; sendData[4] = Reserve; sendData[5] = 0x01;//motor1Sevro ON sendData[6] = 0x01;//motor2Sevro ON if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;} if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;} motor1 = abs(Rrpm); motor2 = abs(Lrpm); sendData[9] = (motor1>>8);//motor1speedH sendData[10] = (motor1 & 0xFF);//motor1speedL sendData[11] = (motor2>>8);//motor2speedH sendData[12] = (motor2 & 0xFF);//motor2speedL sendData[13] = End; tmpCRC = CRC_Verify(sendData, 14); sendData[14] = (tmpCRC & 0xFF); sendData[15] = (tmpCRC>>8); int i; for (i=0;i<16;i++) { RS232.printf("%c",sendData[i]); } RS232.printf("\r\n"); } void TwistToMotors() { w = 0.302;//0.2 ;//m rate = 20;//50; timeout_ticks = 2; Dimeter = 0.127;//0.15; float right,left; float motor_rpm_r, motor_rpm_l; double vel_data[2]; right = ( 1.0 * dx ) + (dr * w /2); // left = ( 1.0 * dx ) - (dr * w /2); // vel_data[0] = right*rate/Dimeter/60*1000; // vel_data[1] = left*rate/Dimeter/60*1000; if (dx!=0) { if (dx>0) { if (dr >=0) { motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; motor_rpm_l=300; } else { motor_rpm_r=300; motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; } } else { if(dr>=0) { motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; motor_rpm_l=(-300); } else { motor_rpm_r=(-300); motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; } } } else { if(dr>=0) { motor_rpm_r=100; motor_rpm_l=-100; } else { motor_rpm_r=-100; motor_rpm_l=100; } } vel_data[0]=motor_rpm_r; vel_data[1]=motor_rpm_l; //================================================================ Original Version /*if (dr>=0) { right = ( 1.0 * dx ) + (dr * w /2); left = ( 1.0 * dx ); } else { right = ( 1.0 * dx ); left = ( 1.0 * dx ) - (dr * w /2); } vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60; vel_data[1] = left*rate/(Dimeter/2)/(2*3.1416)*60;*/ //=================================================================== Sendmessage(vel_data[0],vel_data[1]); 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 ); //} //RS232.printf("Wr = %.1f\n",vel_data[0]); //RS232.printf("Wl = %.1f\n",vel_data[1]); ticks_since_target += 1; } int myabs( int a ){ if ( a < 0 ){ return -a; } return a; } int str2int(const char* str, int star, int end) { int i; int ret = 0; for (i = star; i < end+1; i++) { ret = ret *10 + (str[i] - '0'); } return ret; } //====================================================================================== void messageCb(const geometry_msgs::Twist &msg) { ticks_since_target = 0; dx = msg.linear.x; dy = msg.linear.y; dr = msg.angular.z; TwistToMotors(); //ReadENC(Motor1); } 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"); } } //======================================================= bool Init() { DO_0 = 0; DO_1 = 0; seq = 0; nh.initNode(); nh.advertise(imu_pub); nh.advertise(pub_lmotor); nh.advertise(pub_rmotor); nh.advertise(BT_pub); nh.advertise(DI_pub); nh.subscribe(cmd_vel_sub); nh.subscribe(ACT_sub); 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.setXGyroOffset(offset.gx); mpu.setYGyroOffset(offset.gy); mpu.setZGyroOffset(offset.gz); mpu.setDMPEnabled(true); // Enable DMP packetSize = mpu.dmpGetFIFOPacketSize(); dmpReady = true; // Enable interrupt. //pc.printf("Init finish!\n"); return true; } //======================================================= int main() { RS232.baud(PC_BAUDRATE); MBED_ASSERT(Init() == true); CANMessage rxMsg; DI_0.mode(PullUp); CAN_T = 0; CAN_R = 0; wait_ms(50); CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name wait_ms(50); can1.frequency(500000); wait_ms(50); while(1){ seq++; t.start(); 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 ); //============DI================== int reading = DI_0; if (reading == lastButtonState) { db_conter = db_conter+1; } else{ db_conter = 0; } if (db_conter > 3) { // if (reading != buttonState) { buttonState = reading; DI.data = buttonState; // } } lastButtonState = reading; DI_pub.publish( &DI); //========================================= if (can1.read(rxMsg) && (t.read_ms() > 5000)) { // RS232.printf(" ID = 0x%.3x\r\n", rxMsg.id); if(rxMsg.id == CAN_DATA){ SOC = rxMsg.data[0]>>1; Tempert = rxMsg.data[1]-50; RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + rxMsg.data[2]; Current = ((unsigned int)rxMsg.data[5] << 8) + rxMsg.data[4]; MaxCellV = rxMsg.data[6]; MinCellV = rxMsg.data[7]; // RS232.printf("SOC = %d\r\n",SOC); // RS232.printf("Tempert = %d\r\n",Tempert); // RS232.printf("RackVoltage = %.2f\r\n",RackVoltage*0.1); // RS232.printf("Current = %.2f\r\n",Current*0.1); // RS232.printf("MaxCellV = %.2f\r\n",MaxCellV*0.02); // RS232.printf("MinCellV = %.2f\r\n",MinCellV*0.02); BTState.header.stamp = nh.now(); BTState.header.frame_id = 0; BTState.header.seq = seq; BTState.voltage = RackVoltage*0.1; BTState.current = Current; BTState.design_capacity = 80; BTState.percentage = SOC; BT_pub.publish( &BTState ); t.reset(); } // if } // if nh.spinOnce(); wait_ms(10); } }