using NucleoF103
Dependencies: mbed ros_lib_kinetic
main.cpp
- Committer:
- WeberYang
- Date:
- 2018-04-12
- Revision:
- 0:576de68cbb45
File content as of revision 0:576de68cbb45:
#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; ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L); ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R); 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.2 ; rate = 50; timeout_ticks = 2; Dimeter = 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"); } void TwistToMotors() { float right,left; double vel_data[2]; 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; 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 (1) { if (t.read_ms() > vel_timer) { vel_timer = t.read_ms() + 500; } nh.spinOnce(); wait_ms(10); } }