main.cpp

00001 /*
00002 0412 combine the sevro motor encoder to publish
00003 */
00004 #include "MPU6050_6Axis_MotionApps20.h"
00005 #include "mbed.h"
00006 #include "CAN.h"
00007 #include "I2Cdev.h"
00008 #include "MPU6050.h"
00009 
00010 #include <ros.h>
00011 #include <ros/time.h>
00012 #include <std_msgs/Int16.h>
00013 #include <std_msgs/String.h>
00014 #include <std_msgs/Float32.h>
00015 #include <sensor_msgs/BatteryState.h>
00016 #include <geometry_msgs/Twist.h> //set buffer larger than 50byte
00017 #include <math.h>
00018 #include <stdio.h>
00019 #include <tiny_msgs/tinyVector.h>
00020 #include <tiny_msgs/tinyIMU.h>
00021 #include <string> 
00022 #include <cstdlib>
00023 
00024 #define Start 0xAA
00025 #define Address 0x7F
00026 #define ReturnType 0x00
00027 #define Clean 0x00
00028 #define Reserve 0x00
00029 #define End 0x55 
00030 #define Motor1 1 
00031 #define Motor2 2 
00032 #define LENG 31   //0x42 + 31 bytes equal to 32 bytes
00033 
00034 #define Write 0x06
00035 #define Read 0x03
00036 #define DI1 0x0214      //0214H means digital input DI1 for sevro on
00037 #define APR 0x0066      //0066H means encoder abs rev
00038 #define SP1 0x0112
00039 
00040 #define CAN_DATA  0x470
00041 #define CAN_STATUS  0x471
00042 
00043 #define IDLE            0
00044 #define ACT_MG_ON       1
00045 #define ACT_MG_OFF      2
00046 #define Check_BMS_ON    3
00047 #define Check_BMS_OFF   4
00048 #define WAIT_BAT        5
00049 //Serial pc(USBTX,USBRX);
00050 Timer t;
00051 Serial              RS232(PA_9, PA_10);
00052 DigitalOut          Receiver(D7);                     //RS485_E
00053 DigitalOut          CAN_T(D14);
00054 DigitalOut          CAN_R(D15);
00055 DigitalOut          DO_0(PC_5);
00056 DigitalOut          DO_1(PC_6);
00057 DigitalOut          DO_2(PC_8);
00058 DigitalOut          DO_3(PC_9);
00059 DigitalOut          DO_4(PA_12);
00060 DigitalIn           DI_0(PB_13);
00061 
00062 //CAN                 can1(PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
00063 //CANMsg              rxMsg;
00064 //CANMessage          rxMsg;
00065 Ticker CheckDataR;
00066 
00067 MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin
00068 
00069 ros::NodeHandle  nh;
00070 //======================================================================
00071 tiny_msgs::tinyIMU imu_msg;
00072 ros::Publisher imu_pub("tinyImu", &imu_msg);
00073 //======================================================================
00074 
00075 //======================================================================
00076 std_msgs::Float32 VelAngular_L;
00077 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
00078 //======================================================================
00079 
00080 //======================================================================
00081 std_msgs::Float32 VelAngular_R;
00082 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
00083 //======================================================================
00084 
00085 //======================================================================
00086 sensor_msgs::BatteryState BTState;
00087 ros::Publisher BT_pub("BatteryState", &BTState);
00088 //======================================================================
00089 
00090 //======================================================================
00091 std_msgs::Int16 DI;
00092 ros::Publisher DI_pub("DI_pub", &DI);
00093 //======================================================================
00094 
00095 //======================================================================
00096 std_msgs::Int16 ACT_state;
00097 ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
00098 //======================================================================
00099 
00100 //======================================================================
00101 std_msgs::Int16 Error_state;
00102 ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
00103 //======================================================================
00104 
00105 uint32_t seq;
00106 
00107 //========define ACT_state return code============================================
00108 #define Satndby         0
00109 #define Busy            1
00110 #define Sensor_error    2
00111 #define BMS_error       3
00112 //========================================================
00113 #define IMU_FIFO_RATE_DIVIDER 0x09
00114 #define IMU_SAMPLE_RATE_DIVIDER 4
00115 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
00116 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
00117  
00118 #define PC_BAUDRATE 38400     
00119  
00120 #define DEG_TO_RAD(x) ( x * 0.01745329 )
00121 #define RAD_TO_DEG(x) ( x * 57.29578 )
00122 
00123 const int FIFO_BUFFER_SIZE = 128;
00124 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
00125 uint16_t fifoCount;
00126 uint16_t packetSize;
00127 bool dmpReady;
00128 uint8_t mpuIntStatus;
00129 const int snprintf_buffer_size = 100;
00130 char snprintf_buffer[snprintf_buffer_size];
00131 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
00132 int16_t ax, ay, az;
00133 int16_t gx, gy, gz;
00134 float Lrpm,Rrpm;
00135 float ticks_since_target;
00136 float motor_rpm_r, motor_rpm_l;
00137 float timeout_ticks;
00138 int counter;
00139     double w;
00140     double rate;
00141     double Dimeter;
00142     float dx,dy,dr;
00143 int lastsensorState = 1;    
00144 int sensorState;     
00145 int db_conter = 0;
00146 int buffer[9] = {0};
00147 int dataH,datanum;
00148 int motor_seq,motor_old_seq;
00149 int state_code;
00150 int error_code;
00151 //=========RS485
00152 char recChar=0;
00153 bool recFlag=false;
00154 char recArr[20];
00155 int index=0;
00156 int BMS_state;
00157         uint32_t SOC;
00158         uint32_t Tempert;
00159         uint32_t RackVoltage = 0;
00160         uint32_t Current = 0;
00161         uint32_t MaxCellV = 0;
00162         uint32_t MinCellV = 0;
00163 
00164 struct Offset {
00165     int16_t ax, ay, az;
00166     int16_t gx, gy, gz;
00167 }offset = {150+600, -350+300, 1000, -110-100, 5, 0};//{150, -350, 1000, -110, 5, 0};    // Measured values
00168  
00169 struct MPU6050_DmpData {
00170     Quaternion q;
00171     VectorFloat gravity;    // g
00172     float roll, pitch, yaw;     // rad
00173 }dmpData;
00174  
00175 long map(long x, long in_min, long in_max, long out_min, long out_max) {
00176   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00177 }
00178 //==========define sub function========================
00179 bool Init();
00180 void dmpDataUpdate();
00181 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
00182 int myabs( int a );
00183 void TwistToMotors();   
00184 //===================================================
00185  
00186 
00187 //=======================     motor      =================================================
00188 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
00189 {
00190     unsigned int i, j;
00191     //#define wPolynom 0xA001
00192     unsigned int wCrc = 0xffff;
00193     unsigned int wPolynom = 0xA001;
00194     /*---------------------------------------------------------------------------------*/
00195     for (i = 0; i < iBufLen; i++)
00196     {
00197         wCrc ^= cBuffer[i];
00198         for (j = 0; j < 8; j++)
00199         {
00200             if (wCrc &0x0001)
00201             {
00202                 wCrc = (wCrc >> 1) ^ wPolynom;
00203             }
00204             else
00205             { 
00206                 wCrc = wCrc >> 1;
00207             }
00208         }
00209     }
00210     return wCrc;
00211 }
00212 void Sendmessage(float Rrpm,float Lrpm)
00213 {
00214 //    RS232.printf("Wr = %.1f\n",Rrpm);
00215 //    RS232.printf("Wl = %.1f\n",Lrpm);
00216 unsigned char sendData[16];
00217 unsigned int tmpCRC;
00218 int motor1,motor2;
00219    
00220     sendData[0] = Start;
00221     sendData[1] = Address;
00222     sendData[2] = ReturnType;
00223     sendData[3] = Clean;
00224     sendData[4] = Reserve;
00225     sendData[5]  = 0x01;//motor1Sevro ON
00226     sendData[6] = 0x01;//motor2Sevro ON
00227 if (Rrpm>0){sendData[7] = 0x00;}else{sendData[7] = 0x01;}
00228 if (Lrpm>0){sendData[8] = 0x01;}else{sendData[8] = 0x00;}
00229    motor1 =  abs(Rrpm);
00230    motor2 =  abs(Lrpm); 
00231     
00232     sendData[9] = (motor1>>8);//motor1speedH
00233     sendData[10] = (motor1 & 0xFF);//motor1speedL
00234     sendData[11] = (motor2>>8);//motor2speedH
00235     sendData[12] = (motor2 & 0xFF);//motor2speedL
00236     sendData[13] = End;
00237     tmpCRC = CRC_Verify(sendData, 14);
00238     sendData[14] = (tmpCRC & 0xFF);
00239     sendData[15] = (tmpCRC>>8);
00240     int i;
00241     for (i=0;i<16;i++)
00242     {
00243         RS232.printf("%c",sendData[i]);
00244     }
00245     RS232.printf("\r\n");
00246 }
00247 void TwistToMotors()
00248 {
00249     float right,left;
00250 //    double vel_data[2];
00251     float vel_data[2];
00252     float motor_rpm_vx, motor_rpm_theta;
00253     motor_old_seq = motor_seq;
00254     w = 0.302;//0.2 ;//m
00255     rate = 20;//50;
00256     timeout_ticks = 2;
00257     Dimeter = 0.127;//0.15;
00258     
00259     // prevent agv receive weird 1.0 command from cmd_vel
00260     if (dr == 1.0){
00261         dr = 0.001;
00262     }
00263     right = ( 1.0 * dx ) + (dr * w /2);
00264     left = ( 1.0 * dx ) - (dr * w /2);
00265     motor_rpm_vx = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
00266     if((motor_rpm_vx !=0) && (myabs(motor_rpm_vx)<100)){
00267         if(motor_rpm_vx >0){
00268             motor_rpm_vx = 100;
00269         }
00270         else{
00271             motor_rpm_vx = -100;
00272         }
00273     }
00274     motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);
00275     motor_rpm_r = motor_rpm_vx+ motor_rpm_theta;
00276     motor_rpm_l = motor_rpm_vx- motor_rpm_theta;
00277     if (myabs(motor_rpm_r)<100|| myabs(motor_rpm_l)<100){
00278         if( dx==0){
00279             if(dr>0){
00280                 motor_rpm_r=100;
00281                 motor_rpm_l=-100;
00282             }else if (dr<0){
00283                 motor_rpm_r=-100;
00284                 motor_rpm_l=100; 
00285             }else{
00286                 motor_rpm_r=0;
00287                 motor_rpm_l=0;
00288             }
00289         }
00290         else if(dx>0){
00291             if (myabs(motor_rpm_r)<100){
00292                 motor_rpm_r =100;
00293             }
00294             if (myabs(motor_rpm_l)<100){
00295                 motor_rpm_l =100;
00296             }
00297         }
00298         else{
00299             if(myabs(motor_rpm_r)<100){
00300                 motor_rpm_r =-100;
00301             }
00302             if(myabs(motor_rpm_l)<100){
00303                 motor_rpm_l =-100;
00304             }
00305         }
00306     }
00307         vel_data[0] = motor_rpm_r;
00308         vel_data[1] = motor_rpm_l;
00309         
00310 //===================================================================
00311     //Sendmessage(vel_data[0],vel_data[1]);
00312        
00313     //Sendmessage(motor_rpm_l,motor_rpm_r);
00314        
00315     VelAngular_R.data = vel_data[0];
00316     VelAngular_L.data = vel_data[1];
00317     //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
00318     //}
00319     //else{
00320     pub_rmotor.publish( &VelAngular_R );
00321     pub_lmotor.publish( &VelAngular_L );
00322     //}
00323     //RS232.printf("Wr = %.1f\n",vel_data[0]);
00324     //RS232.printf("Wl = %.1f\n",vel_data[1]);
00325     ticks_since_target += 1;
00326 
00327 }
00328 int myabs( int a ){
00329     if ( a < 0 ){
00330         return -a;
00331         }
00332         return a;
00333         }
00334 
00335 int str2int(const char* str, int star, int end)
00336 {
00337     int i;
00338     int ret = 0;
00339     for (i = star; i < end+1; i++)
00340     {
00341         ret = ret *10 + (str[i] - '0');
00342     }
00343     return ret;     
00344 }       
00345 
00346 void update_state(int code1,int code2){
00347 
00348 }
00349 //======================================================================
00350 std_msgs::Int16 DO;
00351 //DO_0 MAG_1
00352 //DO_1,MAG_2
00353 //DO_2,MAG_3
00354 //DO_3,BMS
00355 //DO_4,MainRelay
00356 int State;
00357 void DO_ACT(const std_msgs::Int16 &msg){
00358     //0xFF for action procedure
00359     if (msg.data == 0x21){
00360         error_code = 99;
00361         State = WAIT_BAT;//ACT_MG_ON;
00362     }
00363     
00364     if (msg.data == 0x20){
00365         error_code = 99;
00366         State = ACT_MG_OFF;
00367     }
00368     
00369     if (msg.data == 0x00){
00370         DO_0 = 0;
00371         DO_1 = 0;
00372         DO_2 = 0;
00373         DO_3 = 0;
00374         DO_4 = 0;
00375     }
00376     if (msg.data == 0x40){    
00377             //BMS trigger
00378             DO_3 = 1;            
00379             wait(3);
00380             DO_3 = 0;
00381     }
00382         if (msg.data == 0x50){    
00383             //Main Relay off
00384             DO_4 = 0;            
00385     }
00386         if (msg.data == 0x51){    
00387             //Main Relay on
00388             DO_4 = 1;            
00389     }
00390     if (msg.data == 0x31){    
00391             //Lock triggrt
00392             DO_0 = 0;
00393             DO_1 = 0;
00394             DO_2 = 0;
00395 
00396             DO_0 = 0;
00397             DO_1 = 1;
00398             DO_2 = 0;
00399                         
00400             wait_ms(500);
00401 
00402             DO_0 = 0;
00403             DO_1 = 0;
00404             DO_2 = 0;
00405     }
00406     
00407     if (msg.data == 0x30){    
00408             //unLock triggrt
00409             DO_0 = 0;
00410             DO_1 = 0;
00411             DO_2 = 0;
00412 
00413             DO_0 = 1;
00414             DO_1 = 0;
00415             DO_2 = 0;
00416 
00417             DO_0 = 1;
00418             DO_1 = 0;
00419             DO_2 = 1;
00420 
00421             wait_ms(500);
00422           
00423             DO_0 = 1;
00424             DO_1 = 0;
00425             DO_2 = 0;
00426 
00427             DO_0 = 0;
00428             DO_1 = 0;
00429             DO_2 = 0;
00430     }
00431 
00432 }
00433 ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
00434 //======================================================================
00435 //======================================================================================
00436 void messageCb(const geometry_msgs::Twist &msg)
00437 {  
00438 //    RS232.printf("messageCb");
00439     ticks_since_target = 0;
00440     dx = msg.linear.x;
00441     dy = msg.linear.y;
00442     dr = msg.angular.z;
00443 //    RS232.printf("dx = %d,dy = %d,dr = %d\r\n",dx,dy,dr);
00444     TwistToMotors(); 
00445     //ReadENC(Motor1);
00446 }
00447 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
00448  //======================================================================================
00449 void dmpDataUpdate() {
00450     // Check that this interrupt has enabled.
00451     if (dmpReady == false) return;
00452     
00453     mpuIntStatus = mpu.getIntStatus();
00454     fifoCount = mpu.getFIFOCount();
00455     
00456     // Check that this interrupt is a FIFO buffer overflow interrupt.
00457     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
00458         mpu.resetFIFO();
00459         //pc.printf("FIFO overflow!\n");
00460         return;
00461         
00462     // Check that this interrupt is a Data Ready interrupt.
00463     } else if (mpuIntStatus & 0x02) {
00464         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
00465         
00466         mpu.getFIFOBytes(fifoBuffer, packetSize);
00467         
00468     #ifdef OUTPUT_QUATERNION
00469         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00470         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;
00471         pc.printf(snprintf_buffer);
00472     #endif
00473         
00474     #ifdef OUTPUT_EULER
00475         float euler[3];
00476         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00477         mpu.dmpGetEuler(euler, &dmpData.q);
00478         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;
00479         pc.printf(snprintf_buffer);
00480     #endif
00481         
00482     #ifdef OUTPUT_ROLL_PITCH_YAW
00483         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
00484         mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
00485         float rollPitchYaw[3];
00486         mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
00487         dmpData.roll = rollPitchYaw[2];
00488         dmpData.pitch = rollPitchYaw[1];
00489         dmpData.yaw = rollPitchYaw[0];
00490         
00491         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;
00492         pc.printf(snprintf_buffer);
00493                  
00494 #ifdef servotest
00495         int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
00496         if(servoPulse > 1450) servoPulse = 1450;
00497         if(servoPulse < 500) servoPulse = 500;
00498         sv.pulsewidth_us(servoPulse);
00499 #endif
00500     #endif
00501         
00502     #ifdef OUTPUT_FOR_TEAPOT
00503         teapotPacket[2] = fifoBuffer[0];
00504         teapotPacket[3] = fifoBuffer[1];
00505         teapotPacket[4] = fifoBuffer[4];        
00506         teapotPacket[5] = fifoBuffer[5];
00507         teapotPacket[6] = fifoBuffer[8];
00508         teapotPacket[7] = fifoBuffer[9];
00509         teapotPacket[8] = fifoBuffer[12];
00510         teapotPacket[9] = fifoBuffer[13];
00511         for (uint8_t i = 0; i < 14; i++) {
00512             pc.putc(teapotPacket[i]);
00513         }
00514     #endif
00515         
00516     #ifdef OUTPUT_TEMPERATURE
00517         float temp = mpu.getTemperature() / 340.0 + 36.53;
00518         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
00519         pc.printf(snprintf_buffer);
00520     #endif
00521         
00522 //        pc.printf("\n");
00523     }
00524 }
00525 //=======================================================
00526 bool Init() {
00527     DO_0 = 0;
00528     DO_1 = 0;
00529     DO_2 = 0;
00530     DO_3 = 0;
00531     DO_4 = 1;//for manual turn on the LiBattery, and turn on the main relay to open Load
00532     seq = 0;
00533     nh.initNode();
00534 //    nh.advertise(imu_pub);
00535     nh.advertise(pub_lmotor);
00536     nh.advertise(pub_rmotor);
00537     nh.advertise(BT_pub);
00538     nh.advertise(DI_pub);
00539     nh.advertise(ACT_state_pub);
00540     nh.advertise(Error_state_pub);
00541     nh.subscribe(cmd_vel_sub);
00542     nh.subscribe(ACT_sub);
00543     
00544     //==========================
00545     /*
00546     mpu.initialize();
00547     if (mpu.testConnection()) {
00548 //        pc.printf("MPU6050 test connection passed.\n");
00549     } else {
00550 //        pc.printf("MPU6050 test connection failed.\n");
00551         return false;
00552     }
00553     if (mpu.dmpInitialize() == 0) {
00554 //        pc.printf("succeed in MPU6050 DMP Initializing.\n");
00555     } else {
00556 //        pc.printf("failed in MPU6050 DMP Initializing.\n");
00557         return false;
00558     }
00559 
00560     //mpu.setXAccelOffsetTC(offset.ax);
00561 //    mpu.setYAccelOffsetTC(offset.ay);
00562 //    mpu.setZAccelOffset(offset.az);
00563     mpu.setXAccelOffset(1000); //2000 -3134
00564     mpu.setYAccelOffset(0);
00565     mpu.setZAccelOffset(0);
00566     mpu.setXGyroOffset(800);//offset.gx);
00567     mpu.setYGyroOffset(0);//offset.gy);
00568     mpu.setZGyroOffset(0);//offset.gz);
00569     
00570     
00571     
00572     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);//2000);
00573     mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
00574     mpu.setDMPEnabled(true);    // Enable DMP
00575     
00576     packetSize = mpu.dmpGetFIFOPacketSize();
00577     dmpReady = true;    // Enable interrupt.
00578         */
00579     //pc.printf("Init finish!\n");
00580  
00581     return true;
00582 }
00583 int MG_ACT(int state)
00584 {
00585 //    int ret;
00586     if (state == 1){    //MAG_ON
00587 //        if (sensorState == 0){     //Battery on-position
00588             //Lock triggrt
00589             DO_0 = 0;
00590             DO_1 = 0;
00591             DO_2 = 0;
00592 
00593             DO_0 = 0;
00594             DO_1 = 1;
00595             DO_2 = 0;
00596                         
00597             wait_ms(500);
00598 
00599             DO_0 = 0;
00600             DO_1 = 0;
00601             DO_2 = 0;
00602             
00603             // if Battery is off, then do turn on
00604 //            if (BMS_state == 0){
00605             //BMS ON
00606             DO_3 = 1;
00607             wait(4);
00608             DO_3 = 0;
00609 //            }
00610             return 1;
00611 //        }
00612     }
00613     else if (state == 2){//MAG_OFF
00614             // if Battery is on, then do trun off
00615 //            if (BMS_state == 1){
00616             //BMS ON
00617             DO_3 = 1;
00618             wait(4);
00619             DO_3 = 0;
00620 //            }
00621 
00622             //unLock triggrt
00623             DO_0 = 0;
00624             DO_1 = 0;
00625             DO_2 = 0;
00626 
00627             DO_0 = 1;
00628             DO_1 = 0;
00629             DO_2 = 0;
00630 
00631             DO_0 = 1;
00632             DO_1 = 0;
00633             DO_2 = 1;
00634 
00635             wait_ms(500);
00636           
00637             DO_0 = 1;
00638             DO_1 = 0;
00639             DO_2 = 0;
00640 
00641             DO_0 = 0;
00642             DO_1 = 0;
00643             DO_2 = 0;
00644 
00645             return 1;
00646     }
00647     return 0;
00648 }
00649 //=======================================================
00650 int main() {
00651     RS232.baud(PC_BAUDRATE);
00652     MBED_ASSERT(Init() == true);
00653     CANMessage rxMsg;
00654     DI_0.mode(PullUp);
00655     CAN_T = 0;
00656     CAN_R = 0;
00657     wait_ms(50);
00658     CAN     can1(D15,D14);//PB_8,PB_9);  // CAN Rx pin name, CAN Tx pin name
00659     wait_ms(50);
00660     can1.frequency(500000);
00661     wait_ms(50);
00662     
00663     //Lock triggrt
00664     wait_ms(500);
00665 
00666     DO_0 = 0;
00667     DO_1 = 0;
00668     DO_2 = 0;
00669 
00670     DO_0 = 0;
00671     DO_1 = 1;
00672     DO_2 = 0;
00673                         
00674     wait_ms(500);
00675 
00676     DO_0 = 0;
00677     DO_1 = 0;
00678     DO_2 = 0;
00679     
00680     wait_ms(500);
00681     
00682     while(1){   
00683         seq++;
00684         motor_seq = seq;
00685         t.start();
00686         //================================
00687         //========define ACT_state return code============================================
00688         //#define Satndby         0x00
00689         //#define Busy            0x01
00690         //#define Sensor_error    0x10
00691         //========================================================
00692         counter++;
00693         state_code = State;
00694         switch (State){
00695             int ret;
00696             case IDLE:
00697                 counter = 0;
00698             break;
00699             
00700             case WAIT_BAT:
00701             if (sensorState == 0){
00702                 State = ACT_MG_ON;
00703                 counter = 0;
00704             }
00705             if (counter>1000){
00706                 State = IDLE;
00707                 error_code = Sensor_error;
00708             }
00709             break;
00710             
00711             case ACT_MG_ON:
00712                 ret = MG_ACT(1);
00713                 if (ret){
00714 //                     DO_4 = 0; 
00715                      State = Check_BMS_ON;
00716                      counter = 0;
00717                 }
00718                 if (counter>10){
00719                     State = IDLE;
00720                     error_code = Sensor_error;
00721                     DO_0 = 0;
00722                     DO_1 = 0;
00723                     DO_2 = 0;
00724                     DO_3 = 0;
00725                     DO_4 = 0;
00726                 }
00727             break;
00728             
00729             case Check_BMS_ON:
00730                 if (BMS_state == 1){
00731                     // turn on parrel wire to share the UPS current
00732                     DO_4 = 1;
00733                     State = IDLE;
00734                     error_code = 0;
00735                 }
00736                 if (counter>100){
00737                     State = IDLE;
00738                     error_code = BMS_error;
00739                     DO_0 = 0;
00740                     DO_1 = 0;
00741                     DO_2 = 0;
00742                     DO_3 = 0;
00743                     DO_4 = 0;
00744                 }   
00745             break;
00746             
00747             case ACT_MG_OFF:
00748                 // turn off parrel wire to avoid voltage feedback to UPS
00749                 DO_4 = 0;
00750                 
00751                 ret = MG_ACT(2);
00752                 if (ret){ 
00753                      State = Check_BMS_OFF;
00754                      counter = 0;
00755                 }
00756                 if (counter>10){
00757                     State = IDLE;
00758                     error_code = Sensor_error;
00759                 }                
00760             break;
00761             
00762             case Check_BMS_OFF:
00763                 if (BMS_state == 0){
00764                     State = IDLE;
00765                     error_code = 0;
00766                 }
00767                 if (counter>100){
00768                     State = IDLE;
00769                     error_code = BMS_error;
00770                 }   
00771             break;
00772             }
00773             
00774         ACT_state.data = state_code;
00775         ACT_state_pub.publish( &ACT_state);
00776         wait_ms(10);
00777         Error_state.data = error_code;
00778         Error_state_pub.publish( &Error_state);
00779         wait_ms(10);
00780         //============DI==================
00781         int reading = DI_0;
00782         if (reading == lastsensorState) {
00783             db_conter = db_conter+1;
00784         }
00785         else{
00786             db_conter = 0;
00787         }        
00788         if (db_conter > 3) {
00789             sensorState = reading;
00790             DI.data = sensorState; 
00791         }
00792 
00793         lastsensorState = reading;
00794         DI_pub.publish( &DI);
00795         wait_ms(10);
00796         //=========================================
00797         if (can1.read(rxMsg) && (t.read_ms() > 5000))  {
00798             if(rxMsg.id == CAN_DATA){
00799                 BMS_state = 1;
00800                 SOC = rxMsg.data[0]>>1;
00801                 Tempert = rxMsg.data[1]-50;
00802                 RackVoltage  = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2];
00803                 Current      = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4];
00804                 MaxCellV = rxMsg.data[6];
00805                 MinCellV = rxMsg.data[7];                
00806                 BTState.header.stamp = nh.now();
00807                 BTState.header.frame_id = 0;
00808                 BTState.header.seq = seq;
00809                 BTState.voltage = RackVoltage*0.1;
00810                 BTState.current = Current;
00811                 BTState.design_capacity = 80;
00812                 BTState.percentage = SOC;
00813                 BT_pub.publish( &BTState );
00814                 t.reset();   
00815             } // if
00816         } // if
00817         else{
00818             BMS_state = 0;
00819         }
00820         
00821         
00822         if (motor_seq  > motor_old_seq + 5){
00823         motor_rpm_r = 0;
00824         motor_rpm_l = 0;
00825         }       
00826         
00827         Sendmessage(motor_rpm_l,motor_rpm_r);
00828         wait_ms(70);
00829         //wait_ms(100);     
00830         nh.spinOnce();    
00831     }
00832 }