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@7:1a234f02746f, 2018-05-10 (annotated)

Committer:
WeberYang
Date:
Thu May 10 08:16:14 2018 +0000
Revision:
7:1a234f02746f
Parent:
6:eadfb1b45bda
Child:
8:4974fc24fbd7
CAN,IMU,motor

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