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@9:f9e9662d26bf, 2018-05-23 (annotated)

Committer:
WeberYang
Date:
Wed May 23 01:49:03 2018 +0000
Revision:
9:f9e9662d26bf
Parent:
8:4974fc24fbd7 
0523 test fork

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