Weber Yang / Mbed 2 deprecated AGV_0227_rag

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?

UserRevisionLine numberNew 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 }