![](/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:
- 9:e10bd4b460d7
- Parent:
- 8:4974fc24fbd7
- Child:
- 10:8b7fce3bba86
--- a/main.cpp Tue May 15 00:46:30 2018 +0000 +++ b/main.cpp Wed May 23 01:54:11 2018 +0000 @@ -40,14 +40,21 @@ #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_8); -DigitalOut DO_1(PC_9); +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 @@ -84,27 +91,23 @@ //====================================================================== //====================================================================== -std_msgs::Int16 DO; - -void DO_ACT(const std_msgs::Int16 &msg){ - if (msg.data & 0x01){ - DO_0 = 1; - } - else{ - DO_0 = 0; - } +std_msgs::Int16 ACT_state; +ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state); +//====================================================================== - if (msg.data & 0x02){ - DO_1 = 1; - } - else{ - DO_1 = 0; - } -} -ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT); //====================================================================== +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 @@ -128,23 +131,27 @@ int16_t gx, gy, gz; float Lrpm,Rrpm; float ticks_since_target; -double timeout_ticks; - +float motor_rpm_r, motor_rpm_l; +float timeout_ticks; +int counter; double w; double rate; double Dimeter; float dx,dy,dr; -int lastButtonState = 1; -int buttonState; +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; @@ -155,7 +162,7 @@ struct Offset { int16_t ax, ay, az; int16_t gx, gy, gz; -}offset = {150, -350, 1000, -110, 5, 0}; // Measured values +}offset = {150+600, -350+300, 1000, -110-100, 5, 0};//{150, -350, 1000, -110, 5, 0}; // Measured values struct MPU6050_DmpData { Quaternion q; @@ -202,8 +209,8 @@ } void Sendmessage(float Rrpm,float Lrpm) { - //RS232.printf("Wr = %.1f\n",Rrpm); - //RS232.printf("Wl = %.1f\n",Lrpm); +// RS232.printf("Wr = %.1f\n",Rrpm); +// RS232.printf("Wl = %.1f\n",Lrpm); unsigned char sendData[16]; unsigned int tmpCRC; int motor1,motor2; @@ -215,8 +222,8 @@ 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;} +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); @@ -237,76 +244,72 @@ } 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; - float right,left; - float motor_rpm_r, motor_rpm_l; - double vel_data[2]; + + // 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); -// 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; + 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_r=-100; - motor_rpm_l=100; + else{ + motor_rpm_vx = -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 ); + 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; + } + } } - 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;*/ + vel_data[0] = motor_rpm_r; + vel_data[1] = motor_rpm_l; + //=================================================================== - Sendmessage(vel_data[0],vel_data[1]); + //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){ @@ -320,7 +323,6 @@ ticks_since_target += 1; } - int myabs( int a ){ if ( a < 0 ){ return -a; @@ -338,13 +340,71 @@ } 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); } @@ -433,14 +493,16 @@ seq = 0; nh.initNode(); - nh.advertise(imu_pub); +// 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"); @@ -454,22 +516,59 @@ // 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.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.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 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); @@ -483,59 +582,106 @@ wait_ms(50); can1.frequency(500000); wait_ms(50); + DO_2 = 1; //let UPS ON + + + while(1){ seq++; + motor_seq = 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 ); + //================================ + //========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 == lastButtonState) { + if (reading == lastsensorState) { db_conter = db_conter+1; } else{ db_conter = 0; - } - + } if (db_conter > 3) { -// if (reading != buttonState) { - buttonState = reading; - DI.data = buttonState; -// } + sensorState = reading; + DI.data = sensorState; } - lastButtonState = reading; + lastsensorState = 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){ + 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]; -// 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); - + MinCellV = rxMsg.data[7]; BTState.header.stamp = nh.now(); BTState.header.frame_id = 0; BTState.header.seq = seq; @@ -547,7 +693,18 @@ t.reset(); } // if } // if - nh.spinOnce(); - wait_ms(10); + 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(); } } \ No newline at end of file