Weber Yang
start to work from here...
Dependencies: MPU6050-DMP mbed ros_lib_kinetic
Fork of
AGV_0411
by 
Weber Yang
Diff: main.cpp
- Revision:
- 9:e10bd4b460d7
- Parent:
- 8:4974fc24fbd7
- Child:
- 10:8b7fce3bba86
--- a/main.cpp Tue May 15 00:46:30 2018 +0000
+++ b/main.cpp Wed May 23 01:54:11 2018 +0000
@@ -40,14 +40,21 @@
#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
//Serial pc(USBTX,USBRX);
Timer t;
Serial RS232(PA_9, PA_10);
DigitalOut Receiver(D7); //RS485_E
DigitalOut CAN_T(D14);
DigitalOut CAN_R(D15);
-DigitalOut DO_0(PC_8);
-DigitalOut DO_1(PC_9);
+DigitalOut DO_0(PC_5);
+DigitalOut DO_1(PC_6);
+DigitalOut DO_2(PC_8);
+DigitalOut DO_3(PC_9);
DigitalIn DI_0(PB_13);
//CAN can1(PB_8,PB_9); // CAN Rx pin name, CAN Tx pin name
@@ -84,27 +91,23 @@
//======================================================================
//======================================================================
-std_msgs::Int16 DO;
-
-void DO_ACT(const std_msgs::Int16 &msg){
- if (msg.data & 0x01){
- DO_0 = 1;
- }
- else{
- DO_0 = 0;
- }
+std_msgs::Int16 ACT_state;
+ros::Publisher ACT_state_pub("ACT_state_pub", &ACT_state);
+//======================================================================
- if (msg.data & 0x02){
- DO_1 = 1;
- }
- else{
- DO_1 = 0;
- }
-}
-ros::Subscriber<std_msgs::Int16> ACT_sub("DO_data", &DO_ACT);
//======================================================================
+std_msgs::Int16 Error_state;
+ros::Publisher Error_state_pub("Error_state_pub", &Error_state);
+//======================================================================
+
uint32_t seq;
+//========define ACT_state return code============================================
+#define Satndby 0x00
+#define Busy 0x01
+#define Sensor_error 0x10
+#define BMS_error 0x11
+//========================================================
#define IMU_FIFO_RATE_DIVIDER 0x09
#define IMU_SAMPLE_RATE_DIVIDER 4
#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
@@ -128,23 +131,27 @@
int16_t gx, gy, gz;
float Lrpm,Rrpm;
float ticks_since_target;
-double timeout_ticks;
-
+float motor_rpm_r, motor_rpm_l;
+float timeout_ticks;
+int counter;
double w;
double rate;
double Dimeter;
float dx,dy,dr;
-int lastButtonState = 1;
-int buttonState;
+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;
@@ -155,7 +162,7 @@
struct Offset {
int16_t ax, ay, az;
int16_t gx, gy, gz;
-}offset = {150, -350, 1000, -110, 5, 0}; // Measured values
+}offset = {150+600, -350+300, 1000, -110-100, 5, 0};//{150, -350, 1000, -110, 5, 0}; // Measured values
struct MPU6050_DmpData {
Quaternion q;
@@ -202,8 +209,8 @@
}
void Sendmessage(float Rrpm,float Lrpm)
{
- //RS232.printf("Wr = %.1f\n",Rrpm);
- //RS232.printf("Wl = %.1f\n",Lrpm);
+// RS232.printf("Wr = %.1f\n",Rrpm);
+// RS232.printf("Wl = %.1f\n",Lrpm);
unsigned char sendData[16];
unsigned int tmpCRC;
int motor1,motor2;
@@ -215,8 +222,8 @@
sendData[4] = Reserve;
sendData[5] = 0x01;//motor1Sevro ON
sendData[6] = 0x01;//motor2Sevro ON
-if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
-if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
+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);
@@ -237,76 +244,72 @@
}
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;
- float right,left;
- float motor_rpm_r, motor_rpm_l;
- double vel_data[2];
+
+ // 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);
-// vel_data[0] = right*rate/Dimeter/60*1000;
-// vel_data[1] = left*rate/Dimeter/60*1000;
- if (dx!=0)
- {
- if (dx>0)
- {
- if (dr >=0)
- {
- motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
- motor_rpm_l=300;
- }
- else
- {
- motor_rpm_r=300;
- motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
- }
- }
- else
- {
- if(dr>=0)
- {
- motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
- motor_rpm_l=(-300);
- }
- else
- {
- motor_rpm_r=(-300);
- motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
- }
- }
- }
- else
- {
- if(dr>=0)
- {
- motor_rpm_r=100;
- motor_rpm_l=-100;
+ 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;
}
- else
- {
- motor_rpm_r=-100;
- motor_rpm_l=100;
+ else{
+ motor_rpm_vx = -100;
}
}
- vel_data[0]=motor_rpm_r;
- vel_data[1]=motor_rpm_l;
- //================================================================ Original Version
- /*if (dr>=0)
- {
- right = ( 1.0 * dx ) + (dr * w /2);
- left = ( 1.0 * dx );
+ 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;
+ }
+ }
}
- else
- {
- right = ( 1.0 * dx );
- left = ( 1.0 * dx ) - (dr * w /2);
- }
- vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60;
- vel_data[1] = left*rate/(Dimeter/2)/(2*3.1416)*60;*/
+ vel_data[0] = motor_rpm_r;
+ vel_data[1] = motor_rpm_l;
+
//===================================================================
- Sendmessage(vel_data[0],vel_data[1]);
+ //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){
@@ -320,7 +323,6 @@
ticks_since_target += 1;
}
-
int myabs( int a ){
if ( a < 0 ){
return -a;
@@ -338,13 +340,71 @@
}
return ret;
}
+
+void update_state(int code1,int code2){
+
+}
+//======================================================================
+std_msgs::Int16 DO;
+//DO_0 MAG_1
+//DO_1,MAG_2
+//DO_2,UPS
+//DO_3,BMS
+int State;
+void DO_ACT(const std_msgs::Int16 &msg){
+ //0xFF for action procedure
+ if (msg.data == 0x21){
+ error_code = 99;
+ State = ACT_MG_ON;
+ }
+
+ if (msg.data == 0x20){
+ error_code = 99;
+ State = ACT_MG_OFF;
+ }
+
+ if (msg.data<0x20){
+ if (msg.data & 0x01){
+ DO_0 = 1;
+ }
+ else{
+ DO_0 = 0;
+ }
+
+ if (msg.data & 0x02){
+ DO_1 = 1;
+ }
+ else{
+ DO_1 = 0;
+ }
+
+ if (msg.data & 0x04){
+ DO_2 = 1;
+ }
+ else{
+ DO_2 = 0;
+ }
+
+ if (msg.data & 0x08){
+ DO_3 = 1;
+ }
+ else{
+ DO_3 = 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);
}
@@ -433,14 +493,16 @@
seq = 0;
nh.initNode();
- nh.advertise(imu_pub);
+// 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");
@@ -454,22 +516,59 @@
// pc.printf("failed in MPU6050 DMP Initializing.\n");
return false;
}
- mpu.setXAccelOffset(offset.ax);
- mpu.setYAccelOffset(offset.ay);
- mpu.setZAccelOffset(offset.az);
- mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
+
+ //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.setXGyroOffset(offset.gx);
- mpu.setYGyroOffset(offset.gy);
- mpu.setZGyroOffset(offset.gz);
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 == 1){ //Battery on-position
+ //Lock triggrt
+ DO_1 = 1;
+ wait_ms(100);
+ DO_1 = 0;
+
+ DO_2 = 1;
+ wait(3);
+ DO_2 = 0;
+ return 1;
+ }
+ }
+ else if (state == 2){//MAG_OFF
+ //unLock triggrt
+ DO_2 = 1;
+ wait_ms(100);
+ DO_2 = 0;
+
+ DO_2 = 1;
+ wait(3);
+ DO_2 = 0;
+ return 1;
+ }
+ return 0;
+}
//=======================================================
int main() {
RS232.baud(PC_BAUDRATE);
@@ -483,59 +582,106 @@
wait_ms(50);
can1.frequency(500000);
wait_ms(50);
+ DO_2 = 1; //let UPS ON
+
+
+
while(1){
seq++;
+ motor_seq = seq;
t.start();
- mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-
- imu_msg.header.stamp = nh.now();
- imu_msg.header.frame_id = 0;
- imu_msg.header.seq = seq;
- imu_msg.accel.x = ax;
- imu_msg.accel.y = ay;
- imu_msg.accel.z = az;
- imu_msg.gyro.x = gx;
- imu_msg.gyro.y = gy;
- imu_msg.gyro.z = gz;
-//
- imu_pub.publish( &imu_msg );
+ //================================
+ //========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 ACT_MG_ON:
+ ret = MG_ACT(1);
+ if (ret){
+ State = Check_BMS_ON;
+ counter = 0;
+ }
+ if (counter>10){
+ State = IDLE;
+ error_code = Sensor_error;
+ }
+ break;
+
+ case Check_BMS_ON:
+ if (BMS_state == 1){
+ State = IDLE;
+ error_code = 0;
+ }
+ if (counter>10){
+ State = IDLE;
+ error_code = BMS_error;
+ }
+ break;
+
+ case ACT_MG_OFF:
+ 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>10){
+ State = IDLE;
+ error_code = BMS_error;
+ }
+ break;
+
+ }
+
+ ACT_state.data = state_code;
+ ACT_state_pub.publish( &ACT_state);
+ Error_state.data = error_code;
+ Error_state_pub.publish( &Error_state);
//============DI==================
int reading = DI_0;
- if (reading == lastButtonState) {
+ if (reading == lastsensorState) {
db_conter = db_conter+1;
}
else{
db_conter = 0;
- }
-
+ }
if (db_conter > 3) {
-// if (reading != buttonState) {
- buttonState = reading;
- DI.data = buttonState;
-// }
+ sensorState = reading;
+ DI.data = sensorState;
}
- lastButtonState = reading;
+ lastsensorState = reading;
DI_pub.publish( &DI);
//=========================================
if (can1.read(rxMsg) && (t.read_ms() > 5000)) {
-// RS232.printf(" ID = 0x%.3x\r\n", rxMsg.id);
-
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) + rxMsg.data[2];
Current = ((unsigned int)rxMsg.data[5] << 8) + rxMsg.data[4];
MaxCellV = rxMsg.data[6];
- MinCellV = rxMsg.data[7];
-// RS232.printf("SOC = %d\r\n",SOC);
-// RS232.printf("Tempert = %d\r\n",Tempert);
-// RS232.printf("RackVoltage = %.2f\r\n",RackVoltage*0.1);
-// RS232.printf("Current = %.2f\r\n",Current*0.1);
-// RS232.printf("MaxCellV = %.2f\r\n",MaxCellV*0.02);
-// RS232.printf("MinCellV = %.2f\r\n",MinCellV*0.02);
-
+ MinCellV = rxMsg.data[7];
BTState.header.stamp = nh.now();
BTState.header.frame_id = 0;
BTState.header.seq = seq;
@@ -547,7 +693,18 @@
t.reset();
} // if
} // if
- nh.spinOnce();
- wait_ms(10);
+ 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(100);
+ nh.spinOnce();
}
}
\ No newline at end of file