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

Committer:
WeberYang
Date:
Wed May 23 01:54:11 2018 +0000
Revision:
9:e10bd4b460d7
Parent:
8:4974fc24fbd7
Child:
10:8b7fce3bba86
Demo program

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 9:e10bd4b460d7 43 #define IDLE 0
WeberYang 9:e10bd4b460d7 44 #define ACT_MG_ON 1
WeberYang 9:e10bd4b460d7 45 #define ACT_MG_OFF 2
WeberYang 9:e10bd4b460d7 46 #define Check_BMS_ON 3
WeberYang 9:e10bd4b460d7 47 #define Check_BMS_OFF 4
WeberYang 5:52fb31c1a8c0 48 //Serial pc(USBTX,USBRX);
WeberYang 6:eadfb1b45bda 49 Timer t;
WeberYang 6:eadfb1b45bda 50 Serial RS232(PA_9, PA_10);
WeberYang 5:52fb31c1a8c0 51 DigitalOut Receiver(D7); //RS485_E
WeberYang 6:eadfb1b45bda 52 DigitalOut CAN_T(D14);
WeberYang 6:eadfb1b45bda 53 DigitalOut CAN_R(D15);
WeberYang 9:e10bd4b460d7 54 DigitalOut DO_0(PC_5);
WeberYang 9:e10bd4b460d7 55 DigitalOut DO_1(PC_6);
WeberYang 9:e10bd4b460d7 56 DigitalOut DO_2(PC_8);
WeberYang 9:e10bd4b460d7 57 DigitalOut DO_3(PC_9);
WeberYang 8:4974fc24fbd7 58 DigitalIn DI_0(PB_13);
WeberYang 8:4974fc24fbd7 59
WeberYang 6:eadfb1b45bda 60 //CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
WeberYang 6:eadfb1b45bda 61 //CANMsg rxMsg;
WeberYang 6:eadfb1b45bda 62 //CANMessage rxMsg;
WeberYang 3:51194773aa7e 63 Ticker CheckDataR;
WeberYang 3:51194773aa7e 64
WeberYang 2:648583d6e41a 65 MPU6050 mpu;//(PB_7,PB_6); // sda, scl pin
hardtail 0:6e61e8ec4b42 66
WeberYang 8:4974fc24fbd7 67 ros::NodeHandle nh;
WeberYang 8:4974fc24fbd7 68 //======================================================================
WeberYang 2:648583d6e41a 69 tiny_msgs::tinyIMU imu_msg;
WeberYang 8:4974fc24fbd7 70 ros::Publisher imu_pub("tinyImu", &imu_msg);
WeberYang 8:4974fc24fbd7 71 //======================================================================
WeberYang 8:4974fc24fbd7 72
WeberYang 8:4974fc24fbd7 73 //======================================================================
WeberYang 2:648583d6e41a 74 std_msgs::Float32 VelAngular_L;
WeberYang 8:4974fc24fbd7 75 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
WeberYang 8:4974fc24fbd7 76 //======================================================================
WeberYang 8:4974fc24fbd7 77
WeberYang 8:4974fc24fbd7 78 //======================================================================
WeberYang 2:648583d6e41a 79 std_msgs::Float32 VelAngular_R;
WeberYang 8:4974fc24fbd7 80 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
WeberYang 8:4974fc24fbd7 81 //======================================================================
WeberYang 8:4974fc24fbd7 82
WeberYang 8:4974fc24fbd7 83 //======================================================================
WeberYang 6:eadfb1b45bda 84 sensor_msgs::BatteryState BTState;
WeberYang 8:4974fc24fbd7 85 ros::Publisher BT_pub("BatteryState", &BTState);
WeberYang 8:4974fc24fbd7 86 //======================================================================
WeberYang 2:648583d6e41a 87
WeberYang 8:4974fc24fbd7 88 //======================================================================
WeberYang 8:4974fc24fbd7 89 std_msgs::Int16 DI;
WeberYang 8:4974fc24fbd7 90 ros::Publisher DI_pub("DI_pub", &DI);
WeberYang 8:4974fc24fbd7 91 //======================================================================
WeberYang 8:4974fc24fbd7 92
WeberYang 8:4974fc24fbd7 93 //======================================================================
WeberYang 9:e10bd4b460d7 94 std_msgs::Int16 ACT_state;
WeberYang 9:e10bd4b460d7 95 ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
WeberYang 9:e10bd4b460d7 96 //======================================================================
WeberYang 8:4974fc24fbd7 97
WeberYang 8:4974fc24fbd7 98 //======================================================================
WeberYang 9:e10bd4b460d7 99 std_msgs::Int16 Error_state;
WeberYang 9:e10bd4b460d7 100 ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
WeberYang 9:e10bd4b460d7 101 //======================================================================
WeberYang 9:e10bd4b460d7 102
WeberYang 2:648583d6e41a 103 uint32_t seq;
WeberYang 2:648583d6e41a 104
WeberYang 9:e10bd4b460d7 105 //========define ACT_state return code============================================
WeberYang 9:e10bd4b460d7 106 #define Satndby 0x00
WeberYang 9:e10bd4b460d7 107 #define Busy 0x01
WeberYang 9:e10bd4b460d7 108 #define Sensor_error 0x10
WeberYang 9:e10bd4b460d7 109 #define BMS_error 0x11
WeberYang 9:e10bd4b460d7 110 //========================================================
hardtail 0:6e61e8ec4b42 111 #define IMU_FIFO_RATE_DIVIDER 0x09
hardtail 0:6e61e8ec4b42 112 #define IMU_SAMPLE_RATE_DIVIDER 4
hardtail 0:6e61e8ec4b42 113 #define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
hardtail 0:6e61e8ec4b42 114 #define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
hardtail 0:6e61e8ec4b42 115
WeberYang 6:eadfb1b45bda 116 #define PC_BAUDRATE 115200//38400
hardtail 0:6e61e8ec4b42 117
hardtail 0:6e61e8ec4b42 118 #define DEG_TO_RAD(x) ( x * 0.01745329 )
hardtail 0:6e61e8ec4b42 119 #define RAD_TO_DEG(x) ( x * 57.29578 )
WeberYang 2:648583d6e41a 120
hardtail 0:6e61e8ec4b42 121 const int FIFO_BUFFER_SIZE = 128;
hardtail 0:6e61e8ec4b42 122 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
hardtail 0:6e61e8ec4b42 123 uint16_t fifoCount;
hardtail 0:6e61e8ec4b42 124 uint16_t packetSize;
hardtail 0:6e61e8ec4b42 125 bool dmpReady;
hardtail 0:6e61e8ec4b42 126 uint8_t mpuIntStatus;
hardtail 0:6e61e8ec4b42 127 const int snprintf_buffer_size = 100;
hardtail 0:6e61e8ec4b42 128 char snprintf_buffer[snprintf_buffer_size];
hardtail 0:6e61e8ec4b42 129 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
WeberYang 2:648583d6e41a 130 int16_t ax, ay, az;
WeberYang 2:648583d6e41a 131 int16_t gx, gy, gz;
WeberYang 8:4974fc24fbd7 132 float Lrpm,Rrpm;
WeberYang 8:4974fc24fbd7 133 float ticks_since_target;
WeberYang 9:e10bd4b460d7 134 float motor_rpm_r, motor_rpm_l;
WeberYang 9:e10bd4b460d7 135 float timeout_ticks;
WeberYang 9:e10bd4b460d7 136 int counter;
WeberYang 2:648583d6e41a 137 double w;
WeberYang 2:648583d6e41a 138 double rate;
WeberYang 2:648583d6e41a 139 double Dimeter;
WeberYang 2:648583d6e41a 140 float dx,dy,dr;
WeberYang 9:e10bd4b460d7 141 int lastsensorState = 1;
WeberYang 9:e10bd4b460d7 142 int sensorState;
WeberYang 8:4974fc24fbd7 143 int db_conter = 0;
WeberYang 3:51194773aa7e 144 int buffer[9] = {0};
WeberYang 5:52fb31c1a8c0 145 int dataH,datanum;
WeberYang 9:e10bd4b460d7 146 int motor_seq,motor_old_seq;
WeberYang 9:e10bd4b460d7 147 int state_code;
WeberYang 9:e10bd4b460d7 148 int error_code;
WeberYang 3:51194773aa7e 149 //=========RS485
WeberYang 3:51194773aa7e 150 char recChar=0;
WeberYang 3:51194773aa7e 151 bool recFlag=false;
WeberYang 3:51194773aa7e 152 char recArr[20];
WeberYang 3:51194773aa7e 153 int index=0;
WeberYang 9:e10bd4b460d7 154 int BMS_state;
WeberYang 6:eadfb1b45bda 155 uint32_t SOC;
WeberYang 6:eadfb1b45bda 156 uint32_t Tempert;
WeberYang 6:eadfb1b45bda 157 uint32_t RackVoltage = 0;
WeberYang 6:eadfb1b45bda 158 uint32_t Current = 0;
WeberYang 6:eadfb1b45bda 159 uint32_t MaxCellV = 0;
WeberYang 6:eadfb1b45bda 160 uint32_t MinCellV = 0;
WeberYang 6:eadfb1b45bda 161
hardtail 0:6e61e8ec4b42 162 struct Offset {
hardtail 0:6e61e8ec4b42 163 int16_t ax, ay, az;
hardtail 0:6e61e8ec4b42 164 int16_t gx, gy, gz;
WeberYang 9:e10bd4b460d7 165 }offset = {150+600, -350+300, 1000, -110-100, 5, 0};//{150, -350, 1000, -110, 5, 0}; // Measured values
hardtail 0:6e61e8ec4b42 166
hardtail 0:6e61e8ec4b42 167 struct MPU6050_DmpData {
hardtail 0:6e61e8ec4b42 168 Quaternion q;
hardtail 0:6e61e8ec4b42 169 VectorFloat gravity; // g
hardtail 0:6e61e8ec4b42 170 float roll, pitch, yaw; // rad
hardtail 0:6e61e8ec4b42 171 }dmpData;
hardtail 0:6e61e8ec4b42 172
hardtail 0:6e61e8ec4b42 173 long map(long x, long in_min, long in_max, long out_min, long out_max) {
hardtail 0:6e61e8ec4b42 174 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
hardtail 0:6e61e8ec4b42 175 }
WeberYang 2:648583d6e41a 176 //==========define sub function========================
hardtail 0:6e61e8ec4b42 177 bool Init();
hardtail 0:6e61e8ec4b42 178 void dmpDataUpdate();
WeberYang 2:648583d6e41a 179 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
WeberYang 2:648583d6e41a 180 int myabs( int a );
WeberYang 3:51194773aa7e 181 void TwistToMotors();
WeberYang 2:648583d6e41a 182 //===================================================
hardtail 0:6e61e8ec4b42 183
WeberYang 2:648583d6e41a 184
WeberYang 2:648583d6e41a 185 //======================= motor =================================================
WeberYang 2:648583d6e41a 186 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
WeberYang 2:648583d6e41a 187 {
WeberYang 2:648583d6e41a 188 unsigned int i, j;
WeberYang 2:648583d6e41a 189 //#define wPolynom 0xA001
WeberYang 2:648583d6e41a 190 unsigned int wCrc = 0xffff;
WeberYang 2:648583d6e41a 191 unsigned int wPolynom = 0xA001;
WeberYang 2:648583d6e41a 192 /*---------------------------------------------------------------------------------*/
WeberYang 2:648583d6e41a 193 for (i = 0; i < iBufLen; i++)
WeberYang 2:648583d6e41a 194 {
WeberYang 2:648583d6e41a 195 wCrc ^= cBuffer[i];
WeberYang 2:648583d6e41a 196 for (j = 0; j < 8; j++)
WeberYang 2:648583d6e41a 197 {
WeberYang 2:648583d6e41a 198 if (wCrc &0x0001)
WeberYang 2:648583d6e41a 199 {
WeberYang 2:648583d6e41a 200 wCrc = (wCrc >> 1) ^ wPolynom;
WeberYang 2:648583d6e41a 201 }
WeberYang 2:648583d6e41a 202 else
WeberYang 2:648583d6e41a 203 {
WeberYang 2:648583d6e41a 204 wCrc = wCrc >> 1;
WeberYang 2:648583d6e41a 205 }
WeberYang 2:648583d6e41a 206 }
WeberYang 2:648583d6e41a 207 }
WeberYang 2:648583d6e41a 208 return wCrc;
WeberYang 2:648583d6e41a 209 }
WeberYang 6:eadfb1b45bda 210 void Sendmessage(float Rrpm,float Lrpm)
WeberYang 2:648583d6e41a 211 {
WeberYang 9:e10bd4b460d7 212 // RS232.printf("Wr = %.1f\n",Rrpm);
WeberYang 9:e10bd4b460d7 213 // RS232.printf("Wl = %.1f\n",Lrpm);
WeberYang 6:eadfb1b45bda 214 unsigned char sendData[16];
WeberYang 6:eadfb1b45bda 215 unsigned int tmpCRC;
WeberYang 6:eadfb1b45bda 216 int motor1,motor2;
WeberYang 6:eadfb1b45bda 217
WeberYang 6:eadfb1b45bda 218 sendData[0] = Start;
WeberYang 6:eadfb1b45bda 219 sendData[1] = Address;
WeberYang 6:eadfb1b45bda 220 sendData[2] = ReturnType;
WeberYang 6:eadfb1b45bda 221 sendData[3] = Clean;
WeberYang 6:eadfb1b45bda 222 sendData[4] = Reserve;
WeberYang 6:eadfb1b45bda 223 sendData[5] = 0x01;//motor1Sevro ON
WeberYang 6:eadfb1b45bda 224 sendData[6] = 0x01;//motor2Sevro ON
WeberYang 9:e10bd4b460d7 225 if (Rrpm>0){sendData[7] = 0x00;}else{sendData[7] = 0x01;}
WeberYang 9:e10bd4b460d7 226 if (Lrpm>0){sendData[8] = 0x01;}else{sendData[8] = 0x00;}
WeberYang 6:eadfb1b45bda 227 motor1 = abs(Rrpm);
WeberYang 6:eadfb1b45bda 228 motor2 = abs(Lrpm);
WeberYang 6:eadfb1b45bda 229
WeberYang 6:eadfb1b45bda 230 sendData[9] = (motor1>>8);//motor1speedH
WeberYang 6:eadfb1b45bda 231 sendData[10] = (motor1 & 0xFF);//motor1speedL
WeberYang 6:eadfb1b45bda 232 sendData[11] = (motor2>>8);//motor2speedH
WeberYang 6:eadfb1b45bda 233 sendData[12] = (motor2 & 0xFF);//motor2speedL
WeberYang 6:eadfb1b45bda 234 sendData[13] = End;
WeberYang 6:eadfb1b45bda 235 tmpCRC = CRC_Verify(sendData, 14);
WeberYang 6:eadfb1b45bda 236 sendData[14] = (tmpCRC & 0xFF);
WeberYang 6:eadfb1b45bda 237 sendData[15] = (tmpCRC>>8);
WeberYang 3:51194773aa7e 238 int i;
WeberYang 6:eadfb1b45bda 239 for (i=0;i<16;i++)
WeberYang 3:51194773aa7e 240 {
WeberYang 6:eadfb1b45bda 241 RS232.printf("%c",sendData[i]);
WeberYang 3:51194773aa7e 242 }
WeberYang 6:eadfb1b45bda 243 RS232.printf("\r\n");
WeberYang 6:eadfb1b45bda 244 }
WeberYang 6:eadfb1b45bda 245 void TwistToMotors()
WeberYang 2:648583d6e41a 246 {
WeberYang 9:e10bd4b460d7 247 float right,left;
WeberYang 9:e10bd4b460d7 248 // double vel_data[2];
WeberYang 9:e10bd4b460d7 249 float vel_data[2];
WeberYang 9:e10bd4b460d7 250 float motor_rpm_vx, motor_rpm_theta;
WeberYang 9:e10bd4b460d7 251 motor_old_seq = motor_seq;
WeberYang 6:eadfb1b45bda 252 w = 0.302;//0.2 ;//m
WeberYang 6:eadfb1b45bda 253 rate = 20;//50;
WeberYang 6:eadfb1b45bda 254 timeout_ticks = 2;
WeberYang 6:eadfb1b45bda 255 Dimeter = 0.127;//0.15;
WeberYang 9:e10bd4b460d7 256
WeberYang 9:e10bd4b460d7 257 // prevent agv receive weird 1.0 command from cmd_vel
WeberYang 9:e10bd4b460d7 258 if (dr == 1.0){
WeberYang 9:e10bd4b460d7 259 dr = 0.001;
WeberYang 9:e10bd4b460d7 260 }
WeberYang 6:eadfb1b45bda 261 right = ( 1.0 * dx ) + (dr * w /2);
WeberYang 9:e10bd4b460d7 262 left = ( 1.0 * dx ) - (dr * w /2);
WeberYang 9:e10bd4b460d7 263 motor_rpm_vx = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 9:e10bd4b460d7 264 if((motor_rpm_vx !=0) && (myabs(motor_rpm_vx)<100)){
WeberYang 9:e10bd4b460d7 265 if(motor_rpm_vx >0){
WeberYang 9:e10bd4b460d7 266 motor_rpm_vx = 100;
WeberYang 6:eadfb1b45bda 267 }
WeberYang 9:e10bd4b460d7 268 else{
WeberYang 9:e10bd4b460d7 269 motor_rpm_vx = -100;
WeberYang 6:eadfb1b45bda 270 }
WeberYang 6:eadfb1b45bda 271 }
WeberYang 9:e10bd4b460d7 272 motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 9:e10bd4b460d7 273 motor_rpm_r = motor_rpm_vx+ motor_rpm_theta;
WeberYang 9:e10bd4b460d7 274 motor_rpm_l = motor_rpm_vx- motor_rpm_theta;
WeberYang 9:e10bd4b460d7 275 if (myabs(motor_rpm_r)<100|| myabs(motor_rpm_l)<100){
WeberYang 9:e10bd4b460d7 276 if( dx==0){
WeberYang 9:e10bd4b460d7 277 if(dr>0){
WeberYang 9:e10bd4b460d7 278 motor_rpm_r=100;
WeberYang 9:e10bd4b460d7 279 motor_rpm_l=-100;
WeberYang 9:e10bd4b460d7 280 }else if (dr<0){
WeberYang 9:e10bd4b460d7 281 motor_rpm_r=-100;
WeberYang 9:e10bd4b460d7 282 motor_rpm_l=100;
WeberYang 9:e10bd4b460d7 283 }else{
WeberYang 9:e10bd4b460d7 284 motor_rpm_r=0;
WeberYang 9:e10bd4b460d7 285 motor_rpm_l=0;
WeberYang 9:e10bd4b460d7 286 }
WeberYang 9:e10bd4b460d7 287 }
WeberYang 9:e10bd4b460d7 288 else if(dx>0){
WeberYang 9:e10bd4b460d7 289 if (myabs(motor_rpm_r)<100){
WeberYang 9:e10bd4b460d7 290 motor_rpm_r =100;
WeberYang 9:e10bd4b460d7 291 }
WeberYang 9:e10bd4b460d7 292 if (myabs(motor_rpm_l)<100){
WeberYang 9:e10bd4b460d7 293 motor_rpm_l =100;
WeberYang 9:e10bd4b460d7 294 }
WeberYang 9:e10bd4b460d7 295 }
WeberYang 9:e10bd4b460d7 296 else{
WeberYang 9:e10bd4b460d7 297 if(myabs(motor_rpm_r)<100){
WeberYang 9:e10bd4b460d7 298 motor_rpm_r =-100;
WeberYang 9:e10bd4b460d7 299 }
WeberYang 9:e10bd4b460d7 300 if(myabs(motor_rpm_l)<100){
WeberYang 9:e10bd4b460d7 301 motor_rpm_l =-100;
WeberYang 9:e10bd4b460d7 302 }
WeberYang 9:e10bd4b460d7 303 }
WeberYang 6:eadfb1b45bda 304 }
WeberYang 9:e10bd4b460d7 305 vel_data[0] = motor_rpm_r;
WeberYang 9:e10bd4b460d7 306 vel_data[1] = motor_rpm_l;
WeberYang 9:e10bd4b460d7 307
WeberYang 6:eadfb1b45bda 308 //===================================================================
WeberYang 9:e10bd4b460d7 309 //Sendmessage(vel_data[0],vel_data[1]);
WeberYang 9:e10bd4b460d7 310
WeberYang 9:e10bd4b460d7 311 //Sendmessage(motor_rpm_l,motor_rpm_r);
WeberYang 9:e10bd4b460d7 312
WeberYang 6:eadfb1b45bda 313 VelAngular_R.data = vel_data[0];
WeberYang 6:eadfb1b45bda 314 VelAngular_L.data = vel_data[1];
WeberYang 6:eadfb1b45bda 315 //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
WeberYang 6:eadfb1b45bda 316 //}
WeberYang 6:eadfb1b45bda 317 //else{
WeberYang 6:eadfb1b45bda 318 pub_rmotor.publish( &VelAngular_R );
WeberYang 6:eadfb1b45bda 319 pub_lmotor.publish( &VelAngular_L );
WeberYang 6:eadfb1b45bda 320 //}
WeberYang 6:eadfb1b45bda 321 //RS232.printf("Wr = %.1f\n",vel_data[0]);
WeberYang 6:eadfb1b45bda 322 //RS232.printf("Wl = %.1f\n",vel_data[1]);
WeberYang 6:eadfb1b45bda 323 ticks_since_target += 1;
WeberYang 2:648583d6e41a 324
WeberYang 3:51194773aa7e 325 }
WeberYang 2:648583d6e41a 326 int myabs( int a ){
WeberYang 2:648583d6e41a 327 if ( a < 0 ){
WeberYang 2:648583d6e41a 328 return -a;
WeberYang 2:648583d6e41a 329 }
WeberYang 2:648583d6e41a 330 return a;
WeberYang 2:648583d6e41a 331 }
WeberYang 5:52fb31c1a8c0 332
WeberYang 5:52fb31c1a8c0 333 int str2int(const char* str, int star, int end)
WeberYang 5:52fb31c1a8c0 334 {
WeberYang 5:52fb31c1a8c0 335 int i;
WeberYang 5:52fb31c1a8c0 336 int ret = 0;
WeberYang 5:52fb31c1a8c0 337 for (i = star; i < end+1; i++)
WeberYang 5:52fb31c1a8c0 338 {
WeberYang 5:52fb31c1a8c0 339 ret = ret *10 + (str[i] - '0');
WeberYang 5:52fb31c1a8c0 340 }
WeberYang 5:52fb31c1a8c0 341 return ret;
WeberYang 5:52fb31c1a8c0 342 }
WeberYang 9:e10bd4b460d7 343
WeberYang 9:e10bd4b460d7 344 void update_state(int code1,int code2){
WeberYang 9:e10bd4b460d7 345
WeberYang 9:e10bd4b460d7 346 }
WeberYang 9:e10bd4b460d7 347 //======================================================================
WeberYang 9:e10bd4b460d7 348 std_msgs::Int16 DO;
WeberYang 9:e10bd4b460d7 349 //DO_0 MAG_1
WeberYang 9:e10bd4b460d7 350 //DO_1,MAG_2
WeberYang 9:e10bd4b460d7 351 //DO_2,UPS
WeberYang 9:e10bd4b460d7 352 //DO_3,BMS
WeberYang 9:e10bd4b460d7 353 int State;
WeberYang 9:e10bd4b460d7 354 void DO_ACT(const std_msgs::Int16 &msg){
WeberYang 9:e10bd4b460d7 355 //0xFF for action procedure
WeberYang 9:e10bd4b460d7 356 if (msg.data == 0x21){
WeberYang 9:e10bd4b460d7 357 error_code = 99;
WeberYang 9:e10bd4b460d7 358 State = ACT_MG_ON;
WeberYang 9:e10bd4b460d7 359 }
WeberYang 9:e10bd4b460d7 360
WeberYang 9:e10bd4b460d7 361 if (msg.data == 0x20){
WeberYang 9:e10bd4b460d7 362 error_code = 99;
WeberYang 9:e10bd4b460d7 363 State = ACT_MG_OFF;
WeberYang 9:e10bd4b460d7 364 }
WeberYang 9:e10bd4b460d7 365
WeberYang 9:e10bd4b460d7 366 if (msg.data<0x20){
WeberYang 9:e10bd4b460d7 367 if (msg.data & 0x01){
WeberYang 9:e10bd4b460d7 368 DO_0 = 1;
WeberYang 9:e10bd4b460d7 369 }
WeberYang 9:e10bd4b460d7 370 else{
WeberYang 9:e10bd4b460d7 371 DO_0 = 0;
WeberYang 9:e10bd4b460d7 372 }
WeberYang 9:e10bd4b460d7 373
WeberYang 9:e10bd4b460d7 374 if (msg.data & 0x02){
WeberYang 9:e10bd4b460d7 375 DO_1 = 1;
WeberYang 9:e10bd4b460d7 376 }
WeberYang 9:e10bd4b460d7 377 else{
WeberYang 9:e10bd4b460d7 378 DO_1 = 0;
WeberYang 9:e10bd4b460d7 379 }
WeberYang 9:e10bd4b460d7 380
WeberYang 9:e10bd4b460d7 381 if (msg.data & 0x04){
WeberYang 9:e10bd4b460d7 382 DO_2 = 1;
WeberYang 9:e10bd4b460d7 383 }
WeberYang 9:e10bd4b460d7 384 else{
WeberYang 9:e10bd4b460d7 385 DO_2 = 0;
WeberYang 9:e10bd4b460d7 386 }
WeberYang 9:e10bd4b460d7 387
WeberYang 9:e10bd4b460d7 388 if (msg.data & 0x08){
WeberYang 9:e10bd4b460d7 389 DO_3 = 1;
WeberYang 9:e10bd4b460d7 390 }
WeberYang 9:e10bd4b460d7 391 else{
WeberYang 9:e10bd4b460d7 392 DO_3 = 0;
WeberYang 9:e10bd4b460d7 393 }
WeberYang 9:e10bd4b460d7 394
WeberYang 9:e10bd4b460d7 395 }
WeberYang 9:e10bd4b460d7 396 }
WeberYang 9:e10bd4b460d7 397 ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
WeberYang 9:e10bd4b460d7 398 //======================================================================
WeberYang 6:eadfb1b45bda 399 //======================================================================================
WeberYang 2:648583d6e41a 400 void messageCb(const geometry_msgs::Twist &msg)
WeberYang 2:648583d6e41a 401 {
WeberYang 9:e10bd4b460d7 402 // RS232.printf("messageCb");
WeberYang 2:648583d6e41a 403 ticks_since_target = 0;
WeberYang 2:648583d6e41a 404 dx = msg.linear.x;
WeberYang 2:648583d6e41a 405 dy = msg.linear.y;
WeberYang 2:648583d6e41a 406 dr = msg.angular.z;
WeberYang 9:e10bd4b460d7 407 // RS232.printf("dx = %d,dy = %d,dr = %d\r\n",dx,dy,dr);
WeberYang 2:648583d6e41a 408 TwistToMotors();
WeberYang 5:52fb31c1a8c0 409 //ReadENC(Motor1);
WeberYang 2:648583d6e41a 410 }
WeberYang 2:648583d6e41a 411 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
WeberYang 2:648583d6e41a 412 //======================================================================================
hardtail 0:6e61e8ec4b42 413 void dmpDataUpdate() {
hardtail 0:6e61e8ec4b42 414 // Check that this interrupt has enabled.
hardtail 0:6e61e8ec4b42 415 if (dmpReady == false) return;
hardtail 0:6e61e8ec4b42 416
hardtail 0:6e61e8ec4b42 417 mpuIntStatus = mpu.getIntStatus();
hardtail 0:6e61e8ec4b42 418 fifoCount = mpu.getFIFOCount();
hardtail 0:6e61e8ec4b42 419
hardtail 0:6e61e8ec4b42 420 // Check that this interrupt is a FIFO buffer overflow interrupt.
hardtail 0:6e61e8ec4b42 421 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
hardtail 0:6e61e8ec4b42 422 mpu.resetFIFO();
WeberYang 2:648583d6e41a 423 //pc.printf("FIFO overflow!\n");
hardtail 0:6e61e8ec4b42 424 return;
hardtail 0:6e61e8ec4b42 425
hardtail 0:6e61e8ec4b42 426 // Check that this interrupt is a Data Ready interrupt.
hardtail 0:6e61e8ec4b42 427 } else if (mpuIntStatus & 0x02) {
hardtail 0:6e61e8ec4b42 428 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
hardtail 0:6e61e8ec4b42 429
hardtail 0:6e61e8ec4b42 430 mpu.getFIFOBytes(fifoBuffer, packetSize);
hardtail 0:6e61e8ec4b42 431
hardtail 0:6e61e8ec4b42 432 #ifdef OUTPUT_QUATERNION
hardtail 0:6e61e8ec4b42 433 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 434 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 435 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 436 #endif
hardtail 0:6e61e8ec4b42 437
hardtail 0:6e61e8ec4b42 438 #ifdef OUTPUT_EULER
hardtail 0:6e61e8ec4b42 439 float euler[3];
hardtail 0:6e61e8ec4b42 440 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 441 mpu.dmpGetEuler(euler, &dmpData.q);
hardtail 0:6e61e8ec4b42 442 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 443 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 444 #endif
hardtail 0:6e61e8ec4b42 445
hardtail 0:6e61e8ec4b42 446 #ifdef OUTPUT_ROLL_PITCH_YAW
hardtail 0:6e61e8ec4b42 447 mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
hardtail 0:6e61e8ec4b42 448 mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
hardtail 0:6e61e8ec4b42 449 float rollPitchYaw[3];
hardtail 0:6e61e8ec4b42 450 mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
hardtail 0:6e61e8ec4b42 451 dmpData.roll = rollPitchYaw[2];
hardtail 0:6e61e8ec4b42 452 dmpData.pitch = rollPitchYaw[1];
hardtail 0:6e61e8ec4b42 453 dmpData.yaw = rollPitchYaw[0];
hardtail 0:6e61e8ec4b42 454
hardtail 0:6e61e8ec4b42 455 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 456 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 457
hardtail 0:6e61e8ec4b42 458 #ifdef servotest
hardtail 0:6e61e8ec4b42 459 int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
hardtail 0:6e61e8ec4b42 460 if(servoPulse > 1450) servoPulse = 1450;
hardtail 0:6e61e8ec4b42 461 if(servoPulse < 500) servoPulse = 500;
hardtail 0:6e61e8ec4b42 462 sv.pulsewidth_us(servoPulse);
hardtail 0:6e61e8ec4b42 463 #endif
hardtail 0:6e61e8ec4b42 464 #endif
hardtail 0:6e61e8ec4b42 465
hardtail 0:6e61e8ec4b42 466 #ifdef OUTPUT_FOR_TEAPOT
hardtail 0:6e61e8ec4b42 467 teapotPacket[2] = fifoBuffer[0];
hardtail 0:6e61e8ec4b42 468 teapotPacket[3] = fifoBuffer[1];
hardtail 0:6e61e8ec4b42 469 teapotPacket[4] = fifoBuffer[4];
hardtail 0:6e61e8ec4b42 470 teapotPacket[5] = fifoBuffer[5];
hardtail 0:6e61e8ec4b42 471 teapotPacket[6] = fifoBuffer[8];
hardtail 0:6e61e8ec4b42 472 teapotPacket[7] = fifoBuffer[9];
hardtail 0:6e61e8ec4b42 473 teapotPacket[8] = fifoBuffer[12];
hardtail 0:6e61e8ec4b42 474 teapotPacket[9] = fifoBuffer[13];
hardtail 0:6e61e8ec4b42 475 for (uint8_t i = 0; i < 14; i++) {
hardtail 0:6e61e8ec4b42 476 pc.putc(teapotPacket[i]);
hardtail 0:6e61e8ec4b42 477 }
hardtail 0:6e61e8ec4b42 478 #endif
hardtail 0:6e61e8ec4b42 479
hardtail 0:6e61e8ec4b42 480 #ifdef OUTPUT_TEMPERATURE
hardtail 0:6e61e8ec4b42 481 float temp = mpu.getTemperature() / 340.0 + 36.53;
hardtail 0:6e61e8ec4b42 482 if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
WeberYang 2:648583d6e41a 483 pc.printf(snprintf_buffer);
hardtail 0:6e61e8ec4b42 484 #endif
hardtail 0:6e61e8ec4b42 485
WeberYang 2:648583d6e41a 486 // pc.printf("\n");
WeberYang 2:648583d6e41a 487 }
WeberYang 2:648583d6e41a 488 }
WeberYang 3:51194773aa7e 489 //=======================================================
WeberYang 5:52fb31c1a8c0 490 bool Init() {
WeberYang 8:4974fc24fbd7 491 DO_0 = 0;
WeberYang 8:4974fc24fbd7 492 DO_1 = 0;
WeberYang 8:4974fc24fbd7 493
WeberYang 5:52fb31c1a8c0 494 seq = 0;
WeberYang 2:648583d6e41a 495 nh.initNode();
WeberYang 9:e10bd4b460d7 496 // nh.advertise(imu_pub);
WeberYang 5:52fb31c1a8c0 497 nh.advertise(pub_lmotor);
WeberYang 5:52fb31c1a8c0 498 nh.advertise(pub_rmotor);
WeberYang 6:eadfb1b45bda 499 nh.advertise(BT_pub);
WeberYang 8:4974fc24fbd7 500 nh.advertise(DI_pub);
WeberYang 9:e10bd4b460d7 501 nh.advertise(ACT_state_pub);
WeberYang 9:e10bd4b460d7 502 nh.advertise(Error_state_pub);
WeberYang 2:648583d6e41a 503 nh.subscribe(cmd_vel_sub);
WeberYang 8:4974fc24fbd7 504 nh.subscribe(ACT_sub);
WeberYang 9:e10bd4b460d7 505 /*
WeberYang 7:1a234f02746f 506 mpu.initialize();
WeberYang 7:1a234f02746f 507 if (mpu.testConnection()) {
WeberYang 7:1a234f02746f 508 // pc.printf("MPU6050 test connection passed.\n");
WeberYang 7:1a234f02746f 509 } else {
WeberYang 7:1a234f02746f 510 // pc.printf("MPU6050 test connection failed.\n");
WeberYang 7:1a234f02746f 511 return false;
WeberYang 7:1a234f02746f 512 }
WeberYang 7:1a234f02746f 513 if (mpu.dmpInitialize() == 0) {
WeberYang 7:1a234f02746f 514 // pc.printf("succeed in MPU6050 DMP Initializing.\n");
WeberYang 7:1a234f02746f 515 } else {
WeberYang 7:1a234f02746f 516 // pc.printf("failed in MPU6050 DMP Initializing.\n");
WeberYang 7:1a234f02746f 517 return false;
WeberYang 7:1a234f02746f 518 }
WeberYang 9:e10bd4b460d7 519
WeberYang 9:e10bd4b460d7 520 //mpu.setXAccelOffsetTC(offset.ax);
WeberYang 9:e10bd4b460d7 521 // mpu.setYAccelOffsetTC(offset.ay);
WeberYang 9:e10bd4b460d7 522 // mpu.setZAccelOffset(offset.az);
WeberYang 9:e10bd4b460d7 523 mpu.setXAccelOffset(1000); //2000 -3134
WeberYang 9:e10bd4b460d7 524 mpu.setYAccelOffset(0);
WeberYang 9:e10bd4b460d7 525 mpu.setZAccelOffset(0);
WeberYang 9:e10bd4b460d7 526 mpu.setXGyroOffset(800);//offset.gx);
WeberYang 9:e10bd4b460d7 527 mpu.setYGyroOffset(0);//offset.gy);
WeberYang 9:e10bd4b460d7 528 mpu.setZGyroOffset(0);//offset.gz);
WeberYang 9:e10bd4b460d7 529
WeberYang 9:e10bd4b460d7 530
WeberYang 9:e10bd4b460d7 531
WeberYang 9:e10bd4b460d7 532 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);//2000);
WeberYang 7:1a234f02746f 533 mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
WeberYang 7:1a234f02746f 534 mpu.setDMPEnabled(true); // Enable DMP
WeberYang 9:e10bd4b460d7 535
WeberYang 7:1a234f02746f 536 packetSize = mpu.dmpGetFIFOPacketSize();
WeberYang 7:1a234f02746f 537 dmpReady = true; // Enable interrupt.
WeberYang 9:e10bd4b460d7 538 */
WeberYang 5:52fb31c1a8c0 539 //pc.printf("Init finish!\n");
WeberYang 5:52fb31c1a8c0 540
WeberYang 5:52fb31c1a8c0 541 return true;
WeberYang 5:52fb31c1a8c0 542 }
WeberYang 9:e10bd4b460d7 543 int MG_ACT(int state)
WeberYang 9:e10bd4b460d7 544 {
WeberYang 9:e10bd4b460d7 545 // int ret;
WeberYang 9:e10bd4b460d7 546 if (state == 1){ //MAG_ON
WeberYang 9:e10bd4b460d7 547 if (sensorState == 1){ //Battery on-position
WeberYang 9:e10bd4b460d7 548 //Lock triggrt
WeberYang 9:e10bd4b460d7 549 DO_1 = 1;
WeberYang 9:e10bd4b460d7 550 wait_ms(100);
WeberYang 9:e10bd4b460d7 551 DO_1 = 0;
WeberYang 9:e10bd4b460d7 552
WeberYang 9:e10bd4b460d7 553 DO_2 = 1;
WeberYang 9:e10bd4b460d7 554 wait(3);
WeberYang 9:e10bd4b460d7 555 DO_2 = 0;
WeberYang 9:e10bd4b460d7 556 return 1;
WeberYang 9:e10bd4b460d7 557 }
WeberYang 9:e10bd4b460d7 558 }
WeberYang 9:e10bd4b460d7 559 else if (state == 2){//MAG_OFF
WeberYang 9:e10bd4b460d7 560 //unLock triggrt
WeberYang 9:e10bd4b460d7 561 DO_2 = 1;
WeberYang 9:e10bd4b460d7 562 wait_ms(100);
WeberYang 9:e10bd4b460d7 563 DO_2 = 0;
WeberYang 9:e10bd4b460d7 564
WeberYang 9:e10bd4b460d7 565 DO_2 = 1;
WeberYang 9:e10bd4b460d7 566 wait(3);
WeberYang 9:e10bd4b460d7 567 DO_2 = 0;
WeberYang 9:e10bd4b460d7 568 return 1;
WeberYang 9:e10bd4b460d7 569 }
WeberYang 9:e10bd4b460d7 570 return 0;
WeberYang 9:e10bd4b460d7 571 }
WeberYang 5:52fb31c1a8c0 572 //=======================================================
WeberYang 5:52fb31c1a8c0 573 int main() {
WeberYang 6:eadfb1b45bda 574 RS232.baud(PC_BAUDRATE);
WeberYang 5:52fb31c1a8c0 575 MBED_ASSERT(Init() == true);
WeberYang 6:eadfb1b45bda 576 CANMessage rxMsg;
WeberYang 8:4974fc24fbd7 577 DI_0.mode(PullUp);
WeberYang 6:eadfb1b45bda 578 CAN_T = 0;
WeberYang 6:eadfb1b45bda 579 CAN_R = 0;
WeberYang 6:eadfb1b45bda 580 wait_ms(50);
WeberYang 6:eadfb1b45bda 581 CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
WeberYang 6:eadfb1b45bda 582 wait_ms(50);
WeberYang 6:eadfb1b45bda 583 can1.frequency(500000);
WeberYang 6:eadfb1b45bda 584 wait_ms(50);
WeberYang 9:e10bd4b460d7 585 DO_2 = 1; //let UPS ON
WeberYang 9:e10bd4b460d7 586
WeberYang 9:e10bd4b460d7 587
WeberYang 9:e10bd4b460d7 588
WeberYang 6:eadfb1b45bda 589 while(1){
WeberYang 2:648583d6e41a 590 seq++;
WeberYang 9:e10bd4b460d7 591 motor_seq = seq;
WeberYang 6:eadfb1b45bda 592 t.start();
WeberYang 9:e10bd4b460d7 593 //================================
WeberYang 9:e10bd4b460d7 594 //========define ACT_state return code============================================
WeberYang 9:e10bd4b460d7 595 //#define Satndby 0x00
WeberYang 9:e10bd4b460d7 596 //#define Busy 0x01
WeberYang 9:e10bd4b460d7 597 //#define Sensor_error 0x10
WeberYang 9:e10bd4b460d7 598 //========================================================
WeberYang 9:e10bd4b460d7 599 counter++;
WeberYang 9:e10bd4b460d7 600 state_code = State;
WeberYang 9:e10bd4b460d7 601 switch (State){
WeberYang 9:e10bd4b460d7 602 int ret;
WeberYang 9:e10bd4b460d7 603 case IDLE:
WeberYang 9:e10bd4b460d7 604 counter = 0;
WeberYang 9:e10bd4b460d7 605 break;
WeberYang 9:e10bd4b460d7 606
WeberYang 9:e10bd4b460d7 607 case ACT_MG_ON:
WeberYang 9:e10bd4b460d7 608 ret = MG_ACT(1);
WeberYang 9:e10bd4b460d7 609 if (ret){
WeberYang 9:e10bd4b460d7 610 State = Check_BMS_ON;
WeberYang 9:e10bd4b460d7 611 counter = 0;
WeberYang 9:e10bd4b460d7 612 }
WeberYang 9:e10bd4b460d7 613 if (counter>10){
WeberYang 9:e10bd4b460d7 614 State = IDLE;
WeberYang 9:e10bd4b460d7 615 error_code = Sensor_error;
WeberYang 9:e10bd4b460d7 616 }
WeberYang 9:e10bd4b460d7 617 break;
WeberYang 9:e10bd4b460d7 618
WeberYang 9:e10bd4b460d7 619 case Check_BMS_ON:
WeberYang 9:e10bd4b460d7 620 if (BMS_state == 1){
WeberYang 9:e10bd4b460d7 621 State = IDLE;
WeberYang 9:e10bd4b460d7 622 error_code = 0;
WeberYang 9:e10bd4b460d7 623 }
WeberYang 9:e10bd4b460d7 624 if (counter>10){
WeberYang 9:e10bd4b460d7 625 State = IDLE;
WeberYang 9:e10bd4b460d7 626 error_code = BMS_error;
WeberYang 9:e10bd4b460d7 627 }
WeberYang 9:e10bd4b460d7 628 break;
WeberYang 9:e10bd4b460d7 629
WeberYang 9:e10bd4b460d7 630 case ACT_MG_OFF:
WeberYang 9:e10bd4b460d7 631 ret = MG_ACT(2);
WeberYang 9:e10bd4b460d7 632 if (ret){
WeberYang 9:e10bd4b460d7 633 State = Check_BMS_OFF;
WeberYang 9:e10bd4b460d7 634 counter = 0;
WeberYang 9:e10bd4b460d7 635 }
WeberYang 9:e10bd4b460d7 636 if (counter>10){
WeberYang 9:e10bd4b460d7 637 State = IDLE;
WeberYang 9:e10bd4b460d7 638 error_code = Sensor_error;
WeberYang 9:e10bd4b460d7 639 }
WeberYang 9:e10bd4b460d7 640 break;
WeberYang 9:e10bd4b460d7 641
WeberYang 9:e10bd4b460d7 642 case Check_BMS_OFF:
WeberYang 9:e10bd4b460d7 643 if (BMS_state == 0){
WeberYang 9:e10bd4b460d7 644 State = IDLE;
WeberYang 9:e10bd4b460d7 645 error_code = 0;
WeberYang 9:e10bd4b460d7 646 }
WeberYang 9:e10bd4b460d7 647 if (counter>10){
WeberYang 9:e10bd4b460d7 648 State = IDLE;
WeberYang 9:e10bd4b460d7 649 error_code = BMS_error;
WeberYang 9:e10bd4b460d7 650 }
WeberYang 9:e10bd4b460d7 651 break;
WeberYang 9:e10bd4b460d7 652
WeberYang 9:e10bd4b460d7 653 }
WeberYang 9:e10bd4b460d7 654
WeberYang 9:e10bd4b460d7 655 ACT_state.data = state_code;
WeberYang 9:e10bd4b460d7 656 ACT_state_pub.publish( &ACT_state);
WeberYang 9:e10bd4b460d7 657 Error_state.data = error_code;
WeberYang 9:e10bd4b460d7 658 Error_state_pub.publish( &Error_state);
WeberYang 8:4974fc24fbd7 659 //============DI==================
WeberYang 8:4974fc24fbd7 660 int reading = DI_0;
WeberYang 9:e10bd4b460d7 661 if (reading == lastsensorState) {
WeberYang 8:4974fc24fbd7 662 db_conter = db_conter+1;
WeberYang 8:4974fc24fbd7 663 }
WeberYang 8:4974fc24fbd7 664 else{
WeberYang 8:4974fc24fbd7 665 db_conter = 0;
WeberYang 9:e10bd4b460d7 666 }
WeberYang 8:4974fc24fbd7 667 if (db_conter > 3) {
WeberYang 9:e10bd4b460d7 668 sensorState = reading;
WeberYang 9:e10bd4b460d7 669 DI.data = sensorState;
WeberYang 8:4974fc24fbd7 670 }
WeberYang 8:4974fc24fbd7 671
WeberYang 9:e10bd4b460d7 672 lastsensorState = reading;
WeberYang 8:4974fc24fbd7 673 DI_pub.publish( &DI);
WeberYang 8:4974fc24fbd7 674
WeberYang 8:4974fc24fbd7 675 //=========================================
WeberYang 6:eadfb1b45bda 676 if (can1.read(rxMsg) && (t.read_ms() > 5000)) {
WeberYang 6:eadfb1b45bda 677 if(rxMsg.id == CAN_DATA){
WeberYang 9:e10bd4b460d7 678 BMS_state = 1;
WeberYang 6:eadfb1b45bda 679 SOC = rxMsg.data[0]>>1;
WeberYang 6:eadfb1b45bda 680 Tempert = rxMsg.data[1]-50;
WeberYang 6:eadfb1b45bda 681 RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + rxMsg.data[2];
WeberYang 6:eadfb1b45bda 682 Current = ((unsigned int)rxMsg.data[5] << 8) + rxMsg.data[4];
WeberYang 6:eadfb1b45bda 683 MaxCellV = rxMsg.data[6];
WeberYang 9:e10bd4b460d7 684 MinCellV = rxMsg.data[7];
WeberYang 6:eadfb1b45bda 685 BTState.header.stamp = nh.now();
WeberYang 6:eadfb1b45bda 686 BTState.header.frame_id = 0;
WeberYang 6:eadfb1b45bda 687 BTState.header.seq = seq;
WeberYang 6:eadfb1b45bda 688 BTState.voltage = RackVoltage*0.1;
WeberYang 6:eadfb1b45bda 689 BTState.current = Current;
WeberYang 6:eadfb1b45bda 690 BTState.design_capacity = 80;
WeberYang 6:eadfb1b45bda 691 BTState.percentage = SOC;
WeberYang 6:eadfb1b45bda 692 BT_pub.publish( &BTState );
WeberYang 6:eadfb1b45bda 693 t.reset();
WeberYang 6:eadfb1b45bda 694 } // if
WeberYang 6:eadfb1b45bda 695 } // if
WeberYang 9:e10bd4b460d7 696 else{
WeberYang 9:e10bd4b460d7 697 BMS_state = 0;
WeberYang 9:e10bd4b460d7 698 }
WeberYang 9:e10bd4b460d7 699
WeberYang 9:e10bd4b460d7 700
WeberYang 9:e10bd4b460d7 701 if (motor_seq > motor_old_seq + 5){
WeberYang 9:e10bd4b460d7 702 motor_rpm_r = 0;
WeberYang 9:e10bd4b460d7 703 motor_rpm_l = 0;
WeberYang 9:e10bd4b460d7 704 }
WeberYang 9:e10bd4b460d7 705
WeberYang 9:e10bd4b460d7 706 Sendmessage(motor_rpm_l,motor_rpm_r);
WeberYang 9:e10bd4b460d7 707 wait_ms(100);
WeberYang 9:e10bd4b460d7 708 nh.spinOnce();
hardtail 0:6e61e8ec4b42 709 }
hardtail 0:6e61e8ec4b42 710 }