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