![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
start to work from here...
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_0411 by
main.cpp
- Committer:
- WeberYang
- Date:
- 2018-05-23
- Revision:
- 9:e10bd4b460d7
- Parent:
- 8:4974fc24fbd7
- Child:
- 10:8b7fce3bba86
File content as of revision 9:e10bd4b460d7:
/* 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 #define IDLE 0 #define ACT_MG_ON 1 #define ACT_MG_OFF 2 #define Check_BMS_ON 3 #define Check_BMS_OFF 4 //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_5); DigitalOut DO_1(PC_6); DigitalOut DO_2(PC_8); DigitalOut DO_3(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 ACT_state; ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state); //====================================================================== //====================================================================== std_msgs::Int16 Error_state; ros::Publisher Error_state_pub("Error_state_pub", &Error_state); //====================================================================== uint32_t seq; //========define ACT_state return code============================================ #define Satndby 0x00 #define Busy 0x01 #define Sensor_error 0x10 #define BMS_error 0x11 //======================================================== #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; float motor_rpm_r, motor_rpm_l; float timeout_ticks; int counter; double w; double rate; double Dimeter; float dx,dy,dr; int lastsensorState = 1; int sensorState; int db_conter = 0; int buffer[9] = {0}; int dataH,datanum; int motor_seq,motor_old_seq; int state_code; int error_code; //=========RS485 char recChar=0; bool recFlag=false; char recArr[20]; int index=0; int BMS_state; 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+600, -350+300, 1000, -110-100, 5, 0};//{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] = 0x00;}else{sendData[7] = 0x01;} if (Lrpm>0){sendData[8] = 0x01;}else{sendData[8] = 0x00;} 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() { float right,left; // double vel_data[2]; float vel_data[2]; float motor_rpm_vx, motor_rpm_theta; motor_old_seq = motor_seq; 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_vx = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416); if((motor_rpm_vx !=0) && (myabs(motor_rpm_vx)<100)){ if(motor_rpm_vx >0){ motor_rpm_vx = 100; } else{ motor_rpm_vx = -100; } } motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416); motor_rpm_r = motor_rpm_vx+ motor_rpm_theta; motor_rpm_l = motor_rpm_vx- motor_rpm_theta; if (myabs(motor_rpm_r)<100|| myabs(motor_rpm_l)<100){ if( dx==0){ if(dr>0){ motor_rpm_r=100; motor_rpm_l=-100; }else if (dr<0){ motor_rpm_r=-100; motor_rpm_l=100; }else{ motor_rpm_r=0; motor_rpm_l=0; } } else if(dx>0){ if (myabs(motor_rpm_r)<100){ motor_rpm_r =100; } if (myabs(motor_rpm_l)<100){ motor_rpm_l =100; } } else{ if(myabs(motor_rpm_r)<100){ motor_rpm_r =-100; } if(myabs(motor_rpm_l)<100){ motor_rpm_l =-100; } } } vel_data[0] = motor_rpm_r; vel_data[1] = motor_rpm_l; //=================================================================== //Sendmessage(vel_data[0],vel_data[1]); //Sendmessage(motor_rpm_l,motor_rpm_r); 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 update_state(int code1,int code2){ } //====================================================================== std_msgs::Int16 DO; //DO_0 MAG_1 //DO_1,MAG_2 //DO_2,UPS //DO_3,BMS int State; void DO_ACT(const std_msgs::Int16 &msg){ //0xFF for action procedure if (msg.data == 0x21){ error_code = 99; State = ACT_MG_ON; } if (msg.data == 0x20){ error_code = 99; State = ACT_MG_OFF; } if (msg.data<0x20){ if (msg.data & 0x01){ DO_0 = 1; } else{ DO_0 = 0; } if (msg.data & 0x02){ DO_1 = 1; } else{ DO_1 = 0; } if (msg.data & 0x04){ DO_2 = 1; } else{ DO_2 = 0; } if (msg.data & 0x08){ DO_3 = 1; } else{ DO_3 = 0; } } } ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT); //====================================================================== //====================================================================================== void messageCb(const geometry_msgs::Twist &msg) { // RS232.printf("messageCb"); ticks_since_target = 0; dx = msg.linear.x; dy = msg.linear.y; dr = msg.angular.z; // RS232.printf("dx = %d,dy = %d,dr = %d\r\n",dx,dy,dr); 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.advertise(ACT_state_pub); nh.advertise(Error_state_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.setXAccelOffsetTC(offset.ax); // mpu.setYAccelOffsetTC(offset.ay); // mpu.setZAccelOffset(offset.az); mpu.setXAccelOffset(1000); //2000 -3134 mpu.setYAccelOffset(0); mpu.setZAccelOffset(0); mpu.setXGyroOffset(800);//offset.gx); mpu.setYGyroOffset(0);//offset.gy); mpu.setZGyroOffset(0);//offset.gz); mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);//2000); mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); mpu.setDMPEnabled(true); // Enable DMP packetSize = mpu.dmpGetFIFOPacketSize(); dmpReady = true; // Enable interrupt. */ //pc.printf("Init finish!\n"); return true; } int MG_ACT(int state) { // int ret; if (state == 1){ //MAG_ON if (sensorState == 1){ //Battery on-position //Lock triggrt DO_1 = 1; wait_ms(100); DO_1 = 0; DO_2 = 1; wait(3); DO_2 = 0; return 1; } } else if (state == 2){//MAG_OFF //unLock triggrt DO_2 = 1; wait_ms(100); DO_2 = 0; DO_2 = 1; wait(3); DO_2 = 0; return 1; } return 0; } //======================================================= 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); DO_2 = 1; //let UPS ON while(1){ seq++; motor_seq = seq; t.start(); //================================ //========define ACT_state return code============================================ //#define Satndby 0x00 //#define Busy 0x01 //#define Sensor_error 0x10 //======================================================== counter++; state_code = State; switch (State){ int ret; case IDLE: counter = 0; break; case ACT_MG_ON: ret = MG_ACT(1); if (ret){ State = Check_BMS_ON; counter = 0; } if (counter>10){ State = IDLE; error_code = Sensor_error; } break; case Check_BMS_ON: if (BMS_state == 1){ State = IDLE; error_code = 0; } if (counter>10){ State = IDLE; error_code = BMS_error; } break; case ACT_MG_OFF: ret = MG_ACT(2); if (ret){ State = Check_BMS_OFF; counter = 0; } if (counter>10){ State = IDLE; error_code = Sensor_error; } break; case Check_BMS_OFF: if (BMS_state == 0){ State = IDLE; error_code = 0; } if (counter>10){ State = IDLE; error_code = BMS_error; } break; } ACT_state.data = state_code; ACT_state_pub.publish( &ACT_state); Error_state.data = error_code; Error_state_pub.publish( &Error_state); //============DI================== int reading = DI_0; if (reading == lastsensorState) { db_conter = db_conter+1; } else{ db_conter = 0; } if (db_conter > 3) { sensorState = reading; DI.data = sensorState; } lastsensorState = reading; DI_pub.publish( &DI); //========================================= if (can1.read(rxMsg) && (t.read_ms() > 5000)) { if(rxMsg.id == CAN_DATA){ BMS_state = 1; 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]; 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 else{ BMS_state = 0; } if (motor_seq > motor_old_seq + 5){ motor_rpm_r = 0; motor_rpm_l = 0; } Sendmessage(motor_rpm_l,motor_rpm_r); wait_ms(100); nh.spinOnce(); } }