Diff: main.cpp

Revision:

3:51194773aa7e

Parent:

2:648583d6e41a

Child:

4:b728b0359ec8

diff -r 648583d6e41a -r 51194773aa7e main.cpp
--- a/main.cpp	Thu Apr 12 01:15:57 2018 +0000
+++ b/main.cpp	Fri Apr 13 06:58:49 2018 +0000
@@ -1,5 +1,5 @@
 /*
-
+0412 combine the sevro motor encoder to publish
 */
 #include "MPU6050_6Axis_MotionApps20.h"
 #include "mbed.h"
@@ -30,6 +30,8 @@
 Serial              RS485(PA_9, PA_10);
 DigitalOut          RS485_E(D7);                     //RS485_E
 DigitalOut          myled(LED1);
+Ticker CheckDataR;
+
 MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin
 
 tiny_msgs::tinyIMU imu_msg;
@@ -73,6 +75,15 @@
     double rate;
     double Dimeter;
     float dx,dy,dr;
+        
+int buffer[9] = {0};
+
+//=========RS485
+char recChar=0;
+bool recFlag=false;
+char recArr[20];
+int index=0;
+
 struct Offset {
     int16_t ax, ay, az;
     int16_t gx, gy, gz;
@@ -92,10 +103,14 @@
 void dmpDataUpdate();
 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
 void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data);
+void ReadAddress(char MotorAddress,int16_t DataAddress);
 void SVON(char MotorStation);
+void ReadENC(char MotorStation);
 void SVOFF(char MotorStation);
 int myabs( int a );
-void TwistToMotors();        
+void TwistToMotors();   
+void flushSerialBuffer();
+void readData();
 //===================================================
  
 bool Init() {
@@ -201,12 +216,53 @@
     }
     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;
+//    char dataH,dataL;
+    int i;
+    
+    //MotorAddress = address;
+    Function = 0x03;
+    //DataAddress = data;
+    //Data = 0x0001;
+    DataAddressH = ((DataAddress>>8)&0xFF);
+    DataAddressL = ((DataAddress>>0)&0xFF);
+    
+    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++)
+    {
+        RS485.printf("%c",sendData[i]);
+    }
+    
+    wait_ms(10);
+    RS485_E = 0;
+    //onInterrupt();
+    //===========================================
+}  
 void SVON(char MotorStation)
 {
     //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
-    setAddress(MotorStation,0x0214,0x0100);
+    setAddress(MotorStation,0x0214,0x0001);
 } 
 
 void SVOFF(char MotorStation)
@@ -215,6 +271,12 @@
     setAddress(MotorStation,0x0214,0x0101);
     wait_ms(10);
 } 
+void ReadENC(char MotorStation)
+{
+    ReadAddress(MotorStation,0x0066);
+    wait_ms(10);
+    
+}
 int myabs( int a ){
     if ( a < 0 ){
         return -a;
@@ -255,17 +317,10 @@
 
     setAddress(Motor1,0x0112,Lrpm);
     setAddress(Motor2,0x0112,Rrpm);
-    
+
     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 );
-    //}
-    //pc.printf("Wr = %.1f\n",vel_data[0]);
-    //pc.printf("Wl = %.1f\n",vel_data[1]);
+
     ticks_since_target += 1;
 }
         
@@ -277,6 +332,7 @@
     dy = msg.linear.y;
     dr = msg.angular.z;
     TwistToMotors(); 
+    ReadENC(Motor1);
 }
 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
  //======================================================================================
@@ -358,19 +414,51 @@
     }
 }
 
+//================RS485=======================================
+void flushSerialBuffer() {
+
+    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;
+    //=====================
+}
+//=======================================================
+
 int main() {
     RS485.baud(PC_BAUDRATE);
     MBED_ASSERT(Init() == true);
-    
+//    myled = 0;
     nh.initNode();
     nh.advertise(imu_pub);
 //    nh.advertise(pub_lmotor);
 //    nh.advertise(pub_rmotor);
     nh.subscribe(cmd_vel_sub);
-    SVON(1);
-    SVON(2);
+//    SVON(1);
+//    SVON(2);
+    flushSerialBuffer();
+    RS485.attach(&readData);//,0.25);
+    //CheckDataR.attach(&CALL_chDataR,0.25);
     while(1) 
     { 
+//==================================
+
+//=============================
+    
         seq++;
         mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
@@ -387,6 +475,9 @@
         nh.spinOnce();
         wait_ms(10);
         //writing current accelerometer and gyro position 
-        //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz);    
+        //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz);   
+        
+//        RS485.attach(&onInterrupt,Serial::RxIrq);
+         
     }
 }
\ No newline at end of file