![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
rag
Dependencies: mbed ros_lib_kinetic
main.cpp@0:4ff8854a49e8, 2018-04-12 (annotated)
- Committer:
- WeberYang
- Date:
- Thu Apr 12 00:57:02 2018 +0000
- Revision:
- 0:4ff8854a49e8
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
WeberYang | 0:4ff8854a49e8 | 1 | #include "mbed.h" |
WeberYang | 0:4ff8854a49e8 | 2 | #include <ros.h> |
WeberYang | 0:4ff8854a49e8 | 3 | #include <ros/time.h> |
WeberYang | 0:4ff8854a49e8 | 4 | #include <std_msgs/Empty.h> |
WeberYang | 0:4ff8854a49e8 | 5 | #include <std_msgs/String.h> |
WeberYang | 0:4ff8854a49e8 | 6 | #include<geometry_msgs/Twist.h> //set buffer larger than 50byte |
WeberYang | 0:4ff8854a49e8 | 7 | #include <std_msgs/Float32.h> |
WeberYang | 0:4ff8854a49e8 | 8 | #include <math.h> |
WeberYang | 0:4ff8854a49e8 | 9 | #include <stdio.h> |
WeberYang | 0:4ff8854a49e8 | 10 | #include <string.h> |
WeberYang | 0:4ff8854a49e8 | 11 | #include <iostream> |
WeberYang | 0:4ff8854a49e8 | 12 | #define Start 0xAA |
WeberYang | 0:4ff8854a49e8 | 13 | #define Address 0x7F |
WeberYang | 0:4ff8854a49e8 | 14 | #define ReturnType 0x00 |
WeberYang | 0:4ff8854a49e8 | 15 | #define Clean 0x00 |
WeberYang | 0:4ff8854a49e8 | 16 | #define Reserve 0x00 |
WeberYang | 0:4ff8854a49e8 | 17 | #define End 0x55 |
WeberYang | 0:4ff8854a49e8 | 18 | |
WeberYang | 0:4ff8854a49e8 | 19 | Serial RS232 (PB_10, PB_11,115200); // This one works |
WeberYang | 0:4ff8854a49e8 | 20 | std_msgs::String str_msg; |
WeberYang | 0:4ff8854a49e8 | 21 | std_msgs::Float32 VelAngular_L; |
WeberYang | 0:4ff8854a49e8 | 22 | std_msgs::Float32 VelAngular_R; |
WeberYang | 0:4ff8854a49e8 | 23 | ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L); |
WeberYang | 0:4ff8854a49e8 | 24 | ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R); |
WeberYang | 0:4ff8854a49e8 | 25 | |
WeberYang | 0:4ff8854a49e8 | 26 | ros::NodeHandle nh; |
WeberYang | 0:4ff8854a49e8 | 27 | geometry_msgs::Twist vel; |
WeberYang | 0:4ff8854a49e8 | 28 | //float left; |
WeberYang | 0:4ff8854a49e8 | 29 | //float right; |
WeberYang | 0:4ff8854a49e8 | 30 | float Lrpm,Rrpm; |
WeberYang | 0:4ff8854a49e8 | 31 | float ticks_since_target; |
WeberYang | 0:4ff8854a49e8 | 32 | double timeout_ticks; |
WeberYang | 0:4ff8854a49e8 | 33 | |
WeberYang | 0:4ff8854a49e8 | 34 | double w; |
WeberYang | 0:4ff8854a49e8 | 35 | double rate; |
WeberYang | 0:4ff8854a49e8 | 36 | double Dimeter; |
WeberYang | 0:4ff8854a49e8 | 37 | float dx,dy,dr; |
WeberYang | 0:4ff8854a49e8 | 38 | void init_variables(); |
WeberYang | 0:4ff8854a49e8 | 39 | void Sendmessage(float Rrpm,float Lrpm); |
WeberYang | 0:4ff8854a49e8 | 40 | |
WeberYang | 0:4ff8854a49e8 | 41 | unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen) |
WeberYang | 0:4ff8854a49e8 | 42 | { |
WeberYang | 0:4ff8854a49e8 | 43 | unsigned int i, j; |
WeberYang | 0:4ff8854a49e8 | 44 | //#define wPolynom 0xA001 |
WeberYang | 0:4ff8854a49e8 | 45 | unsigned int wCrc = 0xffff; |
WeberYang | 0:4ff8854a49e8 | 46 | unsigned int wPolynom = 0xA001; |
WeberYang | 0:4ff8854a49e8 | 47 | /*---------------------------------------------------------------------------------*/ |
WeberYang | 0:4ff8854a49e8 | 48 | for (i = 0; i < iBufLen; i++) |
WeberYang | 0:4ff8854a49e8 | 49 | { |
WeberYang | 0:4ff8854a49e8 | 50 | wCrc ^= cBuffer[i]; |
WeberYang | 0:4ff8854a49e8 | 51 | for (j = 0; j < 8; j++) |
WeberYang | 0:4ff8854a49e8 | 52 | { |
WeberYang | 0:4ff8854a49e8 | 53 | if (wCrc &0x0001) |
WeberYang | 0:4ff8854a49e8 | 54 | { |
WeberYang | 0:4ff8854a49e8 | 55 | wCrc = (wCrc >> 1) ^ wPolynom; |
WeberYang | 0:4ff8854a49e8 | 56 | } |
WeberYang | 0:4ff8854a49e8 | 57 | else |
WeberYang | 0:4ff8854a49e8 | 58 | { |
WeberYang | 0:4ff8854a49e8 | 59 | wCrc = wCrc >> 1; |
WeberYang | 0:4ff8854a49e8 | 60 | } |
WeberYang | 0:4ff8854a49e8 | 61 | } |
WeberYang | 0:4ff8854a49e8 | 62 | } |
WeberYang | 0:4ff8854a49e8 | 63 | return wCrc; |
WeberYang | 0:4ff8854a49e8 | 64 | } |
WeberYang | 0:4ff8854a49e8 | 65 | |
WeberYang | 0:4ff8854a49e8 | 66 | /* |
WeberYang | 0:4ff8854a49e8 | 67 | 起始碼(1Byte): AA |
WeberYang | 0:4ff8854a49e8 | 68 | 地址碼(1Byte): 01 |
WeberYang | 0:4ff8854a49e8 | 69 | 返回數據類型(1Byte):從站驅動器不返回數據 00 |
WeberYang | 0:4ff8854a49e8 | 70 | 故障清除(1Byte)默認發送: 00 |
WeberYang | 0:4ff8854a49e8 | 71 | 保留位(1Byte) 00 |
WeberYang | 0:4ff8854a49e8 | 72 | A 馬達過載控制(1Byte)驅動器使能狀態 01 |
WeberYang | 0:4ff8854a49e8 | 73 | B 馬達控制(1Byte) 01 |
WeberYang | 0:4ff8854a49e8 | 74 | A 運行方向(1Byte)00:正轉,01反轉 00 |
WeberYang | 0:4ff8854a49e8 | 75 | B 運行方向(1Byte)00:正轉,01反轉 00 |
WeberYang | 0:4ff8854a49e8 | 76 | 運行轉速(2Byte)是100–3000r/min, 12 34 |
WeberYang | 0:4ff8854a49e8 | 77 | 運行轉速(2Byte)是100–3000r/min, 12 34 |
WeberYang | 0:4ff8854a49e8 | 78 | 第 1 個 Byte:速度值高 8 位; |
WeberYang | 0:4ff8854a49e8 | 79 | 第 2 個 Byte:速度值低 8 位; |
WeberYang | 0:4ff8854a49e8 | 80 | 無效(2Byte)00:無效數據位。 00 |
WeberYang | 0:4ff8854a49e8 | 81 | 結束碼(1Byte)55:通訊結束編碼識別。 55 |
WeberYang | 0:4ff8854a49e8 | 82 | 校驗碼(2Byte)CRC16 |
WeberYang | 0:4ff8854a49e8 | 83 | */ |
WeberYang | 0:4ff8854a49e8 | 84 | |
WeberYang | 0:4ff8854a49e8 | 85 | // void spinOnce(); |
WeberYang | 0:4ff8854a49e8 | 86 | // void twistCallback(const geometry_msgs::Twist &twist_aux); |
WeberYang | 0:4ff8854a49e8 | 87 | |
WeberYang | 0:4ff8854a49e8 | 88 | |
WeberYang | 0:4ff8854a49e8 | 89 | void init_variables() |
WeberYang | 0:4ff8854a49e8 | 90 | { |
WeberYang | 0:4ff8854a49e8 | 91 | dx = dy = dr =0; |
WeberYang | 0:4ff8854a49e8 | 92 | w = 0.302;//0.2 ;//m |
WeberYang | 0:4ff8854a49e8 | 93 | rate = 20;//50; |
WeberYang | 0:4ff8854a49e8 | 94 | timeout_ticks = 2; |
WeberYang | 0:4ff8854a49e8 | 95 | Dimeter = 0.127;//0.15; |
WeberYang | 0:4ff8854a49e8 | 96 | } |
WeberYang | 0:4ff8854a49e8 | 97 | void Sendmessage(float Rrpm,float Lrpm) |
WeberYang | 0:4ff8854a49e8 | 98 | { |
WeberYang | 0:4ff8854a49e8 | 99 | //RS232.printf("Wr = %.1f\n",Rrpm); |
WeberYang | 0:4ff8854a49e8 | 100 | //RS232.printf("Wl = %.1f\n",Lrpm); |
WeberYang | 0:4ff8854a49e8 | 101 | unsigned char sendData[16]; |
WeberYang | 0:4ff8854a49e8 | 102 | unsigned int tmpCRC; |
WeberYang | 0:4ff8854a49e8 | 103 | int motor1,motor2; |
WeberYang | 0:4ff8854a49e8 | 104 | |
WeberYang | 0:4ff8854a49e8 | 105 | sendData[0] = Start; |
WeberYang | 0:4ff8854a49e8 | 106 | sendData[1] = Address; |
WeberYang | 0:4ff8854a49e8 | 107 | sendData[2] = ReturnType; |
WeberYang | 0:4ff8854a49e8 | 108 | sendData[3] = Clean; |
WeberYang | 0:4ff8854a49e8 | 109 | sendData[4] = Reserve; |
WeberYang | 0:4ff8854a49e8 | 110 | sendData[5] = 0x01;//motor1Sevro ON |
WeberYang | 0:4ff8854a49e8 | 111 | sendData[6] = 0x01;//motor2Sevro ON |
WeberYang | 0:4ff8854a49e8 | 112 | if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;} |
WeberYang | 0:4ff8854a49e8 | 113 | if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;} |
WeberYang | 0:4ff8854a49e8 | 114 | motor1 = abs(Rrpm); |
WeberYang | 0:4ff8854a49e8 | 115 | motor2 = abs(Lrpm); |
WeberYang | 0:4ff8854a49e8 | 116 | |
WeberYang | 0:4ff8854a49e8 | 117 | sendData[9] = (motor1>>8);//motor1speedH |
WeberYang | 0:4ff8854a49e8 | 118 | sendData[10] = (motor1 & 0xFF);//motor1speedL |
WeberYang | 0:4ff8854a49e8 | 119 | sendData[11] = (motor2>>8);//motor2speedH |
WeberYang | 0:4ff8854a49e8 | 120 | sendData[12] = (motor2 & 0xFF);//motor2speedL |
WeberYang | 0:4ff8854a49e8 | 121 | sendData[13] = End; |
WeberYang | 0:4ff8854a49e8 | 122 | tmpCRC = CRC_Verify(sendData, 14); |
WeberYang | 0:4ff8854a49e8 | 123 | sendData[14] = (tmpCRC & 0xFF); |
WeberYang | 0:4ff8854a49e8 | 124 | sendData[15] = (tmpCRC>>8); |
WeberYang | 0:4ff8854a49e8 | 125 | int i; |
WeberYang | 0:4ff8854a49e8 | 126 | for (i=0;i<16;i++) |
WeberYang | 0:4ff8854a49e8 | 127 | { |
WeberYang | 0:4ff8854a49e8 | 128 | RS232.printf("%c",sendData[i]); |
WeberYang | 0:4ff8854a49e8 | 129 | } |
WeberYang | 0:4ff8854a49e8 | 130 | RS232.printf("\r\n"); |
WeberYang | 0:4ff8854a49e8 | 131 | } |
WeberYang | 0:4ff8854a49e8 | 132 | void TwistToMotors() |
WeberYang | 0:4ff8854a49e8 | 133 | { |
WeberYang | 0:4ff8854a49e8 | 134 | float right,left; |
WeberYang | 0:4ff8854a49e8 | 135 | float motor_rpm_r, motor_rpm_l; |
WeberYang | 0:4ff8854a49e8 | 136 | double vel_data[2]; |
WeberYang | 0:4ff8854a49e8 | 137 | right = ( 1.0 * dx ) + (dr * w /2); |
WeberYang | 0:4ff8854a49e8 | 138 | /* |
WeberYang | 0:4ff8854a49e8 | 139 | left = ( 1.0 * dx ) - (dr * w /2); |
WeberYang | 0:4ff8854a49e8 | 140 | vel_data[0] = right*rate/Dimeter/60*1000; |
WeberYang | 0:4ff8854a49e8 | 141 | vel_data[1] = left*rate/Dimeter/60*1000; |
WeberYang | 0:4ff8854a49e8 | 142 | */ |
WeberYang | 0:4ff8854a49e8 | 143 | if (dx!=0) |
WeberYang | 0:4ff8854a49e8 | 144 | { |
WeberYang | 0:4ff8854a49e8 | 145 | if (dx>0) |
WeberYang | 0:4ff8854a49e8 | 146 | { |
WeberYang | 0:4ff8854a49e8 | 147 | if (dr >=0) |
WeberYang | 0:4ff8854a49e8 | 148 | { |
WeberYang | 0:4ff8854a49e8 | 149 | motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; |
WeberYang | 0:4ff8854a49e8 | 150 | motor_rpm_l=300; |
WeberYang | 0:4ff8854a49e8 | 151 | } |
WeberYang | 0:4ff8854a49e8 | 152 | else |
WeberYang | 0:4ff8854a49e8 | 153 | { |
WeberYang | 0:4ff8854a49e8 | 154 | motor_rpm_r=300; |
WeberYang | 0:4ff8854a49e8 | 155 | motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; |
WeberYang | 0:4ff8854a49e8 | 156 | } |
WeberYang | 0:4ff8854a49e8 | 157 | } |
WeberYang | 0:4ff8854a49e8 | 158 | else |
WeberYang | 0:4ff8854a49e8 | 159 | { |
WeberYang | 0:4ff8854a49e8 | 160 | if(dr>=0) |
WeberYang | 0:4ff8854a49e8 | 161 | { |
WeberYang | 0:4ff8854a49e8 | 162 | motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; |
WeberYang | 0:4ff8854a49e8 | 163 | motor_rpm_l=(-300); |
WeberYang | 0:4ff8854a49e8 | 164 | } |
WeberYang | 0:4ff8854a49e8 | 165 | else |
WeberYang | 0:4ff8854a49e8 | 166 | { |
WeberYang | 0:4ff8854a49e8 | 167 | motor_rpm_r=(-300); |
WeberYang | 0:4ff8854a49e8 | 168 | motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60; |
WeberYang | 0:4ff8854a49e8 | 169 | } |
WeberYang | 0:4ff8854a49e8 | 170 | } |
WeberYang | 0:4ff8854a49e8 | 171 | } |
WeberYang | 0:4ff8854a49e8 | 172 | else |
WeberYang | 0:4ff8854a49e8 | 173 | { |
WeberYang | 0:4ff8854a49e8 | 174 | if(dr>=0) |
WeberYang | 0:4ff8854a49e8 | 175 | { |
WeberYang | 0:4ff8854a49e8 | 176 | motor_rpm_r=100; |
WeberYang | 0:4ff8854a49e8 | 177 | motor_rpm_l=-100; |
WeberYang | 0:4ff8854a49e8 | 178 | } |
WeberYang | 0:4ff8854a49e8 | 179 | else |
WeberYang | 0:4ff8854a49e8 | 180 | { |
WeberYang | 0:4ff8854a49e8 | 181 | motor_rpm_r=-100; |
WeberYang | 0:4ff8854a49e8 | 182 | motor_rpm_l=100; |
WeberYang | 0:4ff8854a49e8 | 183 | } |
WeberYang | 0:4ff8854a49e8 | 184 | } |
WeberYang | 0:4ff8854a49e8 | 185 | vel_data[0]=motor_rpm_r; |
WeberYang | 0:4ff8854a49e8 | 186 | vel_data[1]=motor_rpm_l; |
WeberYang | 0:4ff8854a49e8 | 187 | //================================================================ Original Version |
WeberYang | 0:4ff8854a49e8 | 188 | /*if (dr>=0) |
WeberYang | 0:4ff8854a49e8 | 189 | { |
WeberYang | 0:4ff8854a49e8 | 190 | right = ( 1.0 * dx ) + (dr * w /2); |
WeberYang | 0:4ff8854a49e8 | 191 | left = ( 1.0 * dx ); |
WeberYang | 0:4ff8854a49e8 | 192 | } |
WeberYang | 0:4ff8854a49e8 | 193 | else |
WeberYang | 0:4ff8854a49e8 | 194 | { |
WeberYang | 0:4ff8854a49e8 | 195 | right = ( 1.0 * dx ); |
WeberYang | 0:4ff8854a49e8 | 196 | left = ( 1.0 * dx ) - (dr * w /2); |
WeberYang | 0:4ff8854a49e8 | 197 | } |
WeberYang | 0:4ff8854a49e8 | 198 | vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60; |
WeberYang | 0:4ff8854a49e8 | 199 | vel_data[1] = left*rate/(Dimeter/2)/(2*3.1416)*60;*/ |
WeberYang | 0:4ff8854a49e8 | 200 | //=================================================================== |
WeberYang | 0:4ff8854a49e8 | 201 | Sendmessage(vel_data[0],vel_data[1]); |
WeberYang | 0:4ff8854a49e8 | 202 | VelAngular_R.data = vel_data[0]; |
WeberYang | 0:4ff8854a49e8 | 203 | VelAngular_L.data = vel_data[1]; |
WeberYang | 0:4ff8854a49e8 | 204 | //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){ |
WeberYang | 0:4ff8854a49e8 | 205 | //} |
WeberYang | 0:4ff8854a49e8 | 206 | //else{ |
WeberYang | 0:4ff8854a49e8 | 207 | pub_rmotor.publish( &VelAngular_R ); |
WeberYang | 0:4ff8854a49e8 | 208 | pub_lmotor.publish( &VelAngular_L ); |
WeberYang | 0:4ff8854a49e8 | 209 | //} |
WeberYang | 0:4ff8854a49e8 | 210 | //RS232.printf("Wr = %.1f\n",vel_data[0]); |
WeberYang | 0:4ff8854a49e8 | 211 | //RS232.printf("Wl = %.1f\n",vel_data[1]); |
WeberYang | 0:4ff8854a49e8 | 212 | ticks_since_target += 1; |
WeberYang | 0:4ff8854a49e8 | 213 | |
WeberYang | 0:4ff8854a49e8 | 214 | } |
WeberYang | 0:4ff8854a49e8 | 215 | |
WeberYang | 0:4ff8854a49e8 | 216 | void messageCb(const geometry_msgs::Twist &msg) |
WeberYang | 0:4ff8854a49e8 | 217 | { |
WeberYang | 0:4ff8854a49e8 | 218 | ticks_since_target = 0; |
WeberYang | 0:4ff8854a49e8 | 219 | dx = msg.linear.x; |
WeberYang | 0:4ff8854a49e8 | 220 | dy = msg.linear.y; |
WeberYang | 0:4ff8854a49e8 | 221 | dr = msg.angular.z; |
WeberYang | 0:4ff8854a49e8 | 222 | //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr); |
WeberYang | 0:4ff8854a49e8 | 223 | TwistToMotors(); |
WeberYang | 0:4ff8854a49e8 | 224 | } |
WeberYang | 0:4ff8854a49e8 | 225 | |
WeberYang | 0:4ff8854a49e8 | 226 | void ACT( const std_msgs::String& ACT) { |
WeberYang | 0:4ff8854a49e8 | 227 | char sendData[16]; |
WeberYang | 0:4ff8854a49e8 | 228 | //sendData = ACT; |
WeberYang | 0:4ff8854a49e8 | 229 | int motor1,motor2; |
WeberYang | 0:4ff8854a49e8 | 230 | |
WeberYang | 0:4ff8854a49e8 | 231 | sendData[0] = Start; |
WeberYang | 0:4ff8854a49e8 | 232 | sendData[1] = Address; |
WeberYang | 0:4ff8854a49e8 | 233 | sendData[2] = ReturnType; |
WeberYang | 0:4ff8854a49e8 | 234 | sendData[3] = Clean; |
WeberYang | 0:4ff8854a49e8 | 235 | sendData[4] = Reserve; |
WeberYang | 0:4ff8854a49e8 | 236 | sendData[5] = 0x00;//motor1Sevro OFF |
WeberYang | 0:4ff8854a49e8 | 237 | sendData[6] = 0x00;//motor2Sevro OFF |
WeberYang | 0:4ff8854a49e8 | 238 | motor1 = 0; |
WeberYang | 0:4ff8854a49e8 | 239 | motor2 = 0; |
WeberYang | 0:4ff8854a49e8 | 240 | sendData[9] = (motor1>>8);//motor1speedH |
WeberYang | 0:4ff8854a49e8 | 241 | sendData[10] = (motor1 & 0xFF);//motor1speedL |
WeberYang | 0:4ff8854a49e8 | 242 | sendData[11] = (motor2>>8);//motor2speedH |
WeberYang | 0:4ff8854a49e8 | 243 | sendData[12] = (motor2 & 0xFF);//motor2speedL |
WeberYang | 0:4ff8854a49e8 | 244 | sendData[13] = End; |
WeberYang | 0:4ff8854a49e8 | 245 | |
WeberYang | 0:4ff8854a49e8 | 246 | |
WeberYang | 0:4ff8854a49e8 | 247 | sendData[14] = Start; |
WeberYang | 0:4ff8854a49e8 | 248 | sendData[15] = Start; |
WeberYang | 0:4ff8854a49e8 | 249 | int i; |
WeberYang | 0:4ff8854a49e8 | 250 | for (i=0;i<16;i++) |
WeberYang | 0:4ff8854a49e8 | 251 | { |
WeberYang | 0:4ff8854a49e8 | 252 | RS232.printf("%c",sendData[i]); |
WeberYang | 0:4ff8854a49e8 | 253 | } |
WeberYang | 0:4ff8854a49e8 | 254 | RS232.printf("\r\n"); |
WeberYang | 0:4ff8854a49e8 | 255 | } |
WeberYang | 0:4ff8854a49e8 | 256 | ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb); |
WeberYang | 0:4ff8854a49e8 | 257 | ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT ); |
WeberYang | 0:4ff8854a49e8 | 258 | |
WeberYang | 0:4ff8854a49e8 | 259 | Timer t; |
WeberYang | 0:4ff8854a49e8 | 260 | int main(int argc, char **argv) |
WeberYang | 0:4ff8854a49e8 | 261 | { |
WeberYang | 0:4ff8854a49e8 | 262 | long vel_timer = 0; |
WeberYang | 0:4ff8854a49e8 | 263 | init_variables(); |
WeberYang | 0:4ff8854a49e8 | 264 | t.start(); |
WeberYang | 0:4ff8854a49e8 | 265 | nh.initNode(); |
WeberYang | 0:4ff8854a49e8 | 266 | nh.advertise(pub_lmotor); |
WeberYang | 0:4ff8854a49e8 | 267 | nh.advertise(pub_rmotor); |
WeberYang | 0:4ff8854a49e8 | 268 | nh.subscribe(cmd_vel_sub); |
WeberYang | 0:4ff8854a49e8 | 269 | nh.subscribe(sub_action); |
WeberYang | 0:4ff8854a49e8 | 270 | |
WeberYang | 0:4ff8854a49e8 | 271 | while (1) |
WeberYang | 0:4ff8854a49e8 | 272 | { |
WeberYang | 0:4ff8854a49e8 | 273 | if (t.read_ms() > vel_timer) |
WeberYang | 0:4ff8854a49e8 | 274 | { |
WeberYang | 0:4ff8854a49e8 | 275 | vel_timer = t.read_ms() + 500; |
WeberYang | 0:4ff8854a49e8 | 276 | } |
WeberYang | 0:4ff8854a49e8 | 277 | nh.spinOnce(); |
WeberYang | 0:4ff8854a49e8 | 278 | |
WeberYang | 0:4ff8854a49e8 | 279 | wait_ms(10); |
WeberYang | 0:4ff8854a49e8 | 280 | } |
WeberYang | 0:4ff8854a49e8 | 281 | } |