Revision 2:648583d6e41a, committed 2018-04-12

Comitter:

WeberYang

Date:

Thu Apr 12 01:15:57 2018 +0000

Parent:

1:41ec5f98abab

Commit message:

beta1.0 finish Delta servo motor control and IMU-6050

Changed in this revision

--- a/MPU6050-DMP.lib	Wed Oct 25 08:17:05 2017 +0000
+++ b/MPU6050-DMP.lib	Thu Apr 12 01:15:57 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/hardtail/code/MPU6050-DMP/#fe57b4405983
+https://os.mbed.com/users/WeberYang/code/MPU6050-DMP/#6b1b4bcaf494

--- a/USBDevice.lib	Wed Oct 25 08:17:05 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/teams/LSE2-2014/code/USBDevice/#7dd4ca6f4a33

--- a/main.cpp	Wed Oct 25 08:17:05 2017 +0000
+++ b/main.cpp	Thu Apr 12 01:15:57 2018 +0000
@@ -1,64 +1,59 @@
-/* Demo code for MPU6050 DMP
- * I thank Ian Hua.
- * Copyright (c) 2015 Match
- *
- * THE PROGRAM IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE PROGRAM OR THE USE OR OTHER DEALINGS IN
- * THE PROGRAM.
- */
- 
- 
-// Define Necessary.
-//#define OUTPUT_QUATERNION
-//#define OUTPUT_EULER
-#define OUTPUT_ROLL_PITCH_YAW
-//#define OUTPUT_FOR_TEAPOT
-//#define OUTPUT_TEMPERATURE
- 
+/*
+
+*/
 #include "MPU6050_6Axis_MotionApps20.h"
 #include "mbed.h"
+//#include "CANMsg.h"
+#include "I2Cdev.h"
+#include "MPU6050.h"
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/String.h>
+#include <std_msgs/Float32.h>
+#include <sensor_msgs/BatteryState.h>
+#include <geometry_msgs/Twist.h> //set buffer larger than 50byte
+#include <math.h>
 #include <stdio.h>
+#include <tiny_msgs/tinyVector.h>
+#include <tiny_msgs/tinyIMU.h>
 
-#define servotest
-#ifdef servotest
-PwmOut sv(P0_0);
-#endif
+#define Start 0xAA
+#define Address 0x7F
+#define ReturnType 0x00
+#define Clean 0x00
+#define Reserve 0x00
+#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
+DigitalOut          myled(LED1);
+MPU6050 mpu;//(PB_7,PB_6);     // sda, scl pin
 
-// FIFO rate = 200Hz / (1 + this value)
-// For example, 0x01 is 100Hz, 0x03 is 50Hz.
-// 0x00 to 0x09
+tiny_msgs::tinyIMU imu_msg;
+std_msgs::String str_msg;
+std_msgs::Float32 VelAngular_L;
+std_msgs::Float32 VelAngular_R;
+
+ros::NodeHandle  nh;
+ros::Publisher imu_pub("tinyImu", &imu_msg);
+ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
+ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
+
+uint32_t seq;
+
 #define IMU_FIFO_RATE_DIVIDER 0x09
- 
-// Sample rate = 1kHz / (1 + this valye)
-// For example, 4 is 200Hz.
 #define IMU_SAMPLE_RATE_DIVIDER 4
- 
-// measuring range of gyroscope (±n deg/s)
-// But other value doesn't yet support.
 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
- 
-// measuring range of acceleration sensor (±n g)
-// But other value doesn't yet support.
 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
  
-#define PC_BAUDRATE 921600
-            
-
-#include "USBSerial.h"
+#define PC_BAUDRATE 115200     
  
 #define DEG_TO_RAD(x) ( x * 0.01745329 )
 #define RAD_TO_DEG(x) ( x * 57.29578 )
- 
- 
-//RawSerial pc(USBTX, USBRX);
-USBSerial pc;
-MPU6050 mpu;     // sda, scl pin
-InterruptIn INT0(P0_9);     // INT0 pin
- 
+
 const int FIFO_BUFFER_SIZE = 128;
 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
 uint16_t fifoCount;
@@ -68,7 +63,16 @@
 const int snprintf_buffer_size = 100;
 char snprintf_buffer[snprintf_buffer_size];
 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
- 
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+  float Lrpm,Rrpm;
+    float ticks_since_target;
+    double timeout_ticks;
+
+    double w;
+    double rate;
+    double Dimeter;
+    float dx,dy,dr;
 struct Offset {
     int16_t ax, ay, az;
     int16_t gx, gy, gz;
@@ -83,40 +87,37 @@
 long map(long x, long in_min, long in_max, long out_min, long out_max) {
   return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
-
+//==========define sub function========================
 bool Init();
 void dmpDataUpdate();
- 
- 
-int main() {
-    MBED_ASSERT(Init() == true);
-    
-    while(1) {
-        
-    }
-}
- 
+unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
+void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data);
+void SVON(char MotorStation);
+void SVOFF(char MotorStation);
+int myabs( int a );
+void TwistToMotors();        
+//===================================================
  
 bool Init() {
     //pc.baud(PC_BAUDRATE);
-    
-    while(!pc.readable());
-            pc.getc();
+//    pc.printf("Init.\n");
+
+    seq = 0;
+    //while(!pc.readable());
+    //        pc.getc();
     
-    INT0.mode(PullDown);
-    INT0.fall(dmpDataUpdate);
-    
+        nh.initNode();
     mpu.initialize();
     if (mpu.testConnection()) {
-        pc.puts("MPU6050 test connection passed.\n");
+//        pc.printf("MPU6050 test connection passed.\n");
     } else {
-        pc.puts("MPU6050 test connection failed.\n");
+//        pc.printf("MPU6050 test connection failed.\n");
         return false;
     }
     if (mpu.dmpInitialize() == 0) {
-        pc.puts("succeed in MPU6050 DMP Initializing.\n");
+//        pc.printf("succeed in MPU6050 DMP Initializing.\n");
     } else {
-        pc.puts("failed in MPU6050 DMP Initializing.\n");
+//        pc.printf("failed in MPU6050 DMP Initializing.\n");
         return false;
     }
     mpu.setXAccelOffset(offset.ax);
@@ -131,16 +132,155 @@
     packetSize = mpu.dmpGetFIFOPacketSize();
     dmpReady = true;    // Enable interrupt.
     
-    pc.puts("Init finish!\n");
- 
- #ifdef servotest
-    sv.pulsewidth_us(1450);
- #endif
+    //pc.printf("Init finish!\n");
  
     return true;
 }
- 
- 
+//=======================     motor      =================================================
+unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
+{
+    unsigned int i, j;
+    //#define wPolynom 0xA001
+    unsigned int wCrc = 0xffff;
+    unsigned int wPolynom = 0xA001;
+    /*---------------------------------------------------------------------------------*/
+    for (i = 0; i < iBufLen; i++)
+    {
+        wCrc ^= cBuffer[i];
+        for (j = 0; j < 8; j++)
+        {
+            if (wCrc &0x0001)
+            {
+                wCrc = (wCrc >> 1) ^ wPolynom;
+            }
+            else
+            { 
+                wCrc = wCrc >> 1;
+            }
+        }
+    }
+    return wCrc;
+}
+   
+void setAddress(char MotorAddress,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;
+    //===========================================
+}   
+void SVON(char MotorStation)
+{
+    //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
+    setAddress(MotorStation,0x0214,0x0100);
+} 
+
+void SVOFF(char MotorStation)
+{
+    //void setAddress(char MotorAddress,int16_t DataAddress,int16_t Data)
+    setAddress(MotorStation,0x0214,0x0101);
+    wait_ms(10);
+} 
+int myabs( int a ){
+    if ( a < 0 ){
+        return -a;
+        }
+        return a;
+        }
+void TwistToMotors()
+{
+    seq = seq + 1;
+    //int lower_bound = 100;
+    //int upper_bound = 300;
+    float right,left;
+    float motor_rpm_r, motor_rpm_l;
+    //double vel_data[2];
+    int16_t Lrpm,Rrpm;
+    float vel_data[2];
+    w = 0.302;//0.2 ;//m
+    rate = 20;//50;
+    timeout_ticks = 2;
+    Dimeter = 0.127;//0.15;
+
+    // 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);
+    motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
+    motor_rpm_l =  left*rate/(Dimeter/2)*60/(2*3.1416);
+    vel_data[0] = motor_rpm_r;
+    vel_data[1] = motor_rpm_l;  
+
+    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];
+    //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;
+}
+        
+
+void messageCb(const geometry_msgs::Twist &msg)
+{  
+    ticks_since_target = 0;
+    dx = msg.linear.x;
+    dy = msg.linear.y;
+    dr = msg.angular.z;
+    TwistToMotors(); 
+}
+ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
+ //======================================================================================
+ //
 void dmpDataUpdate() {
     // Check that this interrupt has enabled.
     if (dmpReady == false) return;
@@ -151,7 +291,7 @@
     // Check that this interrupt is a FIFO buffer overflow interrupt.
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
         mpu.resetFIFO();
-        //pc.puts("FIFO overflow!\n");
+        //pc.printf("FIFO overflow!\n");
         return;
         
     // Check that this interrupt is a Data Ready interrupt.
@@ -163,7 +303,7 @@
     #ifdef OUTPUT_QUATERNION
         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
         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;
-        pc.puts(snprintf_buffer);
+        pc.printf(snprintf_buffer);
     #endif
         
     #ifdef OUTPUT_EULER
@@ -171,7 +311,7 @@
         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
         mpu.dmpGetEuler(euler, &dmpData.q);
         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;
-        pc.puts(snprintf_buffer);
+        pc.printf(snprintf_buffer);
     #endif
         
     #ifdef OUTPUT_ROLL_PITCH_YAW
@@ -184,7 +324,7 @@
         dmpData.yaw = rollPitchYaw[0];
         
         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;
-        pc.puts(snprintf_buffer);
+        pc.printf(snprintf_buffer);
                  
 #ifdef servotest
         int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
@@ -211,9 +351,42 @@
     #ifdef OUTPUT_TEMPERATURE
         float temp = mpu.getTemperature() / 340.0 + 36.53;
         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
-        pc.puts(snprintf_buffer);
+        pc.printf(snprintf_buffer);
     #endif
         
-        pc.puts("\n");
+//        pc.printf("\n");
+    }
+}
+
+int main() {
+    RS485.baud(PC_BAUDRATE);
+    MBED_ASSERT(Init() == true);
+    
+    nh.initNode();
+    nh.advertise(imu_pub);
+//    nh.advertise(pub_lmotor);
+//    nh.advertise(pub_rmotor);
+    nh.subscribe(cmd_vel_sub);
+    SVON(1);
+    SVON(2);
+    while(1) 
+    { 
+        seq++;
+        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 );
+        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);    
     }
 }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_kinetic.lib	Thu Apr 12 01:15:57 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tiny_msgs/tinyIMU.h	Thu Apr 12 01:15:57 2018 +0000
@@ -0,0 +1,55 @@
+#ifndef _ROS_tiny_msgs_tinyIMU_h
+#define _ROS_tiny_msgs_tinyIMU_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+#include "std_msgs/Header.h"
+#include "tinyVector.h"
+
+namespace tiny_msgs
+{
+
+  class tinyIMU : public ros::Msg
+  {
+    public:
+      typedef std_msgs::Header _header_type;
+      _header_type header;
+      typedef tiny_msgs::tinyVector _accel_type;
+      _accel_type accel;
+      typedef tiny_msgs::tinyVector _gyro_type;
+      _gyro_type gyro;
+
+    tinyIMU():
+      header(),
+      accel(),
+      gyro()
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      offset += this->header.serialize(outbuffer + offset);
+      offset += this->accel.serialize(outbuffer + offset);
+      offset += this->gyro.serialize(outbuffer + offset);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      offset += this->header.deserialize(inbuffer + offset);
+      offset += this->accel.deserialize(inbuffer + offset);
+      offset += this->gyro.deserialize(inbuffer + offset);
+     return offset;
+    }
+
+    const char * getType(){ return "tiny_msgs/tinyIMU"; };
+    const char * getMD5(){ return "53582bc8b7315f3bc7728d82df98bb24"; };
+
+  };
+
+}
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tiny_msgs/tinyVector.h	Thu Apr 12 01:15:57 2018 +0000
@@ -0,0 +1,98 @@
+#ifndef _ROS_tiny_msgs_tinyVector_h
+#define _ROS_tiny_msgs_tinyVector_h
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include "ros/msg.h"
+
+namespace tiny_msgs
+{
+
+  class tinyVector : public ros::Msg
+  {
+    public:
+      typedef int16_t _x_type;
+      _x_type x;
+      typedef int16_t _y_type;
+      _y_type y;
+      typedef int16_t _z_type;
+      _z_type z;
+
+    tinyVector():
+      x(0),
+      y(0),
+      z(0)
+    {
+    }
+
+    virtual int serialize(unsigned char *outbuffer) const
+    {
+      int offset = 0;
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_x;
+      u_x.real = this->x;
+      *(outbuffer + offset + 0) = (u_x.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_x.base >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->x);
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_y;
+      u_y.real = this->y;
+      *(outbuffer + offset + 0) = (u_y.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_y.base >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->y);
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_z;
+      u_z.real = this->z;
+      *(outbuffer + offset + 0) = (u_z.base >> (8 * 0)) & 0xFF;
+      *(outbuffer + offset + 1) = (u_z.base >> (8 * 1)) & 0xFF;
+      offset += sizeof(this->z);
+      return offset;
+    }
+
+    virtual int deserialize(unsigned char *inbuffer)
+    {
+      int offset = 0;
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_x;
+      u_x.base = 0;
+      u_x.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_x.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->x = u_x.real;
+      offset += sizeof(this->x);
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_y;
+      u_y.base = 0;
+      u_y.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_y.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->y = u_y.real;
+      offset += sizeof(this->y);
+      union {
+        int16_t real;
+        uint16_t base;
+      } u_z;
+      u_z.base = 0;
+      u_z.base |= ((uint16_t) (*(inbuffer + offset + 0))) << (8 * 0);
+      u_z.base |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1);
+      this->z = u_z.real;
+      offset += sizeof(this->z);
+     return offset;
+    }
+
+    const char * getType(){ return "tinyVector"; };
+    const char * getMD5(){ return "85729383565f7e059d4a213b3db1317b"; };
+
+  };
+
+}
+#endif
\ No newline at end of file