![](/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
Diff: main.cpp
- Revision:
- 6:eadfb1b45bda
- Parent:
- 5:52fb31c1a8c0
- Child:
- 7:1a234f02746f
--- a/main.cpp Tue Apr 17 07:59:09 2018 +0000 +++ b/main.cpp Thu May 10 01:39:34 2018 +0000 @@ -3,10 +3,10 @@ */ #include "MPU6050_6Axis_MotionApps20.h" #include "mbed.h" - -//#include "CANMsg.h" +#include "CAN.h" #include "I2Cdev.h" #include "MPU6050.h" + #include <ros.h> #include <ros/time.h> #include <std_msgs/Empty.h> @@ -36,10 +36,19 @@ #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); -Serial rs485(PA_9, PA_10); +Timer t; +Serial RS232(PA_9, PA_10); DigitalOut Receiver(D7); //RS485_E -DigitalOut myled(LED1); +DigitalOut CAN_T(D14); +DigitalOut CAN_R(D15); +//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 @@ -48,12 +57,13 @@ std_msgs::String str_msg; std_msgs::Float32 VelAngular_L; std_msgs::Float32 VelAngular_R; +sensor_msgs::BatteryState BTState; 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); - +ros::Publisher BT_pub("BatteryState", &BTState); uint32_t seq; #define IMU_FIFO_RATE_DIVIDER 0x09 @@ -61,7 +71,7 @@ #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 -#define PC_BAUDRATE 38400 +#define PC_BAUDRATE 115200//38400 #define DEG_TO_RAD(x) ( x * 0.01745329 ) #define RAD_TO_DEG(x) ( x * 57.29578 ) @@ -94,6 +104,13 @@ 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; @@ -112,15 +129,8 @@ bool Init(); void dmpDataUpdate(); unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen); -void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data); -void ReadAddress(char MotorAddress,int16_t DataAddress); -void SVON(char MotorStation); -void ReadENC(char MotorStation); -void SVOFF(char MotorStation); int myabs( int a ); void TwistToMotors(); -void flushSerialBuffer(); -void readData(); //=================================================== @@ -149,96 +159,133 @@ } return wCrc; } - -void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data) +void Sendmessage(float Rrpm,float Lrpm) { - unsigned char sendData[16]; -// int16_t tmpCRC; -// char DataAddressH,DataAddressL; -// char dataH,dataL; + //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; - Receiver = 1; - wait_ms(1); - sendData[0] = MotorAddress; - sendData[1] = func; - sendData[2] = ((DataAddress>>8)&0xFF); - sendData[3] = ((DataAddress>>0)&0xFF); - sendData[4] = ((Data>>8)&0xFF); - sendData[5] = ((Data>>0)&0xFF); - sendData[6] = sendData[0]+sendData[1]+sendData[2]+sendData[3]+sendData[4]+sendData[5]; - sendData[6] = (~sendData[6]+1); - rs485.printf(":"); - for (i=0;i<7;i++) + for (i=0;i<16;i++) { - rs485.printf("%02X",sendData[i]); + RS232.printf("%c",sendData[i]); } - wait_ms(1); - Receiver = 0; - wait_ms(10); //give 10ms let receive complete. -} - -void SVON(char MotorStation) + RS232.printf("\r\n"); +} +void TwistToMotors() { - //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData) - setAddress(MotorStation,Write,DI1,0x0001); -} + 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; -void SVOFF(char MotorStation) -{ - //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData) - setAddress(MotorStation,Write,DI1,0x0101); - wait_ms(10); -} -void ReadENC(char MotorStation) -{ - //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t readDataNum) - setAddress(MotorStation,Read,APR,0x0001); - 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,Write,0x0112,Lrpm); -// setAddress(Motor2,Write,0x0112,Rrpm); - - ticks_since_target += 1; -} int str2int(const char* str, int star, int end) { @@ -250,7 +297,7 @@ } return ret; } - +//====================================================================================== void messageCb(const geometry_msgs::Twist &msg) { ticks_since_target = 0; @@ -262,7 +309,6 @@ } ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb); //====================================================================================== - // void dmpDataUpdate() { // Check that this interrupt has enabled. if (dmpReady == false) return; @@ -339,59 +385,6 @@ // pc.printf("\n"); } } - -//================rs485======================================= -void flushSerialBuffer() { - - while (rs485.readable()) { - rs485.getc(); - } -} -void readData() -{ - while (rs485.readable()) - { - recChar = rs485.getc(); - recArr[index] = recChar; - index++; - if ((recChar == '\r') || (index>18)) - { - datanum = str2int(recArr,1,2); - if (datanum == 1) - { - dataH = str2int(recArr,7,10); - //pc.printf("M1 = %d\r\n",dataH); - VelAngular_R.data = dataH; - } - else - { -// pc.printf("M1 = no data\r\n"); - VelAngular_R.data = 0; - } - - if (datanum == 2) - { - dataH = str2int(recArr,7,10); - VelAngular_L.data = dataH; - } - else - { -// pc.printf("M2 = no data\r\n"); - VelAngular_L.data = 0; - } - - pub_rmotor.publish( &VelAngular_R); - pub_lmotor.publish( &VelAngular_L); - - recArr[index] = 0; - index = 0; - //pc.printf("%s\r\n", recArr); - - flushSerialBuffer(); - recFlag = true; - } - } -} //======================================================= bool Init() { seq = 0; @@ -399,31 +392,32 @@ nh.advertise(imu_pub); nh.advertise(pub_lmotor); nh.advertise(pub_rmotor); + nh.advertise(BT_pub); nh.subscribe(cmd_vel_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.setXGyroOffsetUser(offset.gx); - mpu.setYGyroOffsetUser(offset.gy); - mpu.setZGyroOffsetUser(offset.gz); - mpu.setDMPEnabled(true); // Enable DMP - packetSize = mpu.dmpGetFIFOPacketSize(); - dmpReady = true; // Enable interrupt. + //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"); @@ -431,34 +425,61 @@ } //======================================================= int main() { - rs485.baud(PC_BAUDRATE); + RS232.baud(PC_BAUDRATE); MBED_ASSERT(Init() == true); - - SVON(1); - SVON(2); - flushSerialBuffer(); - rs485.attach(&readData);//,0.25); - while(1) - { + CANMessage rxMsg; + 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++; - mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + 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 ); + // 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 ); + 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(); - setAddress(Motor1,Read,APR,0x0001); - setAddress(Motor2,Read,APR,0x0001); - setAddress(Motor1,Write,SP1,Lrpm); - setAddress(Motor2,Write,SP1,Rrpm); - wait_ms(10); - + wait_ms(20); } } \ No newline at end of file