Diff: main.cpp

Revision:

5:52fb31c1a8c0

Parent:

4:b728b0359ec8

Child:

6:eadfb1b45bda

--- a/main.cpp	Fri Apr 13 07:03:10 2018 +0000
+++ b/main.cpp	Tue Apr 17 07:59:09 2018 +0000
@@ -3,6 +3,7 @@
 */
 #include "MPU6050_6Axis_MotionApps20.h"
 #include "mbed.h"
+
 //#include "CANMsg.h"
 #include "I2Cdev.h"
 #include "MPU6050.h"
@@ -17,6 +18,8 @@
 #include <stdio.h>
 #include <tiny_msgs/tinyVector.h>
 #include <tiny_msgs/tinyIMU.h>
+#include <string> 
+#include <cstdlib>
 
 #define Start 0xAA
 #define Address 0x7F
@@ -26,9 +29,16 @@
 #define End 0x55 
 #define Motor1 1 
 #define Motor2 2 
-//Serial              pc(PA_2, PA_3,9600);
-Serial              RS485(PA_9, PA_10);
-DigitalOut          RS485_E(D7);                     //RS485_E
+#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
+//Serial pc(USBTX,USBRX);
+Serial              rs485(PA_9, PA_10);
+DigitalOut          Receiver(D7);                     //RS485_E
 DigitalOut          myled(LED1);
 Ticker CheckDataR;
 
@@ -51,7 +61,7 @@
 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
  
-#define PC_BAUDRATE 115200     
+#define PC_BAUDRATE 38400     
  
 #define DEG_TO_RAD(x) ( x * 0.01745329 )
 #define RAD_TO_DEG(x) ( x * 57.29578 )
@@ -77,7 +87,7 @@
     float dx,dy,dr;
         
 int buffer[9] = {0};
-
+int dataH,datanum;
 //=========RS485
 char recChar=0;
 bool recFlag=false;
@@ -102,7 +112,7 @@
 bool Init();
 void dmpDataUpdate();
 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
-void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data);
+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);
@@ -113,44 +123,7 @@
 void readData();
 //===================================================
  
-bool Init() {
-    //pc.baud(PC_BAUDRATE);
-//    pc.printf("Init.\n");
 
-    seq = 0;
-    //while(!pc.readable());
-    //        pc.getc();
-    
-        nh.initNode();
-    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");
- 
-    return true;
-}
 //=======================     motor      =================================================
 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
 {
@@ -177,103 +150,49 @@
     return wCrc;
 }
    
-void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
+void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data)
 {
-    unsigned char sendData[8];
-    int16_t tmpCRC;
-    //char MotorAddress;
-    char Function;
-    //int DataAddress,Data;
-    char DataAddressH,DataAddressL;
-    char dataH,dataL;
-    int i;
-    sendData[6] = 0xff;
-    sendData[7] = 0xff;
-    
-    //MotorAddress = address;
-    Function = 0x06;
-    //DataAddress = data;
-    //Data = 0x0001;
-    DataAddressH = ((DataAddress>>8)&0xFF);
-    DataAddressL = ((DataAddress>>0)&0xFF);
-    dataH = ((Data>>8)&0xFF);
-    dataL = ((Data>>0)&0xFF);
-    
-    sendData[0] = MotorAddress;
-    sendData[1] = Function;
-    sendData[2] = DataAddressH;
-    sendData[3] = DataAddressL;
-    sendData[4] = dataH;
-    sendData[5] = dataL;
-    tmpCRC = CRC_Verify(sendData, 6);
-    sendData[6] = (tmpCRC & 0xFF);
-    sendData[7] = (tmpCRC>>8);
-    RS485_E = 1;
-    wait_ms(10);
-    for (i=0;i<8;i++)
-    {
-        RS485.printf("%c",sendData[i]);
-    }
-    wait_ms(10);
-    RS485_E = 0;
-    //RS485.attach(&onInterrupt,Serial::RxIrq);
-    //===========================================
-}   
-   
-void ReadAddress(char MotorAddress,int16_t DataAddress)
-{
-    unsigned char sendData[8];
-    int16_t tmpCRC;
-    //char MotorAddress;
-    char Function;
-    //int DataAddress,Data;
-    char DataAddressH,DataAddressL;
+    unsigned char sendData[16];
+//    int16_t tmpCRC;
+//    char DataAddressH,DataAddressL;
 //    char dataH,dataL;
     int i;
-    
-    //MotorAddress = address;
-    Function = 0x03;
-    //DataAddress = data;
-    //Data = 0x0001;
-    DataAddressH = ((DataAddress>>8)&0xFF);
-    DataAddressL = ((DataAddress>>0)&0xFF);
-    
+    Receiver = 1;
+    wait_ms(1);   
     sendData[0] = MotorAddress;
-    sendData[1] = Function;
-    sendData[2] = DataAddressH;
-    sendData[3] = DataAddressL;
-    sendData[4] = 0x00;//dataH;
-    sendData[5] = 0x02;//dataL;
-    tmpCRC = CRC_Verify(sendData, 6);
-    sendData[6] = (tmpCRC & 0xFF);
-    sendData[7] = (tmpCRC>>8);
-    RS485_E = 1;
-    wait_ms(10);
-    for (i=0;i<8;i++)
+    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++)
     {
-        RS485.printf("%c",sendData[i]);
+        rs485.printf("%02X",sendData[i]);
     }
-    
-    wait_ms(10);
-    RS485_E = 0;
-    //onInterrupt();
-    //===========================================
-}  
+    wait_ms(1);
+    Receiver = 0;
+    wait_ms(10);        //give 10ms let receive complete.
+}   
+
 void SVON(char MotorStation)
 {
-    //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
-    setAddress(MotorStation,0x0214,0x0001);
+    //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData)
+    setAddress(MotorStation,Write,DI1,0x0001);
 } 
 
 void SVOFF(char MotorStation)
 {
-    //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
-    setAddress(MotorStation,0x0214,0x0101);
+    //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData)
+    setAddress(MotorStation,Write,DI1,0x0101);
     wait_ms(10);
 } 
 void ReadENC(char MotorStation)
 {
-    ReadAddress(MotorStation,0x0066);
+    //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t readDataNum)
+    setAddress(MotorStation,Read,APR,0x0001);
     wait_ms(10);
     
 }
@@ -315,15 +234,22 @@
     Lrpm = motor_rpm_l;
     Rrpm = motor_rpm_r;
 
-    setAddress(Motor1,0x0112,Lrpm);
-    setAddress(Motor2,0x0112,Rrpm);
-
-    VelAngular_R.data = vel_data[0];
-    VelAngular_L.data = vel_data[1];
+//    setAddress(Motor1,Write,0x0112,Lrpm);
+//    setAddress(Motor2,Write,0x0112,Rrpm);
 
     ticks_since_target += 1;
 }
-        
+
+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');
+    }
+    return ret;     
+}       
 
 void messageCb(const geometry_msgs::Twist &msg)
 {  
@@ -332,7 +258,7 @@
     dy = msg.linear.y;
     dr = msg.angular.z;
     TwistToMotors(); 
-    ReadENC(Motor1);
+    //ReadENC(Motor1);
 }
 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
  //======================================================================================
@@ -414,45 +340,104 @@
     }
 }
 
-//================RS485=======================================
+//================rs485=======================================
 void flushSerialBuffer() {
 
-    while (RS485.readable()) { 
-        RS485.getc(); 
+    while (rs485.readable()) { 
+        rs485.getc(); 
     } 
 }
-    
-void readData() {
-    //RS485_E = 1;        //1=write
-    //wait_ms(3);
-    //RS485.putc('C');
-    //wait_ms(1);
-    
-    RS485_E = 0;
-    wait_ms(3);
-    flushSerialBuffer();
-    wait_ms(1);
-    RS485_E = 1;
-    wait_ms(3);
-    myled = !myled;
-    //=====================
+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;
+        }
+    }
 }
 //=======================================================
-
-int main() {
-    RS485.baud(PC_BAUDRATE);
-    MBED_ASSERT(Init() == true);
-//    myled = 0;
+bool Init() {
+    seq = 0;
     nh.initNode();
     nh.advertise(imu_pub);
-//    nh.advertise(pub_lmotor);
-//    nh.advertise(pub_rmotor);
+    nh.advertise(pub_lmotor);
+    nh.advertise(pub_rmotor);
     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.
+    
+    //pc.printf("Init finish!\n");
+ 
+    return true;
+}
+//=======================================================
+int main() {
+    rs485.baud(PC_BAUDRATE);
+    MBED_ASSERT(Init() == true);
+
     SVON(1);
     SVON(2);
     flushSerialBuffer();
-    RS485.attach(&readData);//,0.25);
-    //CheckDataR.attach(&CALL_chDataR,0.25);
+    rs485.attach(&readData);//,0.25);
     while(1) 
     {   
         seq++;
@@ -469,6 +454,10 @@
         imu_msg.gyro.z = gz;       
         imu_pub.publish( &imu_msg );
         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);
          
     }