Delta Battery, CAN_bus, DIO P2P, ROS
Dependencies: mbed mbed-rtos ros_lib_kinetic
main.cpp@3:51194773aa7e, 2018-04-13 (annotated)
- Committer:
- WeberYang
- Date:
- Fri Apr 13 06:58:49 2018 +0000
- Revision:
- 3:51194773aa7e
- Parent:
- 2:648583d6e41a
- Child:
- 4:b728b0359ec8
AGV
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 | 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 | 3:51194773aa7e | 451 | // SVON(1); |
WeberYang | 3:51194773aa7e | 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 | 2:648583d6e41a | 457 | { |
WeberYang | 3:51194773aa7e | 458 | //================================== |
WeberYang | 3:51194773aa7e | 459 | |
WeberYang | 3:51194773aa7e | 460 | //============================= |
WeberYang | 3:51194773aa7e | 461 | |
WeberYang | 2:648583d6e41a | 462 | seq++; |
WeberYang | 2:648583d6e41a | 463 | mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
WeberYang | 2:648583d6e41a | 464 | |
WeberYang | 2:648583d6e41a | 465 | imu_msg.header.stamp = nh.now(); |
WeberYang | 2:648583d6e41a | 466 | imu_msg.header.frame_id = 0; |
WeberYang | 2:648583d6e41a | 467 | imu_msg.header.seq = seq; |
WeberYang | 2:648583d6e41a | 468 | imu_msg.accel.x = ax; |
WeberYang | 2:648583d6e41a | 469 | imu_msg.accel.y = ay; |
WeberYang | 2:648583d6e41a | 470 | imu_msg.accel.z = az; |
WeberYang | 2:648583d6e41a | 471 | imu_msg.gyro.x = gx; |
WeberYang | 2:648583d6e41a | 472 | imu_msg.gyro.y = gy; |
WeberYang | 2:648583d6e41a | 473 | imu_msg.gyro.z = gz; |
WeberYang | 2:648583d6e41a | 474 | imu_pub.publish( &imu_msg ); |
WeberYang | 2:648583d6e41a | 475 | nh.spinOnce(); |
WeberYang | 2:648583d6e41a | 476 | wait_ms(10); |
WeberYang | 2:648583d6e41a | 477 | //writing current accelerometer and gyro position |
WeberYang | 3:51194773aa7e | 478 | //pc.printf("%d;%d;%d;%d;%d;%d\r\n",ax,ay,az,gx,gy,gz); |
WeberYang | 3:51194773aa7e | 479 | |
WeberYang | 3:51194773aa7e | 480 | // RS485.attach(&onInterrupt,Serial::RxIrq); |
WeberYang | 3:51194773aa7e | 481 | |
hardtail | 0:6e61e8ec4b42 | 482 | } |
hardtail | 0:6e61e8ec4b42 | 483 | } |