Weber Yang / Mbed 2 deprecated AGV_SMT_Sevro

Branch for servo motor

Dependencies:   MPU6050-DMP mbed ros_lib_kinetic

Fork of AGV_SMT by Weber Yang

main.cpp@4:b728b0359ec8, 2018-04-13 (annotated)

Committer:
WeberYang
Date:
Fri Apr 13 07:03:10 2018 +0000
Revision:
4:b728b0359ec8
Parent:
3:51194773aa7e
Child:
5:52fb31c1a8c0
add SVON

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