AGV using ros with TROY motor
Dependencies: mbed ros_lib_kinetic
main.cpp
- Committer:
- WeberYang
- Date:
- 2018-04-12
- Revision:
- 0:85456b971234
- Child:
- 2:fcf8a7739eed
File content as of revision 0:85456b971234:
#include "mbed.h" #include <ros.h> #include <ros/time.h> #include <std_msgs/Empty.h> #include <std_msgs/String.h> #include<geometry_msgs/Twist.h> //set buffer larger than 50byte #include <std_msgs/Float32.h> #include <math.h> #include <stdio.h> #include <string.h> #include <iostream> #define Start 0xAA #define Address 0x7F #define ReturnType 0x00 #define Clean 0x00 #define Reserve 0x00 #define End 0x55 Serial RS232 (PB_10, PB_11,115200); // This one works std_msgs::String str_msg; std_msgs::Float32 VelAngular_L; std_msgs::Float32 VelAngular_R; // ============= ============= float motor_rpm_r, motor_rpm_l; // ============= ============= ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L); ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R); int seq = 0; // monitor times of callbacks int old_seq = 0; // monitor times of the command ros::NodeHandle nh; geometry_msgs::Twist vel; //float left; //float right; float Lrpm,Rrpm; float ticks_since_target; double timeout_ticks; double w; double rate; double Dimeter; float dx,dy,dr; void init_variables(); void Sendmessage(float Rrpm,float Lrpm); 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; } /* 起始碼(1Byte): AA 地址碼(1Byte): 01 返回數據類型(1Byte):從站驅動器不返回數據 00 故障清除(1Byte)默認發送: 00 保留位(1Byte) 00 A 馬達過載控制(1Byte)驅動器使能狀態 01 B 馬達控制(1Byte) 01 A 運行方向(1Byte)00:正轉,01反轉 00 B 運行方向(1Byte)00:正轉,01反轉 00 運行轉速(2Byte)是100–3000r/min, 12 34 運行轉速(2Byte)是100–3000r/min, 12 34 第 1 個 Byte:速度值高 8 位; 第 2 個 Byte:速度值低 8 位; 無效(2Byte)00:無效數據位。 00 結束碼(1Byte)55:通訊結束編碼識別。 55 校驗碼(2Byte)CRC16 */ // void spinOnce(); // void twistCallback(const geometry_msgs::Twist &twist_aux); void init_variables() { dx = dy = dr =0; w = 0.302;//0.2 ;//m rate = 20;//50; timeout_ticks = 2; Dimeter = 0.127;//0.15; } 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] = 0x01;}else{sendData[7] = 0x00;} if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;} 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"); } int myabs( int a ){ if ( a < 0 ){ return -a; } return a; } void TwistToMotors() { old_seq = seq; seq = seq + 1; //int lower_bound = 100; //int upper_bound = 300; float right,left; // float motor_rpm_r, motor_rpm_l; //double vel_data[2]; float vel_data[2]; float motor_rpm_vx, motor_rpm_theta, motor_rpm_v; // prevent agv receive weird 1.0 command from cmd_vel right = ( 1.0 * dx ) + (dr * w /2); left = ( 1.0 * dx ) - (dr * w /2); motor_rpm_v = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416); if((motor_rpm_v!=0) && (myabs(motor_rpm_v)<100)){ if (motor_rpm_v > 0) motor_rpm_v=100; else motor_rpm_v = -100; } motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416); //motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416); //motor_rpm_l = left*rate/(Dimeter/2)*60/(2*3.1416); motor_rpm_r = motor_rpm_v + motor_rpm_theta; motor_rpm_l = motor_rpm_v - 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; motor_rpm_l=motor_rpm_l+(100-motor_rpm_r); } if ( myabs(motor_rpm_l) < 100 ){ motor_rpm_l = 100; motor_rpm_r=motor_rpm_r+(100-motor_rpm_l); } }else{ if ( myabs(motor_rpm_r) < 100 ){ motor_rpm_r = -100; motor_rpm_l=motor_rpm_l+(-100-motor_rpm_r); } if (myabs(motor_rpm_l) < 100 ){ motor_rpm_l = -100; motor_rpm_r=motor_rpm_r+(-100-motor_rpm_l); } } } /* if ( myabs(motor_rpm_r) == myabs(motor_rpm_l) && myabs(motor_rpm_r) < 100){ int s = myabs(motor_rpm_r); motor_rpm_r = motor_rpm_r / s * 101; motor_rpm_l = motor_rpm_l / s * 101; } */ /* if ( myabs(vel_data[0]) < lower_bound ) { // if right wheel rpm < lower_bound int a = 0; a = lower_bound - myabs( vel_data[0] ); if (vel_data[0] > 0) { vel_data[0] = lower_bound; } else { vel_data[0] = -lower_bound; } if (vel_data[1] > 0) { vel_data[1] = vel_data[1] + a; } else { vel_data[1] = vel_data[1] - a; } // } else if ( myabs(vel_data[0]) > upper_bound ) { // if right wheel rpm > upper_bound int a = 0; a = myabs( vel_data[0] ) - upper_bound; if (vel_data[0] > 0) { vel_data[0] = upper_bound; } else { vel_data[0] = -upper_bound; } if (vel_data[1] > 0) { vel_data[1] = vel_data[1] - a; } else { vel_data[1] = vel_data[1] + a; } } if ( myabs(vel_data[1]) < lower_bound ) { // if left wheel rpm < lower_bound int a = 0; a = lower_bound - myabs( vel_data[1] ); if (vel_data[1] > 0) { vel_data[1] = lower_bound; } else { vel_data[1] = -lower_bound; } if (vel_data[0] > 0) { vel_data[0] = vel_data[0] + a; } else { vel_data[0] = vel_data[0] - a; } // } else if (myabs(vel_data[1]) > upper_bound ) { // if left wheel rpm > upper_bound int a = 0; a = myabs( vel_data[1] ) - upper_bound; if (vel_data[1] > 0) { vel_data[1] = upper_bound; } else { vel_data[1] = -upper_bound; } if (vel_data[0] > 0) { vel_data[0] = vel_data[0] - a; } else { vel_data[0] = vel_data[0] + a; } } */ /* if ( myabs(motor_rpm_r) < 100 || myabs(motor_rpm_l) < 100 ){ float a = 0.0; if ( myabs(motor_rpm_r) < myabs(motor_rpm_l) ){ if ( motor_rpm_r < 0 ){ a = -100 - motor_rpm_r; }else if( motor_rpm_r > 0) { a = 100 - motor_rpm_r; }else { a = 0; } motor_rpm_r = motor_rpm_r + a; motor_rpm_l = motor_rpm_l + a; }else if ( myabs(motor_rpm_r) > myabs(motor_rpm_l) ){ if ( motor_rpm_l < 0 ){ a = -100 - motor_rpm_l; }else if( motor_rpm_l > 0) { a = 100 - motor_rpm_l; }else { a = 0; } motor_rpm_l = motor_rpm_l + a; motor_rpm_r = motor_rpm_r + a; } // go straight < 100 if ( motor_rpm_r == motor_rpm_l ){ if ( motor_rpm_l < 0 ){ motor_rpm_l = -100; motor_rpm_r = -100; }else if( motor_rpm_l > 0) { motor_rpm_l = 100; motor_rpm_r = 100; }else { motor_rpm_l = 0; motor_rpm_r = 0; } } // turn < 100 if ( motor_rpm_r == -motor_rpm_l ){ if ( motor_rpm_l < 0 ){ motor_rpm_l = -100; motor_rpm_r = 100; }else if( motor_rpm_l > 0) { motor_rpm_l = 100; motor_rpm_r = -100; }else { motor_rpm_l = 0; motor_rpm_r = 0; } } // } if ( myabs(motor_rpm_r) > 600 ){ if ( motor_rpm_r < 0 ){ motor_rpm_r = -100 * log( float(myabs(motor_rpm_r)) ); }else{ motor_rpm_r = 100 * log( float(myabs(motor_rpm_r)) ); } } if ( myabs(motor_rpm_l) > 600) { if ( motor_rpm_l < 0 ){ motor_rpm_l = -100 * log( float(myabs(motor_rpm_l)) ); }else{ motor_rpm_l = 100 * log( float(myabs(motor_rpm_l)) ); } } */ vel_data[0] = motor_rpm_r; vel_data[1] = motor_rpm_l; //=================================================================== //Sendmessage(vel_data[0],vel_data[1]); 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; } void messageCb(const geometry_msgs::Twist &msg) { ticks_since_target = 0; dx = msg.linear.x; dy = msg.linear.y; dr = msg.angular.z; //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr); TwistToMotors(); } void ACT( const std_msgs::String& ACT) { char sendData[16]; //sendData = ACT; int motor1,motor2; sendData[0] = Start; sendData[1] = Address; sendData[2] = ReturnType; sendData[3] = Clean; sendData[4] = Reserve; sendData[5] = 0x00;//motor1Sevro OFF sendData[6] = 0x00;//motor2Sevro OFF motor1 = 0; motor2 = 0; sendData[9] = (motor1>>8);//motor1speedH sendData[10] = (motor1 & 0xFF);//motor1speedL sendData[11] = (motor2>>8);//motor2speedH sendData[12] = (motor2 & 0xFF);//motor2speedL sendData[13] = End; sendData[14] = Start; sendData[15] = Start; int i; for (i=0;i<16;i++) { RS232.printf("%c",sendData[i]); } RS232.printf("\r\n"); } ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb); //ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT ); Timer t; int main(int argc, char **argv) { long vel_timer = 0; init_variables(); t.start(); nh.initNode(); nh.advertise(pub_lmotor); nh.advertise(pub_rmotor); nh.subscribe(cmd_vel_sub); //nh.subscribe(sub_action); while (true) { //if (t.read_ms() > vel_timer) //{ // vel_timer = t.read_ms() + 50; // TwistToMotors(); // } nh.spinOnce(); if (seq +50 < old_seq){ motor_rpm_r = 0; motor_rpm_l = 0; } Sendmessage(motor_rpm_r,motor_rpm_l); old_seq = old_seq + 1; wait_ms(10); } }