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@8:4974fc24fbd7, 2018-05-15 (annotated)

Committer:
WeberYang
Date:
Tue May 15 00:46:30 2018 +0000
Revision:
8:4974fc24fbd7
Parent:
7:1a234f02746f
Child:
9:e10bd4b460d7
ROS with ; Can open for Battery monitor; RS232 for motor control; I2C for MPU6050; Two Do for Piston; One Di for Sensor;

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