Weber Yang
start to work from here...
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of
AGV_0411
by 
Weber Yang
Diff: main.cpp
- Revision:
- 2:648583d6e41a
- Parent:
- 0:6e61e8ec4b42
- Child:
- 3:51194773aa7e
--- 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