![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Delta Battery, CAN_bus, DIO P2P, ROS
Dependencies: mbed mbed-rtos ros_lib_kinetic
Revision 12:ed085acbbde5, committed 2019-08-22
- Comitter:
- WeberYang
- Date:
- Thu Aug 22 07:44:05 2019 +0000
- Parent:
- 11:6d5307ceb569
- Commit message:
- Delta Battery Can_bus reader, and DIO P2P, ROS.
Changed in this revision
--- a/MPU6050-DMP.lib Tue Oct 02 00:57:35 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/WeberYang/code/MPU6050-DMP/#473800bd51ac
--- a/main.cpp Tue Oct 02 00:57:35 2018 +0000 +++ b/main.cpp Thu Aug 22 07:44:05 2019 +0000 @@ -1,832 +1,236 @@ /* 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 "cmsis_os.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 <sensor_msgs/LaserEcho.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 -#define WAIT_BAT 5 +#define PC_BAUDRATE 115200 + //Serial pc(USBTX,USBRX); Timer t; Serial RS232(PA_9, PA_10); -DigitalOut Receiver(D7); //RS485_E +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); -DigitalOut DO_4(PA_12); -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); -//====================================================================== +DigitalOut DO_0(PC_3); +DigitalOut DO_1(PC_2); +//DigitalOut DO_2(PF_1); +//DigitalOut DO_3(PF_0); +DigitalOut DO_4(PC_15); +DigitalOut DO_5(PC_14); +DigitalOut DO_6(PC_13); +DigitalOut DO_7(PB_7); +DigitalOut DO_8(PA_14); +DigitalOut DO_9(PA_13); +//DigitalOut DO_10(PF_7); +//DigitalOut DO_11(PF_6); +DigitalOut DO_12(PC_12); +DigitalOut DO_13(PC_10); +DigitalOut DO_14(PD_2); +DigitalOut DO_15(PC_11); +DigitalOut led1(LED1); -//====================================================================== -std_msgs::Float32 VelAngular_L; -ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L); -//====================================================================== +//DigitalIn DI_0(PF_5); +DigitalIn DI_1(PC_4); +DigitalIn DI_2(PB_13); +DigitalIn DI_3(PB_14); +DigitalIn DI_4(PB_15); +DigitalIn DI_5(PB_1); +DigitalIn DI_6(PB_2); +DigitalIn DI_7(PB_11); +DigitalIn DI_8(PB_12); +DigitalIn DI_9(PA_11); +DigitalIn DI_10(PA_12); +//DigitalIn DI_11(PD_8); +DigitalIn DI_12(PC_5); +DigitalIn DI_13(PC_6); +DigitalIn DI_14(PC_8); +DigitalIn DI_15(PC_9); -//====================================================================== -std_msgs::Float32 VelAngular_R; -ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R); -//====================================================================== -//====================================================================== -sensor_msgs::BatteryState BTState; -ros::Publisher BT_pub("BatteryState", &BTState); -//====================================================================== +//const unsigned int RX_ID = 0x792; +int id_count[15]; +long Radial_range[15]; +long Radial_speed[15]; +long Radial_Acc[15]; +long Angle[15]; +long Power[15]; +long tmp_dis; +long dis[7]; +long ldata; +float fdata1,fdata2; +float fRange[15],fAngle[15]; -//====================================================================== -std_msgs::Int16 DI; -ros::Publisher DI_pub("DI_pub", &DI); -//====================================================================== +//=========Battery varable================ +uint32_t SOC; +uint32_t Tempert; +uint32_t RackVoltage = 0; +uint32_t Current = 0; +uint32_t MaxCellV = 0; +uint32_t MinCellV = 0; +uint32_t seq; +//======================================== -//====================================================================== -std_msgs::Int16 ACT_state; -ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state); -//====================================================================== +void CAN_thread(const void *args); +void DIO_thread(const void *args); +//CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name +CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name +//CANMsg rxMsg; +CANMessage rxMsg; +osThreadDef(CAN_thread, osPriorityNormal, DEFAULT_STACK_SIZE); +osThreadDef(DIO_thread, osPriorityNormal, DEFAULT_STACK_SIZE); //====================================================================== -std_msgs::Int16 Error_state; -ros::Publisher Error_state_pub("Error_state_pub", &Error_state); -//====================================================================== - -uint32_t seq; +ros::NodeHandle nh; -//========define ACT_state return code============================================ -#define Satndby 0 -#define Busy 1 -#define Sensor_error 2 -#define BMS_error 3 -//======================================================== -#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 38400 - -#define DEG_TO_RAD(x) ( x * 0.01745329 ) -#define RAD_TO_DEG(x) ( x * 57.29578 ) +std_msgs::Int16 str_msg1; +std_msgs::Int16 str_msg2; +std_msgs::Int16 str_msg3; +std_msgs::Int16 DI_data; +std_msgs::Int16 ACT_state; +std_msgs::Int16 Error_state; +std_msgs::Int16 DO; +sensor_msgs::BatteryState BTState; + +ros::Publisher sonar1("sonar1", &str_msg1); +ros::Publisher sonar2("sonar2", &str_msg2); +ros::Publisher sonar3("sonar3", &str_msg3); +ros::Publisher DI_data_pub("DI_data_pub", &DI_data); +ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state); +ros::Publisher Error_state_pub("Error_state_pub", &Error_state); +ros::Publisher BT_pub("BatteryState", &BTState); -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; +void DO_ACT(const std_msgs::Int16 &msg){ + + + DO_0 = (msg.data & 0x001); + DO_1 = (msg.data & 0x002); +// DO_2 = (msg.data & 0x004); +// DO_3 = (msg.data & 0x008); + DO_4 = (msg.data & 0x010); + DO_5 = (msg.data & 0x020); + DO_6 = (msg.data & 0x040); + DO_7 = (msg.data & 0x080); + DO_8 = (msg.data & 0x100); + DO_9 = (msg.data & 0x200); +// DO_10 = (msg.data & 0x0400); + DO_12 = (msg.data & 0x1000); + DO_13 = (msg.data & 0x2000); + DO_14 = (msg.data & 0x4000); + DO_15 = (msg.data & 0x8000); + + led1 = !led1; + osDelay(1); } -//==========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; +ros::Subscriber<std_msgs::Int16> DO_data_sub("DO_data_sub", &DO_ACT); +//====================================================================== +void CAN_thread(const void *args){ + while (true) { + if (can1.read(rxMsg)){ + seq++; + SOC = rxMsg.data[0]>>1; + Tempert = rxMsg.data[1]-50; + RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2]; + Current = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4]; + MaxCellV = rxMsg.data[6]; + MinCellV = rxMsg.data[7]; } else{ - motor_rpm_vx = -100; + BTState.voltage = 0; + BTState.current = 0; + BTState.design_capacity = 80; + BTState.percentage = 0; } - } - 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; - } - } + 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 = 50; + BTState.percentage = SOC; + osDelay(10); } - 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'); +void DIO_thread(const void *args){ + while (true) { + DI_data.data = 0xffff-0x0001-0x0800 - ((int)DI_1<<1) - (((int)DI_2)<<2) - (((int)DI_3)<<3) - (((int)DI_4)<<4); + DI_data.data -= (((int)DI_5)<<5); + DI_data.data -= (((int)DI_6)<<6); + DI_data.data -= (((int)DI_7)<<7); + DI_data.data -= (((int)DI_8)<<8); + DI_data.data -= (((int)DI_9)<<9); + DI_data.data -= (((int)DI_10)<<10); +// DI_data.data -= (((int)DI_11)<<11); + DI_data.data -= (((int)DI_12)<<12); + DI_data.data -= (((int)DI_13)<<13); + DI_data.data -= (((int)DI_14)<<14); + DI_data.data -= (((int)DI_15)<<15); + osDelay(10); } - return ret; -} - -void update_state(int code1,int code2){ - } //====================================================================== -std_msgs::Int16 DO; -//DO_0 MAG_1 -//DO_1,MAG_2 -//DO_2,MAG_3 -//DO_3,BMS -//DO_4,MainRelay -int State; -void DO_ACT(const std_msgs::Int16 &msg){ - //0xFF for action procedure - if (msg.data == 0x21){ - error_code = 99; - State = WAIT_BAT;//ACT_MG_ON; - } - - if (msg.data == 0x20){ - error_code = 99; - State = ACT_MG_OFF; - } - - if (msg.data == 0x00){ - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - DO_3 = 0; - DO_4 = 0; - } - if (msg.data == 0x40){ - //BMS trigger - DO_3 = 1; - wait(3); - DO_3 = 0; - } - if (msg.data == 0x50){ - //Main Relay off - DO_4 = 0; - } - if (msg.data == 0x51){ - //Main Relay on - DO_4 = 1; - } - if (msg.data == 0x31){ - //Lock triggrt - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - DO_0 = 0; - DO_1 = 1; - DO_2 = 0; - - wait_ms(500); - - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - } - - if (msg.data == 0x30){ - //unLock triggrt - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 1; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 1; - DO_1 = 0; - DO_2 = 1; - - wait_ms(500); - - DO_0 = 1; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 0; - DO_1 = 0; - DO_2 = 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; - DO_2 = 0; - DO_3 = 0; - DO_4 = 1;//for manual turn on the LiBattery, and turn on the main relay to open Load - 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 == 0){ //Battery on-position - //Lock triggrt - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 0; - DO_1 = 1; - DO_2 = 0; - - wait_ms(500); - - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - - // if Battery is off, then do turn on -// if (BMS_state == 0){ - //BMS ON - DO_3 = 1; - wait(4); - DO_3 = 0; -// } - return 1; -// } - } - else if (state == 2){//MAG_OFF - // if Battery is on, then do trun off -// if (BMS_state == 1){ - //BMS ON - DO_3 = 1; - wait(4); - DO_3 = 0; -// } - - //unLock triggrt - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 1; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 1; - DO_1 = 0; - DO_2 = 1; - - wait_ms(500); - - DO_0 = 1; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - - return 1; - } - return 0; -} -//======================================================= int main() { RS232.baud(PC_BAUDRATE); - MBED_ASSERT(Init() == true); - CANMessage rxMsg; - DI_0.mode(PullUp); + nh.initNode(); +// nh.advertise(sonar1); +// nh.advertise(sonar2); +// nh.advertise(ACT_state_pub); + nh.advertise(DI_data_pub); + nh.advertise(BT_pub); + nh.subscribe(DO_data_sub); +// DI_0.mode(PullUp); + DI_1.mode(PullUp); + DI_2.mode(PullUp); + DI_3.mode(PullUp); + DI_4.mode(PullUp); + DI_5.mode(PullUp); + DI_6.mode(PullUp); + DI_7.mode(PullUp); + DI_8.mode(PullUp); + DI_9.mode(PullUp); + DI_10.mode(PullUp); +// DI_11.mode(PullUp); + DI_12.mode(PullUp); + DI_13.mode(PullUp); + DI_14.mode(PullUp); + DI_15.mode(PullUp); + wait_ms(10); + osThreadCreate(osThread(CAN_thread), NULL); + osThreadCreate(osThread(DIO_thread), NULL); +// 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 +// 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); - //Lock triggrt - wait_ms(500); - - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - - DO_0 = 0; - DO_1 = 1; - DO_2 = 0; - - wait_ms(500); - - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - - wait_ms(500); - 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 WAIT_BAT: - if (sensorState == 0){ - State = ACT_MG_ON; - counter = 0; - } - if (counter>1000){ - State = IDLE; - error_code = Sensor_error; - } - break; - - case ACT_MG_ON: - ret = MG_ACT(1); - if (ret){ -// DO_4 = 0; - State = Check_BMS_ON; - counter = 0; - } - if (counter>10){ - State = IDLE; - error_code = Sensor_error; - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - DO_3 = 0; - DO_4 = 0; - } - break; - - case Check_BMS_ON: - if (BMS_state == 1){ - // turn on parrel wire to share the UPS current - DO_4 = 1; - State = IDLE; - error_code = 0; - } - if (counter>100){ - State = IDLE; - error_code = BMS_error; - DO_0 = 0; - DO_1 = 0; - DO_2 = 0; - DO_3 = 0; - DO_4 = 0; - } - break; - - case ACT_MG_OFF: - // turn off parrel wire to avoid voltage feedback to UPS - DO_4 = 0; - - 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>100){ - State = IDLE; - error_code = BMS_error; - } - break; - } - - ACT_state.data = state_code; - ACT_state_pub.publish( &ACT_state); - wait_ms(10); - Error_state.data = error_code; - Error_state_pub.publish( &Error_state); - wait_ms(10); - //============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); - wait_ms(10); - //========================================= - 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) + (unsigned int)rxMsg.data[2]; - Current = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)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(70); - //wait_ms(100); - nh.spinOnce(); + DI_data_pub.publish( &DI_data); +// sonar1.publish( &str_msg1); +// sonar2.publish( &str_msg2); + BT_pub.publish( &BTState ); + nh.spinOnce(); + osDelay(50); } } \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Aug 22 07:44:05 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#5713cbbdb706
--- a/tiny_msgs/tinyIMU.h Tue Oct 02 00:57:35 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,55 +0,0 @@ -#ifndef _ROS_tiny_msgs_tinyIMU_h -#define _ROS_tiny_msgs_tinyIMU_h - -#include <stdint.h> -#include <string.h> -#include <stdlib.h> -#include "ros/msg.h" -#include "std_msgs/Header.h" -#include "tinyVector.h" - -namespace tiny_msgs -{ - - class tinyIMU : public ros::Msg - { - public: - typedef std_msgs::Header _header_type; - _header_type header; - typedef tiny_msgs::tinyVector _accel_type; - _accel_type accel; - typedef tiny_msgs::tinyVector _gyro_type; - _gyro_type gyro; - - tinyIMU(): - header(), - accel(), - gyro() - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - offset += this->header.serialize(outbuffer + offset); - offset += this->accel.serialize(outbuffer + offset); - offset += this->gyro.serialize(outbuffer + offset); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - offset += this->header.deserialize(inbuffer + offset); - offset += this->accel.deserialize(inbuffer + offset); - offset += this->gyro.deserialize(inbuffer + offset); - return offset; - } - - const char * getType(){ return "tiny_msgs/tinyIMU"; }; - const char * getMD5(){ return "53582bc8b7315f3bc7728d82df98bb24"; }; - - }; - -} -#endif \ No newline at end of file
--- a/tiny_msgs/tinyVector.h Tue Oct 02 00:57:35 2018 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,98 +0,0 @@ -#ifndef _ROS_tiny_msgs_tinyVector_h -#define _ROS_tiny_msgs_tinyVector_h - -#include <stdint.h> -#include <string.h> -#include <stdlib.h> -#include "ros/msg.h" - -namespace tiny_msgs -{ - - class tinyVector : public ros::Msg - { - public: - typedef int16_t _x_type; - _x_type x; - typedef int16_t _y_type; - _y_type y; - typedef int16_t _z_type; - _z_type z; - - tinyVector(): - x(0), - y(0), - z(0) - { - } - - virtual int serialize(unsigned char *outbuffer) const - { - int offset = 0; - union { - int16_t real; - uint16_t base; - } u_x; - u_x.real = this->x; - *(outbuffer + offset + 0) = (u_x.base >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (u_x.base >> (8 * 1)) & 0xFF; - offset += sizeof(this->x); - union { - int16_t real; - uint16_t base; - } u_y; - u_y.real = this->y; - *(outbuffer + offset + 0) = (u_y.base >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (u_y.base >> (8 * 1)) & 0xFF; - offset += sizeof(this->y); - union { - int16_t real; - uint16_t base; - } u_z; - u_z.real = this->z; - *(outbuffer + offset + 0) = (u_z.base >> (8 * 0)) & 0xFF; - *(outbuffer + offset + 1) = (u_z.base >> (8 * 1)) & 0xFF; - offset += sizeof(this->z); - return offset; - } - - virtual int deserialize(unsigned char *inbuffer) - { - int offset = 0; - union { - int16_t real; - uint16_t base; - } u_x; - u_x.base = 0; - u_x.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); - u_x.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - this->x = u_x.real; - offset += sizeof(this->x); - union { - int16_t real; - uint16_t base; - } u_y; - u_y.base = 0; - u_y.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); - u_y.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - this->y = u_y.real; - offset += sizeof(this->y); - union { - int16_t real; - uint16_t base; - } u_z; - u_z.base = 0; - u_z.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0); - u_z.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); - this->z = u_z.real; - offset += sizeof(this->z); - return offset; - } - - const char * getType(){ return "tinyVector"; }; - const char * getMD5(){ return "85729383565f7e059d4a213b3db1317b"; }; - - }; - -} -#endif \ No newline at end of file