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