Weber Yang / Mbed 2 deprecated AGV_0213

using NucleoF103

Dependencies:   mbed ros_lib_kinetic

main.cpp@0:576de68cbb45, 2018-04-12 (annotated)

Committer:
WeberYang
Date:
Thu Apr 12 00:53:17 2018 +0000
Revision:
0:576de68cbb45
SMT_weber

Who changed what in which revision?

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