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@10:8b7fce3bba86, 2018-05-25 (annotated)

Committer:
WeberYang
Date:
Fri May 25 08:07:15 2018 +0000
Revision:
10:8b7fce3bba86
Parent:
9:e10bd4b460d7
Child:
11:6d5307ceb569
DEMO_version01

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