Weber Yang / Mbed 2 deprecated AGV_0306

AGV using ros with TROY motor

Dependencies:   mbed ros_lib_kinetic

main.cpp@2:fcf8a7739eed, 2018-05-02 (annotated)

Committer:
WeberYang
Date:
Wed May 02 06:12:23 2018 +0000
Revision:
2:fcf8a7739eed
Parent:
0:85456b971234 
new car first version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WeberYang 2:fcf8a7739eed 1 #include "mbed.h"
WeberYang 0:85456b971234 2 #include <ros.h>
WeberYang 0:85456b971234 3 #include <ros/time.h>
WeberYang 0:85456b971234 4 #include <std_msgs/Empty.h>
WeberYang 0:85456b971234 5 #include <std_msgs/String.h>
WeberYang 0:85456b971234 6 #include<geometry_msgs/Twist.h> //set buffer larger than 50byte
WeberYang 0:85456b971234 7 #include <std_msgs/Float32.h>
WeberYang 0:85456b971234 8 #include <math.h>
WeberYang 0:85456b971234 9 #include <stdio.h>
WeberYang 0:85456b971234 10 #include <string.h>
WeberYang 0:85456b971234 11 #include <iostream>
WeberYang 0:85456b971234 12 #define Start 0xAA
WeberYang 0:85456b971234 13 #define Address 0x7F
WeberYang 0:85456b971234 14 #define ReturnType 0x00
WeberYang 0:85456b971234 15 #define Clean 0x00
WeberYang 0:85456b971234 16 #define Reserve 0x00
WeberYang 0:85456b971234 17 #define End 0x55
WeberYang 0:85456b971234 18
WeberYang 0:85456b971234 19 Serial RS232 (PB_10, PB_11,115200); // This one works
WeberYang 0:85456b971234 20 std_msgs::String str_msg;
WeberYang 0:85456b971234 21 std_msgs::Float32 VelAngular_L;
WeberYang 0:85456b971234 22 std_msgs::Float32 VelAngular_R;
WeberYang 0:85456b971234 23
WeberYang 0:85456b971234 24 // ============= =============
WeberYang 0:85456b971234 25 float motor_rpm_r, motor_rpm_l;
WeberYang 0:85456b971234 26 // ============= =============
WeberYang 0:85456b971234 27
WeberYang 0:85456b971234 28 ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
WeberYang 0:85456b971234 29 ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
WeberYang 0:85456b971234 30 int seq = 0; // monitor times of callbacks
WeberYang 0:85456b971234 31 int old_seq = 0; // monitor times of the command
WeberYang 0:85456b971234 32
WeberYang 0:85456b971234 33
WeberYang 0:85456b971234 34 ros::NodeHandle nh;
WeberYang 0:85456b971234 35 geometry_msgs::Twist vel;
WeberYang 0:85456b971234 36 //float left;
WeberYang 0:85456b971234 37 //float right;
WeberYang 0:85456b971234 38 float Lrpm,Rrpm;
WeberYang 0:85456b971234 39 float ticks_since_target;
WeberYang 0:85456b971234 40 double timeout_ticks;
WeberYang 0:85456b971234 41
WeberYang 0:85456b971234 42 double w;
WeberYang 0:85456b971234 43 double rate;
WeberYang 0:85456b971234 44 double Dimeter;
WeberYang 0:85456b971234 45 float dx,dy,dr;
WeberYang 0:85456b971234 46 void init_variables();
WeberYang 0:85456b971234 47 void Sendmessage(float Rrpm,float Lrpm);
WeberYang 0:85456b971234 48
WeberYang 0:85456b971234 49 unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
WeberYang 0:85456b971234 50 {
WeberYang 0:85456b971234 51 unsigned int i, j;
WeberYang 0:85456b971234 52 //#define wPolynom 0xA001
WeberYang 0:85456b971234 53 unsigned int wCrc = 0xffff;
WeberYang 0:85456b971234 54 unsigned int wPolynom = 0xA001;
WeberYang 0:85456b971234 55 /*---------------------------------------------------------------------------------*/
WeberYang 0:85456b971234 56 for (i = 0; i < iBufLen; i++)
WeberYang 0:85456b971234 57 {
WeberYang 0:85456b971234 58 wCrc ^= cBuffer[i];
WeberYang 0:85456b971234 59 for (j = 0; j < 8; j++)
WeberYang 0:85456b971234 60 {
WeberYang 0:85456b971234 61 if (wCrc &0x0001)
WeberYang 0:85456b971234 62 {
WeberYang 0:85456b971234 63 wCrc = (wCrc >> 1) ^ wPolynom;
WeberYang 0:85456b971234 64 }
WeberYang 0:85456b971234 65 else
WeberYang 0:85456b971234 66 {
WeberYang 0:85456b971234 67 wCrc = wCrc >> 1;
WeberYang 0:85456b971234 68 }
WeberYang 0:85456b971234 69 }
WeberYang 0:85456b971234 70 }
WeberYang 0:85456b971234 71 return wCrc;
WeberYang 0:85456b971234 72 }
WeberYang 0:85456b971234 73
WeberYang 0:85456b971234 74 /*
WeberYang 0:85456b971234 75 起始碼(1Byte): AA
WeberYang 0:85456b971234 76 地址碼(1Byte): 01
WeberYang 0:85456b971234 77 返回數據類型(1Byte):從站驅動器不返回數據 00
WeberYang 0:85456b971234 78 故障清除(1Byte)默認發送: 00
WeberYang 0:85456b971234 79 保留位(1Byte) 00
WeberYang 0:85456b971234 80 A 馬達過載控制(1Byte)驅動器使能狀態 01
WeberYang 0:85456b971234 81 B 馬達控制(1Byte) 01
WeberYang 0:85456b971234 82 A 運行方向(1Byte)00:正轉,01反轉 00
WeberYang 0:85456b971234 83 B 運行方向(1Byte)00:正轉,01反轉 00
WeberYang 0:85456b971234 84 運行轉速(2Byte)是100–3000r/min, 12 34
WeberYang 0:85456b971234 85 運行轉速(2Byte)是100–3000r/min, 12 34
WeberYang 0:85456b971234 86 第 1 個 Byte:速度值高 8 位;
WeberYang 0:85456b971234 87 第 2 個 Byte:速度值低 8 位;
WeberYang 0:85456b971234 88 無效(2Byte)00:無效數據位。 00
WeberYang 0:85456b971234 89 結束碼(1Byte)55:通訊結束編碼識別。 55
WeberYang 0:85456b971234 90 校驗碼(2Byte)CRC16
WeberYang 0:85456b971234 91 */
WeberYang 0:85456b971234 92
WeberYang 0:85456b971234 93 // void spinOnce();
WeberYang 0:85456b971234 94 // void twistCallback(const geometry_msgs::Twist &twist_aux);
WeberYang 0:85456b971234 95
WeberYang 0:85456b971234 96
WeberYang 0:85456b971234 97 void init_variables()
WeberYang 0:85456b971234 98 {
WeberYang 0:85456b971234 99 dx = dy = dr =0;
WeberYang 0:85456b971234 100 w = 0.302;//0.2 ;//m
WeberYang 0:85456b971234 101 rate = 20;//50;
WeberYang 0:85456b971234 102 timeout_ticks = 2;
WeberYang 0:85456b971234 103 Dimeter = 0.127;//0.15;
WeberYang 0:85456b971234 104 }
WeberYang 0:85456b971234 105 void Sendmessage(float Rrpm,float Lrpm)
WeberYang 0:85456b971234 106 {
WeberYang 0:85456b971234 107 //RS232.printf("Wr = %.1f\n",Rrpm);
WeberYang 0:85456b971234 108 //RS232.printf("Wl = %.1f\n",Lrpm);
WeberYang 0:85456b971234 109 unsigned char sendData[16];
WeberYang 0:85456b971234 110 unsigned int tmpCRC;
WeberYang 0:85456b971234 111 int motor1,motor2;
WeberYang 0:85456b971234 112
WeberYang 0:85456b971234 113 sendData[0] = Start;
WeberYang 0:85456b971234 114 sendData[1] = Address;
WeberYang 0:85456b971234 115 sendData[2] = ReturnType;
WeberYang 0:85456b971234 116 sendData[3] = Clean;
WeberYang 0:85456b971234 117 sendData[4] = Reserve;
WeberYang 0:85456b971234 118 sendData[5] = 0x01;//motor1Sevro ON
WeberYang 0:85456b971234 119 sendData[6] = 0x01;//motor2Sevro ON
WeberYang 0:85456b971234 120 if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
WeberYang 0:85456b971234 121 if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
WeberYang 0:85456b971234 122 motor1 = abs(Rrpm);
WeberYang 0:85456b971234 123 motor2 = abs(Lrpm);
WeberYang 0:85456b971234 124
WeberYang 0:85456b971234 125 sendData[9] = (motor1>>8);//motor1speedH
WeberYang 0:85456b971234 126 sendData[10] = (motor1 & 0xFF);//motor1speedL
WeberYang 0:85456b971234 127 sendData[11] = (motor2>>8);//motor2speedH
WeberYang 0:85456b971234 128 sendData[12] = (motor2 & 0xFF);//motor2speedL
WeberYang 0:85456b971234 129 sendData[13] = End;
WeberYang 0:85456b971234 130 tmpCRC = CRC_Verify(sendData, 14);
WeberYang 0:85456b971234 131 sendData[14] = (tmpCRC & 0xFF);
WeberYang 0:85456b971234 132 sendData[15] = (tmpCRC>>8);
WeberYang 0:85456b971234 133 int i;
WeberYang 0:85456b971234 134 for (i=0;i<16;i++)
WeberYang 0:85456b971234 135 {
WeberYang 0:85456b971234 136 RS232.printf("%c",sendData[i]);
WeberYang 0:85456b971234 137 }
WeberYang 0:85456b971234 138 RS232.printf("\r\n");
WeberYang 0:85456b971234 139 }
WeberYang 0:85456b971234 140
WeberYang 0:85456b971234 141
WeberYang 0:85456b971234 142 int myabs( int a ){
WeberYang 0:85456b971234 143 if ( a < 0 ){
WeberYang 0:85456b971234 144 return -a;
WeberYang 0:85456b971234 145 }
WeberYang 0:85456b971234 146 return a;
WeberYang 0:85456b971234 147 }
WeberYang 0:85456b971234 148
WeberYang 0:85456b971234 149
WeberYang 0:85456b971234 150
WeberYang 0:85456b971234 151 void TwistToMotors()
WeberYang 0:85456b971234 152 {
WeberYang 0:85456b971234 153 old_seq = seq;
WeberYang 0:85456b971234 154 seq = seq + 1;
WeberYang 0:85456b971234 155 //int lower_bound = 100;
WeberYang 0:85456b971234 156 //int upper_bound = 300;
WeberYang 0:85456b971234 157 float right,left;
WeberYang 0:85456b971234 158 // float motor_rpm_r, motor_rpm_l;
WeberYang 0:85456b971234 159 //double vel_data[2];
WeberYang 0:85456b971234 160 float vel_data[2];
WeberYang 0:85456b971234 161 float motor_rpm_vx, motor_rpm_theta, motor_rpm_v;
WeberYang 0:85456b971234 162
WeberYang 0:85456b971234 163 // prevent agv receive weird 1.0 command from cmd_vel
WeberYang 0:85456b971234 164
WeberYang 0:85456b971234 165 right = ( 1.0 * dx ) + (dr * w /2);
WeberYang 0:85456b971234 166 left = ( 1.0 * dx ) - (dr * w /2);
WeberYang 0:85456b971234 167 motor_rpm_v = ( 1.0 * dx )*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 0:85456b971234 168
WeberYang 0:85456b971234 169 if((motor_rpm_v!=0) && (myabs(motor_rpm_v)<100)){
WeberYang 0:85456b971234 170 if (motor_rpm_v > 0) motor_rpm_v=100;
WeberYang 0:85456b971234 171 else motor_rpm_v = -100;
WeberYang 0:85456b971234 172 }
WeberYang 0:85456b971234 173
WeberYang 0:85456b971234 174 motor_rpm_theta=(dr * w /2)*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 0:85456b971234 175
WeberYang 0:85456b971234 176
WeberYang 0:85456b971234 177 //motor_rpm_r = right*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 0:85456b971234 178 //motor_rpm_l = left*rate/(Dimeter/2)*60/(2*3.1416);
WeberYang 0:85456b971234 179
WeberYang 0:85456b971234 180 motor_rpm_r = motor_rpm_v + motor_rpm_theta;
WeberYang 0:85456b971234 181 motor_rpm_l = motor_rpm_v - motor_rpm_theta;
WeberYang 0:85456b971234 182
WeberYang 0:85456b971234 183
WeberYang 0:85456b971234 184 if (myabs(motor_rpm_r)<100 || myabs(motor_rpm_l)<100){
WeberYang 0:85456b971234 185
WeberYang 0:85456b971234 186 if(dx == 0){
WeberYang 0:85456b971234 187 if(dr > 0){
WeberYang 0:85456b971234 188 motor_rpm_r = 100;
WeberYang 0:85456b971234 189 motor_rpm_l = -100;
WeberYang 0:85456b971234 190 }else if( dr < 0 ){
WeberYang 0:85456b971234 191 motor_rpm_r = -100;
WeberYang 0:85456b971234 192 motor_rpm_l = 100;
WeberYang 0:85456b971234 193 }else{
WeberYang 0:85456b971234 194 motor_rpm_r = 0;
WeberYang 0:85456b971234 195 motor_rpm_l = 0;
WeberYang 0:85456b971234 196 }
WeberYang 0:85456b971234 197 }
WeberYang 0:85456b971234 198 else if( dx > 0 ){
WeberYang 0:85456b971234 199 if( myabs(motor_rpm_r) < 100 ){
WeberYang 0:85456b971234 200 motor_rpm_r = 100;
WeberYang 0:85456b971234 201 motor_rpm_l=motor_rpm_l+(100-motor_rpm_r);
WeberYang 0:85456b971234 202 }
WeberYang 0:85456b971234 203 if ( myabs(motor_rpm_l) < 100 ){
WeberYang 0:85456b971234 204 motor_rpm_l = 100;
WeberYang 0:85456b971234 205 motor_rpm_r=motor_rpm_r+(100-motor_rpm_l);
WeberYang 0:85456b971234 206 }
WeberYang 0:85456b971234 207 }else{
WeberYang 0:85456b971234 208 if ( myabs(motor_rpm_r) < 100 ){
WeberYang 0:85456b971234 209 motor_rpm_r = -100;
WeberYang 0:85456b971234 210 motor_rpm_l=motor_rpm_l+(-100-motor_rpm_r);
WeberYang 0:85456b971234 211 }
WeberYang 0:85456b971234 212
WeberYang 0:85456b971234 213 if (myabs(motor_rpm_l) < 100 ){
WeberYang 0:85456b971234 214 motor_rpm_l = -100;
WeberYang 0:85456b971234 215 motor_rpm_r=motor_rpm_r+(-100-motor_rpm_l);
WeberYang 0:85456b971234 216 }
WeberYang 0:85456b971234 217 }
WeberYang 0:85456b971234 218 }
WeberYang 0:85456b971234 219
WeberYang 0:85456b971234 220 /*
WeberYang 0:85456b971234 221 if ( myabs(motor_rpm_r) == myabs(motor_rpm_l) && myabs(motor_rpm_r) < 100){
WeberYang 0:85456b971234 222 int s = myabs(motor_rpm_r);
WeberYang 0:85456b971234 223
WeberYang 0:85456b971234 224 motor_rpm_r = motor_rpm_r / s * 101;
WeberYang 0:85456b971234 225 motor_rpm_l = motor_rpm_l / s * 101;
WeberYang 0:85456b971234 226
WeberYang 0:85456b971234 227 }
WeberYang 0:85456b971234 228 */
WeberYang 0:85456b971234 229
WeberYang 0:85456b971234 230 /*
WeberYang 0:85456b971234 231 if ( myabs(vel_data[0]) < lower_bound )
WeberYang 0:85456b971234 232 { // if right wheel rpm < lower_bound
WeberYang 0:85456b971234 233 int a = 0;
WeberYang 0:85456b971234 234 a = lower_bound - myabs( vel_data[0] );
WeberYang 0:85456b971234 235
WeberYang 0:85456b971234 236 if (vel_data[0] > 0)
WeberYang 0:85456b971234 237 {
WeberYang 0:85456b971234 238 vel_data[0] = lower_bound;
WeberYang 0:85456b971234 239 }
WeberYang 0:85456b971234 240 else
WeberYang 0:85456b971234 241 {
WeberYang 0:85456b971234 242 vel_data[0] = -lower_bound;
WeberYang 0:85456b971234 243 }
WeberYang 0:85456b971234 244
WeberYang 0:85456b971234 245
WeberYang 0:85456b971234 246 if (vel_data[1] > 0)
WeberYang 0:85456b971234 247 {
WeberYang 0:85456b971234 248 vel_data[1] = vel_data[1] + a;
WeberYang 0:85456b971234 249 }
WeberYang 0:85456b971234 250 else
WeberYang 0:85456b971234 251 {
WeberYang 0:85456b971234 252 vel_data[1] = vel_data[1] - a;
WeberYang 0:85456b971234 253 } //
WeberYang 0:85456b971234 254
WeberYang 0:85456b971234 255 }
WeberYang 0:85456b971234 256
WeberYang 0:85456b971234 257 else if ( myabs(vel_data[0]) > upper_bound )
WeberYang 0:85456b971234 258 { // if right wheel rpm > upper_bound
WeberYang 0:85456b971234 259 int a = 0;
WeberYang 0:85456b971234 260 a = myabs( vel_data[0] ) - upper_bound;
WeberYang 0:85456b971234 261
WeberYang 0:85456b971234 262 if (vel_data[0] > 0)
WeberYang 0:85456b971234 263 {
WeberYang 0:85456b971234 264 vel_data[0] = upper_bound;
WeberYang 0:85456b971234 265 }
WeberYang 0:85456b971234 266 else
WeberYang 0:85456b971234 267 {
WeberYang 0:85456b971234 268 vel_data[0] = -upper_bound;
WeberYang 0:85456b971234 269 }
WeberYang 0:85456b971234 270
WeberYang 0:85456b971234 271
WeberYang 0:85456b971234 272 if (vel_data[1] > 0)
WeberYang 0:85456b971234 273 {
WeberYang 0:85456b971234 274 vel_data[1] = vel_data[1] - a;
WeberYang 0:85456b971234 275 }
WeberYang 0:85456b971234 276 else
WeberYang 0:85456b971234 277 {
WeberYang 0:85456b971234 278 vel_data[1] = vel_data[1] + a;
WeberYang 0:85456b971234 279 }
WeberYang 0:85456b971234 280 }
WeberYang 0:85456b971234 281
WeberYang 0:85456b971234 282
WeberYang 0:85456b971234 283 if ( myabs(vel_data[1]) < lower_bound )
WeberYang 0:85456b971234 284 { // if left wheel rpm < lower_bound
WeberYang 0:85456b971234 285 int a = 0;
WeberYang 0:85456b971234 286 a = lower_bound - myabs( vel_data[1] );
WeberYang 0:85456b971234 287
WeberYang 0:85456b971234 288 if (vel_data[1] > 0)
WeberYang 0:85456b971234 289 {
WeberYang 0:85456b971234 290 vel_data[1] = lower_bound;
WeberYang 0:85456b971234 291 }
WeberYang 0:85456b971234 292 else
WeberYang 0:85456b971234 293 {
WeberYang 0:85456b971234 294 vel_data[1] = -lower_bound;
WeberYang 0:85456b971234 295 }
WeberYang 0:85456b971234 296
WeberYang 0:85456b971234 297
WeberYang 0:85456b971234 298 if (vel_data[0] > 0)
WeberYang 0:85456b971234 299 {
WeberYang 0:85456b971234 300 vel_data[0] = vel_data[0] + a;
WeberYang 0:85456b971234 301 }
WeberYang 0:85456b971234 302 else
WeberYang 0:85456b971234 303 {
WeberYang 0:85456b971234 304 vel_data[0] = vel_data[0] - a;
WeberYang 0:85456b971234 305 } //
WeberYang 0:85456b971234 306 }
WeberYang 0:85456b971234 307
WeberYang 0:85456b971234 308
WeberYang 0:85456b971234 309 else if (myabs(vel_data[1]) > upper_bound )
WeberYang 0:85456b971234 310 { // if left wheel rpm > upper_bound
WeberYang 0:85456b971234 311 int a = 0;
WeberYang 0:85456b971234 312 a = myabs( vel_data[1] ) - upper_bound;
WeberYang 0:85456b971234 313
WeberYang 0:85456b971234 314 if (vel_data[1] > 0)
WeberYang 0:85456b971234 315 {
WeberYang 0:85456b971234 316 vel_data[1] = upper_bound;
WeberYang 0:85456b971234 317 }
WeberYang 0:85456b971234 318 else
WeberYang 0:85456b971234 319 {
WeberYang 0:85456b971234 320 vel_data[1] = -upper_bound;
WeberYang 0:85456b971234 321 }
WeberYang 0:85456b971234 322
WeberYang 0:85456b971234 323
WeberYang 0:85456b971234 324 if (vel_data[0] > 0)
WeberYang 0:85456b971234 325 {
WeberYang 0:85456b971234 326 vel_data[0] = vel_data[0] - a;
WeberYang 0:85456b971234 327 }
WeberYang 0:85456b971234 328 else
WeberYang 0:85456b971234 329 {
WeberYang 0:85456b971234 330 vel_data[0] = vel_data[0] + a;
WeberYang 0:85456b971234 331 }
WeberYang 0:85456b971234 332
WeberYang 0:85456b971234 333 }
WeberYang 0:85456b971234 334
WeberYang 0:85456b971234 335 */
WeberYang 0:85456b971234 336
WeberYang 0:85456b971234 337 /*
WeberYang 0:85456b971234 338 if ( myabs(motor_rpm_r) < 100 || myabs(motor_rpm_l) < 100 ){
WeberYang 0:85456b971234 339 float a = 0.0;
WeberYang 0:85456b971234 340
WeberYang 0:85456b971234 341 if ( myabs(motor_rpm_r) < myabs(motor_rpm_l) ){
WeberYang 0:85456b971234 342
WeberYang 0:85456b971234 343 if ( motor_rpm_r < 0 ){
WeberYang 0:85456b971234 344 a = -100 - motor_rpm_r;
WeberYang 0:85456b971234 345 }else if( motor_rpm_r > 0) {
WeberYang 0:85456b971234 346 a = 100 - motor_rpm_r;
WeberYang 0:85456b971234 347 }else {
WeberYang 0:85456b971234 348 a = 0;
WeberYang 0:85456b971234 349 }
WeberYang 0:85456b971234 350
WeberYang 0:85456b971234 351 motor_rpm_r = motor_rpm_r + a;
WeberYang 0:85456b971234 352 motor_rpm_l = motor_rpm_l + a;
WeberYang 0:85456b971234 353
WeberYang 0:85456b971234 354
WeberYang 0:85456b971234 355
WeberYang 0:85456b971234 356 }else if ( myabs(motor_rpm_r) > myabs(motor_rpm_l) ){
WeberYang 0:85456b971234 357
WeberYang 0:85456b971234 358 if ( motor_rpm_l < 0 ){
WeberYang 0:85456b971234 359 a = -100 - motor_rpm_l;
WeberYang 0:85456b971234 360 }else if( motor_rpm_l > 0) {
WeberYang 0:85456b971234 361 a = 100 - motor_rpm_l;
WeberYang 0:85456b971234 362 }else {
WeberYang 0:85456b971234 363 a = 0;
WeberYang 0:85456b971234 364 }
WeberYang 0:85456b971234 365
WeberYang 0:85456b971234 366 motor_rpm_l = motor_rpm_l + a;
WeberYang 0:85456b971234 367 motor_rpm_r = motor_rpm_r + a;
WeberYang 0:85456b971234 368
WeberYang 0:85456b971234 369 }
WeberYang 0:85456b971234 370
WeberYang 0:85456b971234 371 // go straight < 100
WeberYang 0:85456b971234 372 if ( motor_rpm_r == motor_rpm_l ){
WeberYang 0:85456b971234 373
WeberYang 0:85456b971234 374 if ( motor_rpm_l < 0 ){
WeberYang 0:85456b971234 375 motor_rpm_l = -100;
WeberYang 0:85456b971234 376 motor_rpm_r = -100;
WeberYang 0:85456b971234 377
WeberYang 0:85456b971234 378
WeberYang 0:85456b971234 379 }else if( motor_rpm_l > 0) {
WeberYang 0:85456b971234 380 motor_rpm_l = 100;
WeberYang 0:85456b971234 381 motor_rpm_r = 100;
WeberYang 0:85456b971234 382 }else {
WeberYang 0:85456b971234 383 motor_rpm_l = 0;
WeberYang 0:85456b971234 384 motor_rpm_r = 0;
WeberYang 0:85456b971234 385 }
WeberYang 0:85456b971234 386
WeberYang 0:85456b971234 387
WeberYang 0:85456b971234 388 }
WeberYang 0:85456b971234 389
WeberYang 0:85456b971234 390 // turn < 100
WeberYang 0:85456b971234 391
WeberYang 0:85456b971234 392 if ( motor_rpm_r == -motor_rpm_l ){
WeberYang 0:85456b971234 393
WeberYang 0:85456b971234 394 if ( motor_rpm_l < 0 ){
WeberYang 0:85456b971234 395 motor_rpm_l = -100;
WeberYang 0:85456b971234 396 motor_rpm_r = 100;
WeberYang 0:85456b971234 397 }else if( motor_rpm_l > 0) {
WeberYang 0:85456b971234 398 motor_rpm_l = 100;
WeberYang 0:85456b971234 399 motor_rpm_r = -100;
WeberYang 0:85456b971234 400 }else {
WeberYang 0:85456b971234 401 motor_rpm_l = 0;
WeberYang 0:85456b971234 402 motor_rpm_r = 0;
WeberYang 0:85456b971234 403 }
WeberYang 0:85456b971234 404
WeberYang 0:85456b971234 405 }
WeberYang 0:85456b971234 406 //
WeberYang 0:85456b971234 407
WeberYang 0:85456b971234 408 }
WeberYang 0:85456b971234 409
WeberYang 0:85456b971234 410
WeberYang 0:85456b971234 411
WeberYang 0:85456b971234 412 if ( myabs(motor_rpm_r) > 600 ){
WeberYang 0:85456b971234 413
WeberYang 0:85456b971234 414 if ( motor_rpm_r < 0 ){
WeberYang 0:85456b971234 415 motor_rpm_r = -100 * log( float(myabs(motor_rpm_r)) );
WeberYang 0:85456b971234 416 }else{
WeberYang 0:85456b971234 417 motor_rpm_r = 100 * log( float(myabs(motor_rpm_r)) );
WeberYang 0:85456b971234 418 }
WeberYang 0:85456b971234 419
WeberYang 0:85456b971234 420
WeberYang 0:85456b971234 421
WeberYang 0:85456b971234 422 }
WeberYang 0:85456b971234 423
WeberYang 0:85456b971234 424 if ( myabs(motor_rpm_l) > 600) {
WeberYang 0:85456b971234 425
WeberYang 0:85456b971234 426 if ( motor_rpm_l < 0 ){
WeberYang 0:85456b971234 427 motor_rpm_l = -100 * log( float(myabs(motor_rpm_l)) );
WeberYang 0:85456b971234 428 }else{
WeberYang 0:85456b971234 429 motor_rpm_l = 100 * log( float(myabs(motor_rpm_l)) );
WeberYang 0:85456b971234 430 }
WeberYang 0:85456b971234 431 }
WeberYang 0:85456b971234 432
WeberYang 0:85456b971234 433 */
WeberYang 0:85456b971234 434 vel_data[0] = motor_rpm_r;
WeberYang 0:85456b971234 435 vel_data[1] = motor_rpm_l;
WeberYang 0:85456b971234 436
WeberYang 0:85456b971234 437
WeberYang 0:85456b971234 438
WeberYang 0:85456b971234 439
WeberYang 0:85456b971234 440
WeberYang 0:85456b971234 441 //===================================================================
WeberYang 0:85456b971234 442 //Sendmessage(vel_data[0],vel_data[1]);
WeberYang 0:85456b971234 443
WeberYang 0:85456b971234 444
WeberYang 0:85456b971234 445 VelAngular_R.data = vel_data[0];
WeberYang 0:85456b971234 446 VelAngular_L.data = vel_data[1];
WeberYang 0:85456b971234 447 //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
WeberYang 0:85456b971234 448 //}
WeberYang 0:85456b971234 449 //else{
WeberYang 0:85456b971234 450 pub_rmotor.publish( &VelAngular_R );
WeberYang 0:85456b971234 451 pub_lmotor.publish( &VelAngular_L );
WeberYang 0:85456b971234 452 //}
WeberYang 0:85456b971234 453 //RS232.printf("Wr = %.1f\n",vel_data[0]);
WeberYang 0:85456b971234 454 //RS232.printf("Wl = %.1f\n",vel_data[1]);
WeberYang 0:85456b971234 455 ticks_since_target += 1;
WeberYang 0:85456b971234 456
WeberYang 0:85456b971234 457 }
WeberYang 0:85456b971234 458
WeberYang 0:85456b971234 459 void messageCb(const geometry_msgs::Twist &msg)
WeberYang 0:85456b971234 460 {
WeberYang 0:85456b971234 461 ticks_since_target = 0;
WeberYang 0:85456b971234 462 dx = msg.linear.x;
WeberYang 0:85456b971234 463 dy = msg.linear.y;
WeberYang 0:85456b971234 464 dr = msg.angular.z;
WeberYang 0:85456b971234 465 //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr);
WeberYang 0:85456b971234 466 TwistToMotors();
WeberYang 0:85456b971234 467 }
WeberYang 0:85456b971234 468
WeberYang 0:85456b971234 469 void ACT( const std_msgs::String& ACT) {
WeberYang 0:85456b971234 470 char sendData[16];
WeberYang 0:85456b971234 471 //sendData = ACT;
WeberYang 0:85456b971234 472 int motor1,motor2;
WeberYang 0:85456b971234 473
WeberYang 0:85456b971234 474 sendData[0] = Start;
WeberYang 0:85456b971234 475 sendData[1] = Address;
WeberYang 0:85456b971234 476 sendData[2] = ReturnType;
WeberYang 0:85456b971234 477 sendData[3] = Clean;
WeberYang 0:85456b971234 478 sendData[4] = Reserve;
WeberYang 0:85456b971234 479 sendData[5] = 0x00;//motor1Sevro OFF
WeberYang 0:85456b971234 480 sendData[6] = 0x00;//motor2Sevro OFF
WeberYang 0:85456b971234 481 motor1 = 0;
WeberYang 0:85456b971234 482 motor2 = 0;
WeberYang 0:85456b971234 483 sendData[9] = (motor1>>8);//motor1speedH
WeberYang 0:85456b971234 484 sendData[10] = (motor1 & 0xFF);//motor1speedL
WeberYang 0:85456b971234 485 sendData[11] = (motor2>>8);//motor2speedH
WeberYang 0:85456b971234 486 sendData[12] = (motor2 & 0xFF);//motor2speedL
WeberYang 0:85456b971234 487 sendData[13] = End;
WeberYang 0:85456b971234 488
WeberYang 0:85456b971234 489
WeberYang 0:85456b971234 490 sendData[14] = Start;
WeberYang 0:85456b971234 491 sendData[15] = Start;
WeberYang 0:85456b971234 492 int i;
WeberYang 0:85456b971234 493 for (i=0;i<16;i++)
WeberYang 0:85456b971234 494 {
WeberYang 0:85456b971234 495 RS232.printf("%c",sendData[i]);
WeberYang 0:85456b971234 496 }
WeberYang 0:85456b971234 497 RS232.printf("\r\n");
WeberYang 0:85456b971234 498 }
WeberYang 0:85456b971234 499
WeberYang 0:85456b971234 500
WeberYang 0:85456b971234 501 ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
WeberYang 0:85456b971234 502 //ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT );
WeberYang 0:85456b971234 503
WeberYang 0:85456b971234 504
WeberYang 0:85456b971234 505 Timer t;
WeberYang 0:85456b971234 506 int main(int argc, char **argv)
WeberYang 0:85456b971234 507 {
WeberYang 0:85456b971234 508 long vel_timer = 0;
WeberYang 0:85456b971234 509 init_variables();
WeberYang 0:85456b971234 510 t.start();
WeberYang 0:85456b971234 511 nh.initNode();
WeberYang 0:85456b971234 512 nh.advertise(pub_lmotor);
WeberYang 0:85456b971234 513 nh.advertise(pub_rmotor);
WeberYang 0:85456b971234 514 nh.subscribe(cmd_vel_sub);
WeberYang 0:85456b971234 515 //nh.subscribe(sub_action);
WeberYang 0:85456b971234 516
WeberYang 0:85456b971234 517 while (true)
WeberYang 0:85456b971234 518 {
WeberYang 0:85456b971234 519 //if (t.read_ms() > vel_timer)
WeberYang 0:85456b971234 520 //{
WeberYang 0:85456b971234 521 // vel_timer = t.read_ms() + 50;
WeberYang 0:85456b971234 522 // TwistToMotors();
WeberYang 0:85456b971234 523 // }
WeberYang 0:85456b971234 524
WeberYang 0:85456b971234 525
WeberYang 0:85456b971234 526 nh.spinOnce();
WeberYang 0:85456b971234 527
WeberYang 0:85456b971234 528
WeberYang 0:85456b971234 529 if (seq +50 < old_seq){
WeberYang 0:85456b971234 530 motor_rpm_r = 0;
WeberYang 0:85456b971234 531 motor_rpm_l = 0;
WeberYang 0:85456b971234 532 }
WeberYang 0:85456b971234 533
WeberYang 2:fcf8a7739eed 534 // Sendmessage(-motor_rpm_r,-motor_rpm_l);
WeberYang 2:fcf8a7739eed 535 Sendmessage(-motor_rpm_l,-motor_rpm_r);
WeberYang 0:85456b971234 536
WeberYang 0:85456b971234 537 old_seq = old_seq + 1;
WeberYang 0:85456b971234 538
WeberYang 0:85456b971234 539 wait_ms(10);
WeberYang 0:85456b971234 540
WeberYang 0:85456b971234 541 }
WeberYang 0:85456b971234 542 }