![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |