Weber Yang / Mbed 2 deprecated AGV_SMT

start to work from here...

Dependencies:   MPU6050-DMP mbed ros_lib_kinetic

Fork of AGV_0411 by Weber Yang

main.cpp@5:52fb31c1a8c0, 2018-04-17 (annotated)

Committer:
WeberYang
Date:
Tue Apr 17 07:59:09 2018 +0000
Revision:
5:52fb31c1a8c0
Parent:
4:b728b0359ec8
Child:
6:eadfb1b45bda
0417:motor contorl+ecoder+IMU

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WeberYang 2:648583d6e41a 1 /*
WeberYang 3:51194773aa7e 2 0412 combine the sevro motor encoder to publish
WeberYang 2:648583d6e41a 3 */
hardtail 0:6e61e8ec4b42 4 #include "MPU6050_6Axis_MotionApps20.h"
hardtail 0:6e61e8ec4b42 5 #include "mbed.h"
WeberYang 5:52fb31c1a8c0 6
WeberYang 2:648583d6e41a 7 //#include "CANMsg.h"
WeberYang 2:648583d6e41a 8 #include "I2Cdev.h"
WeberYang 2:648583d6e41a 9 #include "MPU6050.h"
WeberYang 2:648583d6e41a 10 #include <ros.h>
WeberYang 2:648583d6e41a 11 #include <ros/time.h>
WeberYang 2:648583d6e41a 12 #include <std_msgs/Empty.h>
WeberYang 2:648583d6e41a 13 #include <std_msgs/String.h>
WeberYang 2:648583d6e41a 14 #include <std_msgs/Float32.h>
WeberYang 2:648583d6e41a 15 #include <sensor_msgs/BatteryState.h>
WeberYang 2:648583d6e41a 16 #include <geometry_msgs/Twist.h> //set buffer larger than 50byte
WeberYang 2:648583d6e41a 17 #include <math.h>
hardtail 0:6e61e8ec4b42 18 #include <stdio.h>
WeberYang 2:648583d6e41a 19 #include <tiny_msgs/tinyVector.h>
WeberYang 2:648583d6e41a 20 #include <tiny_msgs/tinyIMU.h>
WeberYang 5:52fb31c1a8c0 21 #include <string>
WeberYang 5:52fb31c1a8c0 22 #include <cstdlib>
hardtail 0:6e61e8ec4b42 23
WeberYang 2:648583d6e41a 24 #define Start 0xAA
WeberYang 2:648583d6e41a 25 #define Address 0x7F
WeberYang 2:648583d6e41a 26 #define ReturnType 0x00
WeberYang 2:648583d6e41a 27 #define Clean 0x00
WeberYang 2:648583d6e41a 28 #define Reserve 0x00
WeberYang 2:648583d6e41a 29 #define End 0x55
WeberYang 2:648583d6e41a 30 #define Motor1 1
WeberYang 2:648583d6e41a 31 #define Motor2 2
WeberYang 5:52fb31c1a8c0 32 #define LENG 31 //0x42 + 31 bytes equal to 32 bytes
WeberYang 5:52fb31c1a8c0 33
WeberYang 5:52fb31c1a8c0 34 #define Write 0x06
WeberYang 5:52fb31c1a8c0 35 #define Read 0x03
WeberYang 5:52fb31c1a8c0 36 #define DI1 0x0214 //0214H means digital input DI1 for sevro on
WeberYang 5:52fb31c1a8c0 37 #define APR 0x0066 //0066H means encoder abs rev
WeberYang 5:52fb31c1a8c0 38 #define SP1 0x0112
WeberYang 5:52fb31c1a8c0 39 //Serial pc(USBTX,USBRX);
WeberYang 5:52fb31c1a8c0 40 Serial rs485(PA_9, PA_10);
WeberYang 5:52fb31c1a8c0 41 DigitalOut Receiver(D7); //RS485_E
WeberYang 2:648583d6e41a 42 DigitalOut myled(LED1);
WeberYang 3:51194773aa7e 43 Ticker CheckDataR;
WeberYang 3:51194773aa7e 44
WeberYang 2:648583d6e41a 45 MPU6050 mpu;//(PB_7,PB_6); // sda, scl pin
hardtail 0:6e61e8ec4b42 46
WeberYang 2:648583d6e41a 47 tiny_msgs::tinyIMU imu_msg;
WeberYang 2:648583d6e41a 48 std_msgs::String str_msg;
WeberYang 2:648583d6e41a 49 std_msgs::Float32 VelAngular_L;
WeberYang 2:648583d6e41a 50 std_msgs::Float32 VelAngular_R;
WeberYang 2:648583d6e41a 51
WeberYang 2:648583d6e41a 52 ros::NodeHandle nh;
WeberYang 2:648583d6e41a 53 ros::Publisher imu_pub("tinyImu", &imu_msg);
WeberYang 2:648583d6e41a 54 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
WeberYang 2:648583d6e41a 55 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
WeberYang 2:648583d6e41a 56
WeberYang 2:648583d6e41a 57 uint32_t seq;
WeberYang 2:648583d6e41a 58
hardtail 0:6e61e8ec4b42 59 #define IMU_FIFO_RATE_DIVIDER 0x09
hardtail 0:6e61e8ec4b42 60 #define IMU_SAMPLE_RATE_DIVIDER 4
hardtail 0:6e61e8ec4b42 61 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
hardtail 0:6e61e8ec4b42 62 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
hardtail 0:6e61e8ec4b42 63
WeberYang 5:52fb31c1a8c0 64 #define PC_BAUDRATE 38400
hardtail 0:6e61e8ec4b42 65
hardtail 0:6e61e8ec4b42 66 #define DEG_TO_RAD(x) ( x * 0.01745329 )
hardtail 0:6e61e8ec4b42 67 #define RAD_TO_DEG(x) ( x * 57.29578 )
WeberYang 2:648583d6e41a 68
hardtail 0:6e61e8ec4b42 69 const int FIFO_BUFFER_SIZE = 128;
hardtail 0:6e61e8ec4b42 70 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
hardtail 0:6e61e8ec4b42 71 uint16_t fifoCount;
hardtail 0:6e61e8ec4b42 72 uint16_t packetSize;
hardtail 0:6e61e8ec4b42 73 bool dmpReady;
hardtail 0:6e61e8ec4b42 74 uint8_t mpuIntStatus;
hardtail 0:6e61e8ec4b42 75 const int snprintf_buffer_size = 100;
hardtail 0:6e61e8ec4b42 76 char snprintf_buffer[snprintf_buffer_size];
hardtail 0:6e61e8ec4b42 77 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
WeberYang 2:648583d6e41a 78 int16_t ax, ay, az;
WeberYang 2:648583d6e41a 79 int16_t gx, gy, gz;
WeberYang 2:648583d6e41a 80 float Lrpm,Rrpm;
WeberYang 2:648583d6e41a 81 float ticks_since_target;
WeberYang 2:648583d6e41a 82 double timeout_ticks;
WeberYang 2:648583d6e41a 83
WeberYang 2:648583d6e41a 84 double w;
WeberYang 2:648583d6e41a 85 double rate;
WeberYang 2:648583d6e41a 86 double Dimeter;
WeberYang 2:648583d6e41a 87 float dx,dy,dr;
WeberYang 3:51194773aa7e 88
WeberYang 3:51194773aa7e 89 int buffer[9] = {0};
WeberYang 5:52fb31c1a8c0 90 int dataH,datanum;
WeberYang 3:51194773aa7e 91 //=========RS485
WeberYang 3:51194773aa7e 92 char recChar=0;
WeberYang 3:51194773aa7e 93 bool recFlag=false;
WeberYang 3:51194773aa7e 94 char recArr[20];
WeberYang 3:51194773aa7e 95 int index=0;
WeberYang 3:51194773aa7e 96
hardtail 0:6e61e8ec4b42 97 struct Offset {
hardtail 0:6e61e8ec4b42 98 int16_t ax, ay, az;
hardtail 0:6e61e8ec4b42 99 int16_t gx, gy, gz;
hardtail 0:6e61e8ec4b42 100 }offset = {150, -350, 1000, -110, 5, 0}; // Measured values
hardtail 0:6e61e8ec4b42 101
hardtail 0:6e61e8ec4b42 102 struct MPU6050_DmpData {
hardtail 0:6e61e8ec4b42 103 Quaternion q;
hardtail 0:6e61e8ec4b42 104 VectorFloat gravity; // g
hardtail 0:6e61e8ec4b42 105 float roll, pitch, yaw; // rad
hardtail 0:6e61e8ec4b42 106 }dmpData;
hardtail 0:6e61e8ec4b42 107
hardtail 0:6e61e8ec4b42 108 long map(long x, long in_min, long in_max, long out_min, long out_max) {
hardtail 0:6e61e8ec4b42 109 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
hardtail 0:6e61e8ec4b42 110 }
WeberYang 2:648583d6e41a 111 //==========define sub function========================
hardtail 0:6e61e8ec4b42 112 bool Init();
hardtail 0:6e61e8ec4b42 113 void dmpDataUpdate();
WeberYang 2:648583d6e41a 114 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
WeberYang 5:52fb31c1a8c0 115 void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data);
WeberYang 3:51194773aa7e 116 void ReadAddress(char MotorAddress,int16_t DataAddress);
WeberYang 2:648583d6e41a 117 void SVON(char MotorStation);
WeberYang 3:51194773aa7e 118 void ReadENC(char MotorStation);
WeberYang 2:648583d6e41a 119 void SVOFF(char MotorStation);
WeberYang 2:648583d6e41a 120 int myabs( int a );
WeberYang 3:51194773aa7e 121 void TwistToMotors();
WeberYang 3:51194773aa7e 122 void flushSerialBuffer();
WeberYang 3:51194773aa7e 123 void readData();
WeberYang 2:648583d6e41a 124 //===================================================
hardtail 0:6e61e8ec4b42 125
WeberYang 2:648583d6e41a 126
WeberYang 2:648583d6e41a 127 //======================= motor =================================================
WeberYang 2:648583d6e41a 128 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
WeberYang 2:648583d6e41a 129 {
WeberYang 2:648583d6e41a 130 unsigned int i, j;
WeberYang 2:648583d6e41a 131 //#define wPolynom 0xA001
WeberYang 2:648583d6e41a 132 unsigned int wCrc = 0xffff;
WeberYang 2:648583d6e41a 133 unsigned int wPolynom = 0xA001;
WeberYang 2:648583d6e41a 134 /*---------------------------------------------------------------------------------*/
WeberYang 2:648583d6e41a 135 for (i = 0; i < iBufLen; i++)
WeberYang 2:648583d6e41a 136 {
WeberYang 2:648583d6e41a 137 wCrc ^= cBuffer[i];
WeberYang 2:648583d6e41a 138 for (j = 0; j < 8; j++)
WeberYang 2:648583d6e41a 139 {
WeberYang 2:648583d6e41a 140 if (wCrc &0x0001)
WeberYang 2:648583d6e41a 141 {
WeberYang 2:648583d6e41a 142 wCrc = (wCrc >> 1) ^ wPolynom;
WeberYang 2:648583d6e41a 143 }
WeberYang 2:648583d6e41a 144 else
WeberYang 2:648583d6e41a 145 {
WeberYang 2:648583d6e41a 146 wCrc = wCrc >> 1;
WeberYang 2:648583d6e41a 147 }
WeberYang 2:648583d6e41a 148 }
WeberYang 2:648583d6e41a 149 }
WeberYang 2:648583d6e41a 150 return wCrc;
WeberYang 2:648583d6e41a 151 }
WeberYang 2:648583d6e41a 152
WeberYang 5:52fb31c1a8c0 153 void setAddress(char MotorAddress,char func,int16_t DataAddress,int16_t Data)
WeberYang 2:648583d6e41a 154 {
WeberYang 5:52fb31c1a8c0 155 unsigned char sendData[16];
WeberYang 5:52fb31c1a8c0 156 // int16_t tmpCRC;
WeberYang 5:52fb31c1a8c0 157 // char DataAddressH,DataAddressL;
WeberYang 3:51194773aa7e 158 // char dataH,dataL;
WeberYang 3:51194773aa7e 159 int i;
WeberYang 5:52fb31c1a8c0 160 Receiver = 1;
WeberYang 5:52fb31c1a8c0 161 wait_ms(1);
WeberYang 3:51194773aa7e 162 sendData[0] = MotorAddress;
WeberYang 5:52fb31c1a8c0 163 sendData[1] = func;
WeberYang 5:52fb31c1a8c0 164 sendData[2] = ((DataAddress>>8)&0xFF);
WeberYang 5:52fb31c1a8c0 165 sendData[3] = ((DataAddress>>0)&0xFF);
WeberYang 5:52fb31c1a8c0 166 sendData[4] = ((Data>>8)&0xFF);
WeberYang 5:52fb31c1a8c0 167 sendData[5] = ((Data>>0)&0xFF);
WeberYang 5:52fb31c1a8c0 168 sendData[6] = sendData[0]+sendData[1]+sendData[2]+sendData[3]+sendData[4]+sendData[5];
WeberYang 5:52fb31c1a8c0 169 sendData[6] = (~sendData[6]+1);
WeberYang 5:52fb31c1a8c0 170 rs485.printf(":");
WeberYang 5:52fb31c1a8c0 171 for (i=0;i<7;i++)
WeberYang 3:51194773aa7e 172 {
WeberYang 5:52fb31c1a8c0 173 rs485.printf("%02X",sendData[i]);
WeberYang 3:51194773aa7e 174 }
WeberYang 5:52fb31c1a8c0 175 wait_ms(1);
WeberYang 5:52fb31c1a8c0 176 Receiver = 0;
WeberYang 5:52fb31c1a8c0 177 wait_ms(10); //give 10ms let receive complete.
WeberYang 5:52fb31c1a8c0 178 }
WeberYang 5:52fb31c1a8c0 179
WeberYang 2:648583d6e41a 180 void SVON(char MotorStation)
WeberYang 2:648583d6e41a 181 {
WeberYang 5:52fb31c1a8c0 182 //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData)
WeberYang 5:52fb31c1a8c0 183 setAddress(MotorStation,Write,DI1,0x0001);
WeberYang 2:648583d6e41a 184 }
WeberYang 2:648583d6e41a 185
WeberYang 2:648583d6e41a 186 void SVOFF(char MotorStation)
WeberYang 2:648583d6e41a 187 {
WeberYang 5:52fb31c1a8c0 188 //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t setData)
WeberYang 5:52fb31c1a8c0 189 setAddress(MotorStation,Write,DI1,0x0101);
WeberYang 2:648583d6e41a 190 wait_ms(10);
WeberYang 2:648583d6e41a 191 }
WeberYang 3:51194773aa7e 192 void ReadENC(char MotorStation)
WeberYang 3:51194773aa7e 193 {
WeberYang 5:52fb31c1a8c0 194 //void setAddress(char MotorAddress,char function, int16_t DataAddress,int16_t readDataNum)
WeberYang 5:52fb31c1a8c0 195 setAddress(MotorStation,Read,APR,0x0001);
WeberYang 3:51194773aa7e 196 wait_ms(10);
WeberYang 3:51194773aa7e 197
WeberYang 3:51194773aa7e 198 }
WeberYang 2:648583d6e41a 199 int myabs( int a ){
WeberYang 2:648583d6e41a 200 if ( a < 0 ){
WeberYang 2:648583d6e41a 201 return -a;
WeberYang 2:648583d6e41a 202 }
WeberYang 2:648583d6e41a 203 return a;
WeberYang 2:648583d6e41a 204 }
WeberYang 2:648583d6e41a 205 void TwistToMotors()
WeberYang 2:648583d6e41a 206 {
WeberYang 2:648583d6e41a 207 seq = seq + 1;
WeberYang 2:648583d6e41a 208 //int lower_bound = 100;
WeberYang 2:648583d6e41a 209 //int upper_bound = 300;
WeberYang 2:648583d6e41a 210 float right,left;
WeberYang 2:648583d6e41a 211 float motor_rpm_r, motor_rpm_l;
WeberYang 2:648583d6e41a 212 //double vel_data[2];
WeberYang 2:648583d6e41a 213 int16_t Lrpm,Rrpm;
WeberYang 2:648583d6e41a 214 float vel_data[2];
WeberYang 2:648583d6e41a 215 w = 0.302;//0.2 ;//m
WeberYang 2:648583d6e41a 216 rate = 20;//50;
WeberYang 2:648583d6e41a 217 timeout_ticks = 2;
WeberYang 2:648583d6e41a 218 Dimeter = 0.127;//0.15;
WeberYang 2:648583d6e41a 219
WeberYang 2:648583d6e41a 220 // prevent agv receive weird 1.0 command from cmd_vel
WeberYang 2:648583d6e41a 221 if (dr == 1.0){
WeberYang 2:648583d6e41a 222
WeberYang 2:648583d6e41a 223 dr = 0.001;
WeberYang 2:648583d6e41a 224 }
WeberYang 2:648583d6e41a 225
WeberYang 2:648583d6e41a 226 //
WeberYang 2:648583d6e41a 227 right = ( 1.0 * dx ) + (dr * w /2);
WeberYang 2:648583d6e41a 228 left = ( 1.0 * dx ) - (dr * w /2);
WeberYang 2:648583d6e41a 229 motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 2:648583d6e41a 230 motor_rpm_l = left*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 2:648583d6e41a 231 vel_data[0] = motor_rpm_r;
WeberYang 2:648583d6e41a 232 vel_data[1] = motor_rpm_l;
WeberYang 2:648583d6e41a 233
WeberYang 2:648583d6e41a 234 Lrpm = motor_rpm_l;
WeberYang 2:648583d6e41a 235 Rrpm = motor_rpm_r;
WeberYang 2:648583d6e41a 236
WeberYang 5:52fb31c1a8c0 237 // setAddress(Motor1,Write,0x0112,Lrpm);
WeberYang 5:52fb31c1a8c0 238 // setAddress(Motor2,Write,0x0112,Rrpm);
WeberYang 3:51194773aa7e 239
WeberYang 2:648583d6e41a 240 ticks_since_target += 1;
WeberYang 2:648583d6e41a 241 }
WeberYang 5:52fb31c1a8c0 242
WeberYang 5:52fb31c1a8c0 243 int str2int(const char* str, int star, int end)
WeberYang 5:52fb31c1a8c0 244 {
WeberYang 5:52fb31c1a8c0 245 int i;
WeberYang 5:52fb31c1a8c0 246 int ret = 0;
WeberYang 5:52fb31c1a8c0 247 for (i = star; i < end+1; i++)
WeberYang 5:52fb31c1a8c0 248 {
WeberYang 5:52fb31c1a8c0 249 ret = ret *10 + (str[i] - '0');
WeberYang 5:52fb31c1a8c0 250 }
WeberYang 5:52fb31c1a8c0 251 return ret;
WeberYang 5:52fb31c1a8c0 252 }
WeberYang 2:648583d6e41a 253
WeberYang 2:648583d6e41a 254 void messageCb(const geometry_msgs::Twist &msg)
WeberYang 2:648583d6e41a 255 {
WeberYang 2:648583d6e41a 256 ticks_since_target = 0;
WeberYang 2:648583d6e41a 257 dx = msg.linear.x;
WeberYang 2:648583d6e41a 258 dy = msg.linear.y;
WeberYang 2:648583d6e41a 259 dr = msg.angular.z;
WeberYang 2:648583d6e41a 260 TwistToMotors();
WeberYang 5:52fb31c1a8c0 261 //ReadENC(Motor1);
WeberYang 2:648583d6e41a 262 }
WeberYang 2:648583d6e41a 263 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
WeberYang 2:648583d6e41a 264 //======================================================================================
WeberYang 2:648583d6e41a 265 //
hardtail 0:6e61e8ec4b42 266 void dmpDataUpdate() {
hardtail 0:6e61e8ec4b42 267 // Check that this interrupt has enabled.
hardtail 0:6e61e8ec4b42 268 if (dmpReady == false) return;
hardtail 0:6e61e8ec4b42 269
hardtail 0:6e61e8ec4b42 270 mpuIntStatus = mpu.getIntStatus();
hardtail 0:6e61e8ec4b42 271 fifoCount = mpu.getFIFOCount();
hardtail 0:6e61e8ec4b42 272
hardtail 0:6e61e8ec4b42 273 // Check that this interrupt is a FIFO buffer overflow interrupt.
hardtail 0:6e61e8ec4b42 274 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
hardtail 0:6e61e8ec4b42 275 mpu.resetFIFO();
WeberYang 2:648583d6e41a 276 //pc.printf("FIFO overflow!\n");
hardtail 0:6e61e8ec4b42 277 return;
hardtail 0:6e61e8ec4b42 278
hardtail 0:6e61e8ec4b42 279 // Check that this interrupt is a Data Ready interrupt.
hardtail 0:6e61e8ec4b42 280 } else if (mpuIntStatus & 0x02) {
hardtail 0:6e61e8ec4b42 281 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
hardtail 0:6e61e8ec4b42 282
hardtail 0:6e61e8ec4b42 283 mpu.getFIFOBytes(fifoBuffer, packetSize);
hardtail 0:6e61e8ec4b42 284
hardtail 0:6e61e8ec4b42 285 #ifdef OUTPUT_QUATERNION
hardtail 0:6e61e8ec4b42 286 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 287 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;
WeberYang 2:648583d6e41a 288 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 289 #endif
hardtail 0:6e61e8ec4b42 290
hardtail 0:6e61e8ec4b42 291 #ifdef OUTPUT_EULER
hardtail 0:6e61e8ec4b42 292 float euler[3];
hardtail 0:6e61e8ec4b42 293 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 294 mpu.dmpGetEuler(euler, &dmpData.q);
hardtail 0:6e61e8ec4b42 295 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;
WeberYang 2:648583d6e41a 296 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 297 #endif
hardtail 0:6e61e8ec4b42 298
hardtail 0:6e61e8ec4b42 299 #ifdef OUTPUT_ROLL_PITCH_YAW
hardtail 0:6e61e8ec4b42 300 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 301 mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
hardtail 0:6e61e8ec4b42 302 float rollPitchYaw[3];
hardtail 0:6e61e8ec4b42 303 mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
hardtail 0:6e61e8ec4b42 304 dmpData.roll = rollPitchYaw[2];
hardtail 0:6e61e8ec4b42 305 dmpData.pitch = rollPitchYaw[1];
hardtail 0:6e61e8ec4b42 306 dmpData.yaw = rollPitchYaw[0];
hardtail 0:6e61e8ec4b42 307
hardtail 0:6e61e8ec4b42 308 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;
WeberYang 2:648583d6e41a 309 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 310
hardtail 0:6e61e8ec4b42 311 #ifdef servotest
hardtail 0:6e61e8ec4b42 312 int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
hardtail 0:6e61e8ec4b42 313 if(servoPulse > 1450) servoPulse = 1450;
hardtail 0:6e61e8ec4b42 314 if(servoPulse < 500) servoPulse = 500;
hardtail 0:6e61e8ec4b42 315 sv.pulsewidth_us(servoPulse);
hardtail 0:6e61e8ec4b42 316 #endif
hardtail 0:6e61e8ec4b42 317 #endif
hardtail 0:6e61e8ec4b42 318
hardtail 0:6e61e8ec4b42 319 #ifdef OUTPUT_FOR_TEAPOT
hardtail 0:6e61e8ec4b42 320 teapotPacket[2] = fifoBuffer[0];
hardtail 0:6e61e8ec4b42 321 teapotPacket[3] = fifoBuffer[1];
hardtail 0:6e61e8ec4b42 322 teapotPacket[4] = fifoBuffer[4];
hardtail 0:6e61e8ec4b42 323 teapotPacket[5] = fifoBuffer[5];
hardtail 0:6e61e8ec4b42 324 teapotPacket[6] = fifoBuffer[8];
hardtail 0:6e61e8ec4b42 325 teapotPacket[7] = fifoBuffer[9];
hardtail 0:6e61e8ec4b42 326 teapotPacket[8] = fifoBuffer[12];
hardtail 0:6e61e8ec4b42 327 teapotPacket[9] = fifoBuffer[13];
hardtail 0:6e61e8ec4b42 328 for (uint8_t i = 0; i < 14; i++) {
hardtail 0:6e61e8ec4b42 329 pc.putc(teapotPacket[i]);
hardtail 0:6e61e8ec4b42 330 }
hardtail 0:6e61e8ec4b42 331 #endif
hardtail 0:6e61e8ec4b42 332
hardtail 0:6e61e8ec4b42 333 #ifdef OUTPUT_TEMPERATURE
hardtail 0:6e61e8ec4b42 334 float temp = mpu.getTemperature() / 340.0 + 36.53;
hardtail 0:6e61e8ec4b42 335 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
WeberYang 2:648583d6e41a 336 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 337 #endif
hardtail 0:6e61e8ec4b42 338
WeberYang 2:648583d6e41a 339 // pc.printf("\n");
WeberYang 2:648583d6e41a 340 }
WeberYang 2:648583d6e41a 341 }
WeberYang 2:648583d6e41a 342
WeberYang 5:52fb31c1a8c0 343 //================rs485=======================================
WeberYang 3:51194773aa7e 344 void flushSerialBuffer() {
WeberYang 3:51194773aa7e 345
WeberYang 5:52fb31c1a8c0 346 while (rs485.readable()) {
WeberYang 5:52fb31c1a8c0 347 rs485.getc();
WeberYang 3:51194773aa7e 348 }
WeberYang 3:51194773aa7e 349 }
WeberYang 5:52fb31c1a8c0 350 void readData()
WeberYang 5:52fb31c1a8c0 351 {
WeberYang 5:52fb31c1a8c0 352 while (rs485.readable())
WeberYang 5:52fb31c1a8c0 353 {
WeberYang 5:52fb31c1a8c0 354 recChar = rs485.getc();
WeberYang 5:52fb31c1a8c0 355 recArr[index] = recChar;
WeberYang 5:52fb31c1a8c0 356 index++;
WeberYang 5:52fb31c1a8c0 357 if ((recChar == '\r') || (index>18))
WeberYang 5:52fb31c1a8c0 358 {
WeberYang 5:52fb31c1a8c0 359 datanum = str2int(recArr,1,2);
WeberYang 5:52fb31c1a8c0 360 if (datanum == 1)
WeberYang 5:52fb31c1a8c0 361 {
WeberYang 5:52fb31c1a8c0 362 dataH = str2int(recArr,7,10);
WeberYang 5:52fb31c1a8c0 363 //pc.printf("M1 = %d\r\n",dataH);
WeberYang 5:52fb31c1a8c0 364 VelAngular_R.data = dataH;
WeberYang 5:52fb31c1a8c0 365 }
WeberYang 5:52fb31c1a8c0 366 else
WeberYang 5:52fb31c1a8c0 367 {
WeberYang 5:52fb31c1a8c0 368 // pc.printf("M1 = no data\r\n");
WeberYang 5:52fb31c1a8c0 369 VelAngular_R.data = 0;
WeberYang 5:52fb31c1a8c0 370 }
WeberYang 5:52fb31c1a8c0 371
WeberYang 5:52fb31c1a8c0 372 if (datanum == 2)
WeberYang 5:52fb31c1a8c0 373 {
WeberYang 5:52fb31c1a8c0 374 dataH = str2int(recArr,7,10);
WeberYang 5:52fb31c1a8c0 375 VelAngular_L.data = dataH;
WeberYang 5:52fb31c1a8c0 376 }
WeberYang 5:52fb31c1a8c0 377 else
WeberYang 5:52fb31c1a8c0 378 {
WeberYang 5:52fb31c1a8c0 379 // pc.printf("M2 = no data\r\n");
WeberYang 5:52fb31c1a8c0 380 VelAngular_L.data = 0;
WeberYang 5:52fb31c1a8c0 381 }
WeberYang 5:52fb31c1a8c0 382
WeberYang 5:52fb31c1a8c0 383 pub_rmotor.publish( &VelAngular_R);
WeberYang 5:52fb31c1a8c0 384 pub_lmotor.publish( &VelAngular_L);
WeberYang 5:52fb31c1a8c0 385
WeberYang 5:52fb31c1a8c0 386 recArr[index] = 0;
WeberYang 5:52fb31c1a8c0 387 index = 0;
WeberYang 5:52fb31c1a8c0 388 //pc.printf("%s\r\n", recArr);
WeberYang 5:52fb31c1a8c0 389
WeberYang 5:52fb31c1a8c0 390 flushSerialBuffer();
WeberYang 5:52fb31c1a8c0 391 recFlag = true;
WeberYang 5:52fb31c1a8c0 392 }
WeberYang 5:52fb31c1a8c0 393 }
WeberYang 3:51194773aa7e 394 }
WeberYang 3:51194773aa7e 395 //=======================================================
WeberYang 5:52fb31c1a8c0 396 bool Init() {
WeberYang 5:52fb31c1a8c0 397 seq = 0;
WeberYang 2:648583d6e41a 398 nh.initNode();
WeberYang 2:648583d6e41a 399 nh.advertise(imu_pub);
WeberYang 5:52fb31c1a8c0 400 nh.advertise(pub_lmotor);
WeberYang 5:52fb31c1a8c0 401 nh.advertise(pub_rmotor);
WeberYang 2:648583d6e41a 402 nh.subscribe(cmd_vel_sub);
WeberYang 5:52fb31c1a8c0 403 mpu.initialize();
WeberYang 5:52fb31c1a8c0 404 if (mpu.testConnection()) {
WeberYang 5:52fb31c1a8c0 405 // pc.printf("MPU6050 test connection passed.\n");
WeberYang 5:52fb31c1a8c0 406 } else {
WeberYang 5:52fb31c1a8c0 407 // pc.printf("MPU6050 test connection failed.\n");
WeberYang 5:52fb31c1a8c0 408 return false;
WeberYang 5:52fb31c1a8c0 409 }
WeberYang 5:52fb31c1a8c0 410 if (mpu.dmpInitialize() == 0) {
WeberYang 5:52fb31c1a8c0 411 // pc.printf("succeed in MPU6050 DMP Initializing.\n");
WeberYang 5:52fb31c1a8c0 412 } else {
WeberYang 5:52fb31c1a8c0 413 // pc.printf("failed in MPU6050 DMP Initializing.\n");
WeberYang 5:52fb31c1a8c0 414 return false;
WeberYang 5:52fb31c1a8c0 415 }
WeberYang 5:52fb31c1a8c0 416 mpu.setXAccelOffset(offset.ax);
WeberYang 5:52fb31c1a8c0 417 mpu.setYAccelOffset(offset.ay);
WeberYang 5:52fb31c1a8c0 418 mpu.setZAccelOffset(offset.az);
WeberYang 5:52fb31c1a8c0 419 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
WeberYang 5:52fb31c1a8c0 420 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
WeberYang 5:52fb31c1a8c0 421 mpu.setXGyroOffsetUser(offset.gx);
WeberYang 5:52fb31c1a8c0 422 mpu.setYGyroOffsetUser(offset.gy);
WeberYang 5:52fb31c1a8c0 423 mpu.setZGyroOffsetUser(offset.gz);
WeberYang 5:52fb31c1a8c0 424 mpu.setDMPEnabled(true); // Enable DMP
WeberYang 5:52fb31c1a8c0 425 packetSize = mpu.dmpGetFIFOPacketSize();
WeberYang 5:52fb31c1a8c0 426 dmpReady = true; // Enable interrupt.
WeberYang 5:52fb31c1a8c0 427
WeberYang 5:52fb31c1a8c0 428 //pc.printf("Init finish!\n");
WeberYang 5:52fb31c1a8c0 429
WeberYang 5:52fb31c1a8c0 430 return true;
WeberYang 5:52fb31c1a8c0 431 }
WeberYang 5:52fb31c1a8c0 432 //=======================================================
WeberYang 5:52fb31c1a8c0 433 int main() {
WeberYang 5:52fb31c1a8c0 434 rs485.baud(PC_BAUDRATE);
WeberYang 5:52fb31c1a8c0 435 MBED_ASSERT(Init() == true);
WeberYang 5:52fb31c1a8c0 436
WeberYang 4:b728b0359ec8 437 SVON(1);
WeberYang 4:b728b0359ec8 438 SVON(2);
WeberYang 3:51194773aa7e 439 flushSerialBuffer();
WeberYang 5:52fb31c1a8c0 440 rs485.attach(&readData);//,0.25);
WeberYang 2:648583d6e41a 441 while(1)
WeberYang 4:b728b0359ec8 442 {
WeberYang 2:648583d6e41a 443 seq++;
WeberYang 2:648583d6e41a 444 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
WeberYang 2:648583d6e41a 445
WeberYang 2:648583d6e41a 446 imu_msg.header.stamp = nh.now();
WeberYang 2:648583d6e41a 447 imu_msg.header.frame_id = 0;
WeberYang 2:648583d6e41a 448 imu_msg.header.seq = seq;
WeberYang 2:648583d6e41a 449 imu_msg.accel.x = ax;
WeberYang 2:648583d6e41a 450 imu_msg.accel.y = ay;
WeberYang 2:648583d6e41a 451 imu_msg.accel.z = az;
WeberYang 2:648583d6e41a 452 imu_msg.gyro.x = gx;
WeberYang 2:648583d6e41a 453 imu_msg.gyro.y = gy;
WeberYang 2:648583d6e41a 454 imu_msg.gyro.z = gz;
WeberYang 2:648583d6e41a 455 imu_pub.publish( &imu_msg );
WeberYang 2:648583d6e41a 456 nh.spinOnce();
WeberYang 5:52fb31c1a8c0 457 setAddress(Motor1,Read,APR,0x0001);
WeberYang 5:52fb31c1a8c0 458 setAddress(Motor2,Read,APR,0x0001);
WeberYang 5:52fb31c1a8c0 459 setAddress(Motor1,Write,SP1,Lrpm);
WeberYang 5:52fb31c1a8c0 460 setAddress(Motor2,Write,SP1,Rrpm);
WeberYang 2:648583d6e41a 461 wait_ms(10);
WeberYang 3:51194773aa7e 462
hardtail 0:6e61e8ec4b42 463 }
hardtail 0:6e61e8ec4b42 464 }