Branch for servo motor
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of AGV_SMT by
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?
User | Revision | Line number | New 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 | } |