For nucleoF103 change sda,scl to PB_7,PB_6 Finish Delta Servo control with RS485, and IMU6050

Dependencies:   MPU6050-DMP mbed ros_lib_kinetic

Fork of MPU6050_DMP_test_for1549 by takaaki mastuzawa

Files at this revision

API Documentation at this revision

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

MPU6050-DMP.lib Show annotated file Show diff for this revision Revisions of this file
USBDevice.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
ros_lib_kinetic.lib Show annotated file Show diff for this revision Revisions of this file
tiny_msgs/tinyIMU.h Show annotated file Show diff for this revision Revisions of this file
tiny_msgs/tinyVector.h Show annotated file Show diff for this revision Revisions of this file
diff -r 41ec5f98abab -r 648583d6e41a MPU6050-DMP.lib
--- 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
diff -r 41ec5f98abab -r 648583d6e41a USBDevice.lib
--- 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
diff -r 41ec5f98abab -r 648583d6e41a main.cpp
--- 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
diff -r 41ec5f98abab -r 648583d6e41a ros_lib_kinetic.lib
--- /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
diff -r 41ec5f98abab -r 648583d6e41a tiny_msgs/tinyIMU.h
--- /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
diff -r 41ec5f98abab -r 648583d6e41a tiny_msgs/tinyVector.h
--- /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