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@11:6d5307ceb569, 2018-10-02 (annotated)

Committer:
WeberYang
Date:
Tue Oct 02 00:57:35 2018 +0000
Revision:
11:6d5307ceb569
Parent:
10:8b7fce3bba86 
_0829_demo_version

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