Weber Yang
Delta Battery, CAN_bus, DIO P2P, ROS
Dependencies: mbed mbed-rtos ros_lib_kinetic
Diff: main.cpp
- Revision:
- 12:ed085acbbde5
- Parent:
- 11:6d5307ceb569
--- a/main.cpp Tue Oct 02 00:57:35 2018 +0000
+++ b/main.cpp Thu Aug 22 07:44:05 2019 +0000
@@ -1,832 +1,236 @@
/*
0412 combine the sevro motor encoder to publish
*/
-#include "MPU6050_6Axis_MotionApps20.h"
#include "mbed.h"
#include "CAN.h"
-#include "I2Cdev.h"
-#include "MPU6050.h"
-
+#include "cmsis_os.h"
#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Int16.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/BatteryState.h>
+#include <sensor_msgs/LaserEcho.h>
#include <geometry_msgs/Twist.h> //set buffer larger than 50byte
#include <math.h>
#include <stdio.h>
-#include <tiny_msgs/tinyVector.h>
-#include <tiny_msgs/tinyIMU.h>
#include <string>
#include <cstdlib>
-#define Start 0xAA
-#define Address 0x7F
-#define ReturnType 0x00
-#define Clean 0x00
-#define Reserve 0x00
-#define End 0x55
-#define Motor1 1
-#define Motor2 2
-#define LENG 31 //0x42 + 31 bytes equal to 32 bytes
-
-#define Write 0x06
-#define Read 0x03
-#define DI1 0x0214 //0214H means digital input DI1 for sevro on
-#define APR 0x0066 //0066H means encoder abs rev
-#define SP1 0x0112
-
#define CAN_DATA 0x470
#define CAN_STATUS 0x471
-#define IDLE 0
-#define ACT_MG_ON 1
-#define ACT_MG_OFF 2
-#define Check_BMS_ON 3
-#define Check_BMS_OFF 4
-#define WAIT_BAT 5
+#define PC_BAUDRATE 115200
+
//Serial pc(USBTX,USBRX);
Timer t;
Serial RS232(PA_9, PA_10);
-DigitalOut Receiver(D7); //RS485_E
+DigitalOut Receiver(D7); //RS485_E
DigitalOut CAN_T(D14);
DigitalOut CAN_R(D15);
-DigitalOut DO_0(PC_5);
-DigitalOut DO_1(PC_6);
-DigitalOut DO_2(PC_8);
-DigitalOut DO_3(PC_9);
-DigitalOut DO_4(PA_12);
-DigitalIn DI_0(PB_13);
-//CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
-//CANMsg rxMsg;
-//CANMessage rxMsg;
-Ticker CheckDataR;
-
-MPU6050 mpu;//(PB_7,PB_6); // sda, scl pin
-
-ros::NodeHandle nh;
-//======================================================================
-tiny_msgs::tinyIMU imu_msg;
-ros::Publisher imu_pub("tinyImu", &imu_msg);
-//======================================================================
+DigitalOut DO_0(PC_3);
+DigitalOut DO_1(PC_2);
+//DigitalOut DO_2(PF_1);
+//DigitalOut DO_3(PF_0);
+DigitalOut DO_4(PC_15);
+DigitalOut DO_5(PC_14);
+DigitalOut DO_6(PC_13);
+DigitalOut DO_7(PB_7);
+DigitalOut DO_8(PA_14);
+DigitalOut DO_9(PA_13);
+//DigitalOut DO_10(PF_7);
+//DigitalOut DO_11(PF_6);
+DigitalOut DO_12(PC_12);
+DigitalOut DO_13(PC_10);
+DigitalOut DO_14(PD_2);
+DigitalOut DO_15(PC_11);
+DigitalOut led1(LED1);
-//======================================================================
-std_msgs::Float32 VelAngular_L;
-ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
-//======================================================================
+//DigitalIn DI_0(PF_5);
+DigitalIn DI_1(PC_4);
+DigitalIn DI_2(PB_13);
+DigitalIn DI_3(PB_14);
+DigitalIn DI_4(PB_15);
+DigitalIn DI_5(PB_1);
+DigitalIn DI_6(PB_2);
+DigitalIn DI_7(PB_11);
+DigitalIn DI_8(PB_12);
+DigitalIn DI_9(PA_11);
+DigitalIn DI_10(PA_12);
+//DigitalIn DI_11(PD_8);
+DigitalIn DI_12(PC_5);
+DigitalIn DI_13(PC_6);
+DigitalIn DI_14(PC_8);
+DigitalIn DI_15(PC_9);
-//======================================================================
-std_msgs::Float32 VelAngular_R;
-ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
-//======================================================================
-//======================================================================
-sensor_msgs::BatteryState BTState;
-ros::Publisher BT_pub("BatteryState", &BTState);
-//======================================================================
+//const unsigned int RX_ID = 0x792;
+int id_count[15];
+long Radial_range[15];
+long Radial_speed[15];
+long Radial_Acc[15];
+long Angle[15];
+long Power[15];
+long tmp_dis;
+long dis[7];
+long ldata;
+float fdata1,fdata2;
+float fRange[15],fAngle[15];
-//======================================================================
-std_msgs::Int16 DI;
-ros::Publisher DI_pub("DI_pub", &DI);
-//======================================================================
+//=========Battery varable================
+uint32_t SOC;
+uint32_t Tempert;
+uint32_t RackVoltage = 0;
+uint32_t Current = 0;
+uint32_t MaxCellV = 0;
+uint32_t MinCellV = 0;
+uint32_t seq;
+//========================================
-//======================================================================
-std_msgs::Int16 ACT_state;
-ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
-//======================================================================
+void CAN_thread(const void *args);
+void DIO_thread(const void *args);
+//CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
+CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
+//CANMsg rxMsg;
+CANMessage rxMsg;
+osThreadDef(CAN_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
+osThreadDef(DIO_thread, osPriorityNormal, DEFAULT_STACK_SIZE);
//======================================================================
-std_msgs::Int16 Error_state;
-ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
-//======================================================================
-
-uint32_t seq;
+ros::NodeHandle nh;
-//========define ACT_state return code============================================
-#define Satndby 0
-#define Busy 1
-#define Sensor_error 2
-#define BMS_error 3
-//========================================================
-#define IMU_FIFO_RATE_DIVIDER 0x09
-#define IMU_SAMPLE_RATE_DIVIDER 4
-#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
-#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
-
-#define PC_BAUDRATE 38400
-
-#define DEG_TO_RAD(x) ( x * 0.01745329 )
-#define RAD_TO_DEG(x) ( x * 57.29578 )
+std_msgs::Int16 str_msg1;
+std_msgs::Int16 str_msg2;
+std_msgs::Int16 str_msg3;
+std_msgs::Int16 DI_data;
+std_msgs::Int16 ACT_state;
+std_msgs::Int16 Error_state;
+std_msgs::Int16 DO;
+sensor_msgs::BatteryState BTState;
+
+ros::Publisher sonar1("sonar1", &str_msg1);
+ros::Publisher sonar2("sonar2", &str_msg2);
+ros::Publisher sonar3("sonar3", &str_msg3);
+ros::Publisher DI_data_pub("DI_data_pub", &DI_data);
+ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
+ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
+ros::Publisher BT_pub("BatteryState", &BTState);
-const int FIFO_BUFFER_SIZE = 128;
-uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
-uint16_t fifoCount;
-uint16_t packetSize;
-bool dmpReady;
-uint8_t mpuIntStatus;
-const int snprintf_buffer_size = 100;
-char snprintf_buffer[snprintf_buffer_size];
-uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
-int16_t ax, ay, az;
-int16_t gx, gy, gz;
-float Lrpm,Rrpm;
-float ticks_since_target;
-float motor_rpm_r, motor_rpm_l;
-float timeout_ticks;
-int counter;
- double w;
- double rate;
- double Dimeter;
- float dx,dy,dr;
-int lastsensorState = 1;
-int sensorState;
-int db_conter = 0;
-int buffer[9] = {0};
-int dataH,datanum;
-int motor_seq,motor_old_seq;
-int state_code;
-int error_code;
-//=========RS485
-char recChar=0;
-bool recFlag=false;
-char recArr[20];
-int index=0;
-int BMS_state;
- uint32_t SOC;
- uint32_t Tempert;
- uint32_t RackVoltage = 0;
- uint32_t Current = 0;
- uint32_t MaxCellV = 0;
- uint32_t MinCellV = 0;
-
-struct Offset {
- int16_t ax, ay, az;
- int16_t gx, gy, gz;
-}offset = {150+600, -350+300, 1000, -110-100, 5, 0};//{150, -350, 1000, -110, 5, 0}; // Measured values
-
-struct MPU6050_DmpData {
- Quaternion q;
- VectorFloat gravity; // g
- float roll, pitch, yaw; // rad
-}dmpData;
-
-long map(long x, long in_min, long in_max, long out_min, long out_max) {
- return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+void DO_ACT(const std_msgs::Int16 &msg){
+
+
+ DO_0 = (msg.data & 0x001);
+ DO_1 = (msg.data & 0x002);
+// DO_2 = (msg.data & 0x004);
+// DO_3 = (msg.data & 0x008);
+ DO_4 = (msg.data & 0x010);
+ DO_5 = (msg.data & 0x020);
+ DO_6 = (msg.data & 0x040);
+ DO_7 = (msg.data & 0x080);
+ DO_8 = (msg.data & 0x100);
+ DO_9 = (msg.data & 0x200);
+// DO_10 = (msg.data & 0x0400);
+ DO_12 = (msg.data & 0x1000);
+ DO_13 = (msg.data & 0x2000);
+ DO_14 = (msg.data & 0x4000);
+ DO_15 = (msg.data & 0x8000);
+
+ led1 = !led1;
+ osDelay(1);
}
-//==========define sub function========================
-bool Init();
-void dmpDataUpdate();
-unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen);
-int myabs( int a );
-void TwistToMotors();
-//===================================================
-
-
-//======================= motor =================================================
-unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
-{
- unsigned int i, j;
- //#define wPolynom 0xA001
- unsigned int wCrc = 0xffff;
- unsigned int wPolynom = 0xA001;
- /*---------------------------------------------------------------------------------*/
- for (i = 0; i < iBufLen; i++)
- {
- wCrc ^= cBuffer[i];
- for (j = 0; j < 8; j++)
- {
- if (wCrc &0x0001)
- {
- wCrc = (wCrc >> 1) ^ wPolynom;
- }
- else
- {
- wCrc = wCrc >> 1;
- }
- }
- }
- return wCrc;
-}
-void Sendmessage(float Rrpm,float Lrpm)
-{
-// RS232.printf("Wr = %.1f\n",Rrpm);
-// RS232.printf("Wl = %.1f\n",Lrpm);
-unsigned char sendData[16];
-unsigned int tmpCRC;
-int motor1,motor2;
-
- sendData[0] = Start;
- sendData[1] = Address;
- sendData[2] = ReturnType;
- sendData[3] = Clean;
- sendData[4] = Reserve;
- sendData[5] = 0x01;//motor1Sevro ON
- sendData[6] = 0x01;//motor2Sevro ON
-if (Rrpm>0){sendData[7] = 0x00;}else{sendData[7] = 0x01;}
-if (Lrpm>0){sendData[8] = 0x01;}else{sendData[8] = 0x00;}
- motor1 = abs(Rrpm);
- motor2 = abs(Lrpm);
-
- sendData[9] = (motor1>>8);//motor1speedH
- sendData[10] = (motor1 & 0xFF);//motor1speedL
- sendData[11] = (motor2>>8);//motor2speedH
- sendData[12] = (motor2 & 0xFF);//motor2speedL
- sendData[13] = End;
- tmpCRC = CRC_Verify(sendData, 14);
- sendData[14] = (tmpCRC & 0xFF);
- sendData[15] = (tmpCRC>>8);
- int i;
- for (i=0;i<16;i++)
- {
- RS232.printf("%c",sendData[i]);
- }
- RS232.printf("\r\n");
-}
-void TwistToMotors()
-{
- float right,left;
-// double vel_data[2];
- float vel_data[2];
- float motor_rpm_vx, motor_rpm_theta;
- motor_old_seq = motor_seq;
- w = 0.302;//0.2 ;//m
- rate = 20;//50;
- timeout_ticks = 2;
- Dimeter = 0.127;//0.15;
-
- // prevent agv receive weird 1.0 command from cmd_vel
- if (dr == 1.0){
- dr = 0.001;
- }
- right = ( 1.0 * dx ) + (dr * w /2);
- left = ( 1.0 * dx ) - (dr * w /2);
- motor_rpm_vx = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
- if((motor_rpm_vx !=0) && (myabs(motor_rpm_vx)<100)){
- if(motor_rpm_vx >0){
- motor_rpm_vx = 100;
+ros::Subscriber<std_msgs::Int16> DO_data_sub("DO_data_sub", &DO_ACT);
+//======================================================================
+void CAN_thread(const void *args){
+ while (true) {
+ if (can1.read(rxMsg)){
+ seq++;
+ SOC = rxMsg.data[0]>>1;
+ Tempert = rxMsg.data[1]-50;
+ RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2];
+ Current = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4];
+ MaxCellV = rxMsg.data[6];
+ MinCellV = rxMsg.data[7];
}
else{
- motor_rpm_vx = -100;
+ BTState.voltage = 0;
+ BTState.current = 0;
+ BTState.design_capacity = 80;
+ BTState.percentage = 0;
}
- }
- motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);
- motor_rpm_r = motor_rpm_vx+ motor_rpm_theta;
- motor_rpm_l = motor_rpm_vx- motor_rpm_theta;
- if (myabs(motor_rpm_r)<100|| myabs(motor_rpm_l)<100){
- if( dx==0){
- if(dr>0){
- motor_rpm_r=100;
- motor_rpm_l=-100;
- }else if (dr<0){
- motor_rpm_r=-100;
- motor_rpm_l=100;
- }else{
- motor_rpm_r=0;
- motor_rpm_l=0;
- }
- }
- else if(dx>0){
- if (myabs(motor_rpm_r)<100){
- motor_rpm_r =100;
- }
- if (myabs(motor_rpm_l)<100){
- motor_rpm_l =100;
- }
- }
- else{
- if(myabs(motor_rpm_r)<100){
- motor_rpm_r =-100;
- }
- if(myabs(motor_rpm_l)<100){
- motor_rpm_l =-100;
- }
- }
+ BTState.header.stamp = nh.now();
+ BTState.header.frame_id = 0;
+ BTState.header.seq = seq;
+ BTState.voltage = RackVoltage*0.1;
+ BTState.current = Current;
+ BTState.design_capacity = 50;
+ BTState.percentage = SOC;
+ osDelay(10);
}
- vel_data[0] = motor_rpm_r;
- vel_data[1] = motor_rpm_l;
-
-//===================================================================
- //Sendmessage(vel_data[0],vel_data[1]);
-
- //Sendmessage(motor_rpm_l,motor_rpm_r);
-
- VelAngular_R.data = vel_data[0];
- VelAngular_L.data = vel_data[1];
- //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
- //}
- //else{
- pub_rmotor.publish( &VelAngular_R );
- pub_lmotor.publish( &VelAngular_L );
- //}
- //RS232.printf("Wr = %.1f\n",vel_data[0]);
- //RS232.printf("Wl = %.1f\n",vel_data[1]);
- ticks_since_target += 1;
+}
-}
-int myabs( int a ){
- if ( a < 0 ){
- return -a;
- }
- return a;
- }
-
-int str2int(const char* str, int star, int end)
-{
- int i;
- int ret = 0;
- for (i = star; i < end+1; i++)
- {
- ret = ret *10 + (str[i] - '0');
+void DIO_thread(const void *args){
+ while (true) {
+ DI_data.data = 0xffff-0x0001-0x0800 - ((int)DI_1<<1) - (((int)DI_2)<<2) - (((int)DI_3)<<3) - (((int)DI_4)<<4);
+ DI_data.data -= (((int)DI_5)<<5);
+ DI_data.data -= (((int)DI_6)<<6);
+ DI_data.data -= (((int)DI_7)<<7);
+ DI_data.data -= (((int)DI_8)<<8);
+ DI_data.data -= (((int)DI_9)<<9);
+ DI_data.data -= (((int)DI_10)<<10);
+// DI_data.data -= (((int)DI_11)<<11);
+ DI_data.data -= (((int)DI_12)<<12);
+ DI_data.data -= (((int)DI_13)<<13);
+ DI_data.data -= (((int)DI_14)<<14);
+ DI_data.data -= (((int)DI_15)<<15);
+ osDelay(10);
}
- return ret;
-}
-
-void update_state(int code1,int code2){
-
}
//======================================================================
-std_msgs::Int16 DO;
-//DO_0 MAG_1
-//DO_1,MAG_2
-//DO_2,MAG_3
-//DO_3,BMS
-//DO_4,MainRelay
-int State;
-void DO_ACT(const std_msgs::Int16 &msg){
- //0xFF for action procedure
- if (msg.data == 0x21){
- error_code = 99;
- State = WAIT_BAT;//ACT_MG_ON;
- }
-
- if (msg.data == 0x20){
- error_code = 99;
- State = ACT_MG_OFF;
- }
-
- if (msg.data == 0x00){
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
- DO_3 = 0;
- DO_4 = 0;
- }
- if (msg.data == 0x40){
- //BMS trigger
- DO_3 = 1;
- wait(3);
- DO_3 = 0;
- }
- if (msg.data == 0x50){
- //Main Relay off
- DO_4 = 0;
- }
- if (msg.data == 0x51){
- //Main Relay on
- DO_4 = 1;
- }
- if (msg.data == 0x31){
- //Lock triggrt
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
- DO_0 = 0;
- DO_1 = 1;
- DO_2 = 0;
-
- wait_ms(500);
-
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
- }
-
- if (msg.data == 0x30){
- //unLock triggrt
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 1;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 1;
- DO_1 = 0;
- DO_2 = 1;
-
- wait_ms(500);
-
- DO_0 = 1;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
- }
-
-}
-ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
-//======================================================================
-//======================================================================================
-void messageCb(const geometry_msgs::Twist &msg)
-{
-// RS232.printf("messageCb");
- ticks_since_target = 0;
- dx = msg.linear.x;
- dy = msg.linear.y;
- dr = msg.angular.z;
-// RS232.printf("dx = %d,dy = %d,dr = %d\r\n",dx,dy,dr);
- TwistToMotors();
- //ReadENC(Motor1);
-}
-ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
- //======================================================================================
-void dmpDataUpdate() {
- // Check that this interrupt has enabled.
- if (dmpReady == false) return;
-
- mpuIntStatus = mpu.getIntStatus();
- fifoCount = mpu.getFIFOCount();
-
- // Check that this interrupt is a FIFO buffer overflow interrupt.
- if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
- mpu.resetFIFO();
- //pc.printf("FIFO overflow!\n");
- return;
-
- // Check that this interrupt is a Data Ready interrupt.
- } else if (mpuIntStatus & 0x02) {
- while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
-
- mpu.getFIFOBytes(fifoBuffer, packetSize);
-
- #ifdef OUTPUT_QUATERNION
- mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
- 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;
- pc.printf(snprintf_buffer);
- #endif
-
- #ifdef OUTPUT_EULER
- float euler[3];
- mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
- mpu.dmpGetEuler(euler, &dmpData.q);
- 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;
- pc.printf(snprintf_buffer);
- #endif
-
- #ifdef OUTPUT_ROLL_PITCH_YAW
- mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
- mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
- float rollPitchYaw[3];
- mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
- dmpData.roll = rollPitchYaw[2];
- dmpData.pitch = rollPitchYaw[1];
- dmpData.yaw = rollPitchYaw[0];
-
- 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;
- pc.printf(snprintf_buffer);
-
-#ifdef servotest
- int servoPulse = map((long)(RAD_TO_DEG(dmpData.yaw)*100), -9000, 9000, 500, 1450);
- if(servoPulse > 1450) servoPulse = 1450;
- if(servoPulse < 500) servoPulse = 500;
- sv.pulsewidth_us(servoPulse);
-#endif
- #endif
-
- #ifdef OUTPUT_FOR_TEAPOT
- teapotPacket[2] = fifoBuffer[0];
- teapotPacket[3] = fifoBuffer[1];
- teapotPacket[4] = fifoBuffer[4];
- teapotPacket[5] = fifoBuffer[5];
- teapotPacket[6] = fifoBuffer[8];
- teapotPacket[7] = fifoBuffer[9];
- teapotPacket[8] = fifoBuffer[12];
- teapotPacket[9] = fifoBuffer[13];
- for (uint8_t i = 0; i < 14; i++) {
- pc.putc(teapotPacket[i]);
- }
- #endif
-
- #ifdef OUTPUT_TEMPERATURE
- float temp = mpu.getTemperature() / 340.0 + 36.53;
- if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
- pc.printf(snprintf_buffer);
- #endif
-
-// pc.printf("\n");
- }
-}
-//=======================================================
-bool Init() {
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
- DO_3 = 0;
- DO_4 = 1;//for manual turn on the LiBattery, and turn on the main relay to open Load
- seq = 0;
- nh.initNode();
-// nh.advertise(imu_pub);
- nh.advertise(pub_lmotor);
- nh.advertise(pub_rmotor);
- nh.advertise(BT_pub);
- nh.advertise(DI_pub);
- nh.advertise(ACT_state_pub);
- nh.advertise(Error_state_pub);
- nh.subscribe(cmd_vel_sub);
- nh.subscribe(ACT_sub);
-
- //==========================
- /*
- mpu.initialize();
- if (mpu.testConnection()) {
-// pc.printf("MPU6050 test connection passed.\n");
- } else {
-// pc.printf("MPU6050 test connection failed.\n");
- return false;
- }
- if (mpu.dmpInitialize() == 0) {
-// pc.printf("succeed in MPU6050 DMP Initializing.\n");
- } else {
-// pc.printf("failed in MPU6050 DMP Initializing.\n");
- return false;
- }
-
- //mpu.setXAccelOffsetTC(offset.ax);
-// mpu.setYAccelOffsetTC(offset.ay);
-// mpu.setZAccelOffset(offset.az);
- mpu.setXAccelOffset(1000); //2000 -3134
- mpu.setYAccelOffset(0);
- mpu.setZAccelOffset(0);
- mpu.setXGyroOffset(800);//offset.gx);
- mpu.setYGyroOffset(0);//offset.gy);
- mpu.setZGyroOffset(0);//offset.gz);
-
-
-
- mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);//2000);
- mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
- mpu.setDMPEnabled(true); // Enable DMP
-
- packetSize = mpu.dmpGetFIFOPacketSize();
- dmpReady = true; // Enable interrupt.
- */
- //pc.printf("Init finish!\n");
-
- return true;
-}
-int MG_ACT(int state)
-{
-// int ret;
- if (state == 1){ //MAG_ON
-// if (sensorState == 0){ //Battery on-position
- //Lock triggrt
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 0;
- DO_1 = 1;
- DO_2 = 0;
-
- wait_ms(500);
-
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
-
- // if Battery is off, then do turn on
-// if (BMS_state == 0){
- //BMS ON
- DO_3 = 1;
- wait(4);
- DO_3 = 0;
-// }
- return 1;
-// }
- }
- else if (state == 2){//MAG_OFF
- // if Battery is on, then do trun off
-// if (BMS_state == 1){
- //BMS ON
- DO_3 = 1;
- wait(4);
- DO_3 = 0;
-// }
-
- //unLock triggrt
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 1;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 1;
- DO_1 = 0;
- DO_2 = 1;
-
- wait_ms(500);
-
- DO_0 = 1;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
-
- return 1;
- }
- return 0;
-}
-//=======================================================
int main() {
RS232.baud(PC_BAUDRATE);
- MBED_ASSERT(Init() == true);
- CANMessage rxMsg;
- DI_0.mode(PullUp);
+ nh.initNode();
+// nh.advertise(sonar1);
+// nh.advertise(sonar2);
+// nh.advertise(ACT_state_pub);
+ nh.advertise(DI_data_pub);
+ nh.advertise(BT_pub);
+ nh.subscribe(DO_data_sub);
+// DI_0.mode(PullUp);
+ DI_1.mode(PullUp);
+ DI_2.mode(PullUp);
+ DI_3.mode(PullUp);
+ DI_4.mode(PullUp);
+ DI_5.mode(PullUp);
+ DI_6.mode(PullUp);
+ DI_7.mode(PullUp);
+ DI_8.mode(PullUp);
+ DI_9.mode(PullUp);
+ DI_10.mode(PullUp);
+// DI_11.mode(PullUp);
+ DI_12.mode(PullUp);
+ DI_13.mode(PullUp);
+ DI_14.mode(PullUp);
+ DI_15.mode(PullUp);
+ wait_ms(10);
+ osThreadCreate(osThread(CAN_thread), NULL);
+ osThreadCreate(osThread(DIO_thread), NULL);
+// CANMessage rxMsg;
CAN_T = 0;
CAN_R = 0;
wait_ms(50);
- CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
+// CAN can1(D15,D14);//PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
wait_ms(50);
can1.frequency(500000);
wait_ms(50);
- //Lock triggrt
- wait_ms(500);
-
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
-
- DO_0 = 0;
- DO_1 = 1;
- DO_2 = 0;
-
- wait_ms(500);
-
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
-
- wait_ms(500);
-
while(1){
- seq++;
- motor_seq = seq;
- t.start();
- //================================
- //========define ACT_state return code============================================
- //#define Satndby 0x00
- //#define Busy 0x01
- //#define Sensor_error 0x10
- //========================================================
- counter++;
- state_code = State;
- switch (State){
- int ret;
- case IDLE:
- counter = 0;
- break;
-
- case WAIT_BAT:
- if (sensorState == 0){
- State = ACT_MG_ON;
- counter = 0;
- }
- if (counter>1000){
- State = IDLE;
- error_code = Sensor_error;
- }
- break;
-
- case ACT_MG_ON:
- ret = MG_ACT(1);
- if (ret){
-// DO_4 = 0;
- State = Check_BMS_ON;
- counter = 0;
- }
- if (counter>10){
- State = IDLE;
- error_code = Sensor_error;
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
- DO_3 = 0;
- DO_4 = 0;
- }
- break;
-
- case Check_BMS_ON:
- if (BMS_state == 1){
- // turn on parrel wire to share the UPS current
- DO_4 = 1;
- State = IDLE;
- error_code = 0;
- }
- if (counter>100){
- State = IDLE;
- error_code = BMS_error;
- DO_0 = 0;
- DO_1 = 0;
- DO_2 = 0;
- DO_3 = 0;
- DO_4 = 0;
- }
- break;
-
- case ACT_MG_OFF:
- // turn off parrel wire to avoid voltage feedback to UPS
- DO_4 = 0;
-
- ret = MG_ACT(2);
- if (ret){
- State = Check_BMS_OFF;
- counter = 0;
- }
- if (counter>10){
- State = IDLE;
- error_code = Sensor_error;
- }
- break;
-
- case Check_BMS_OFF:
- if (BMS_state == 0){
- State = IDLE;
- error_code = 0;
- }
- if (counter>100){
- State = IDLE;
- error_code = BMS_error;
- }
- break;
- }
-
- ACT_state.data = state_code;
- ACT_state_pub.publish( &ACT_state);
- wait_ms(10);
- Error_state.data = error_code;
- Error_state_pub.publish( &Error_state);
- wait_ms(10);
- //============DI==================
- int reading = DI_0;
- if (reading == lastsensorState) {
- db_conter = db_conter+1;
- }
- else{
- db_conter = 0;
- }
- if (db_conter > 3) {
- sensorState = reading;
- DI.data = sensorState;
- }
-
- lastsensorState = reading;
- DI_pub.publish( &DI);
- wait_ms(10);
- //=========================================
- if (can1.read(rxMsg) && (t.read_ms() > 5000)) {
- if(rxMsg.id == CAN_DATA){
- BMS_state = 1;
- SOC = rxMsg.data[0]>>1;
- Tempert = rxMsg.data[1]-50;
- RackVoltage = ((unsigned int)rxMsg.data[3] << 8) + (unsigned int)rxMsg.data[2];
- Current = ((unsigned int)rxMsg.data[5] << 8) + (unsigned int)rxMsg.data[4];
- MaxCellV = rxMsg.data[6];
- MinCellV = rxMsg.data[7];
- BTState.header.stamp = nh.now();
- BTState.header.frame_id = 0;
- BTState.header.seq = seq;
- BTState.voltage = RackVoltage*0.1;
- BTState.current = Current;
- BTState.design_capacity = 80;
- BTState.percentage = SOC;
- BT_pub.publish( &BTState );
- t.reset();
- } // if
- } // if
- else{
- BMS_state = 0;
- }
-
-
- if (motor_seq > motor_old_seq + 5){
- motor_rpm_r = 0;
- motor_rpm_l = 0;
- }
-
- Sendmessage(motor_rpm_l,motor_rpm_r);
- wait_ms(70);
- //wait_ms(100);
- nh.spinOnce();
+ DI_data_pub.publish( &DI_data);
+// sonar1.publish( &str_msg1);
+// sonar2.publish( &str_msg2);
+ BT_pub.publish( &BTState );
+ nh.spinOnce();
+ osDelay(50);
}
}
\ No newline at end of file