Weber Yang
test
Dependencies: mbed ros_lib_kinetic
Revision 0:6a62ef9495ec, committed 2018-04-12
- Comitter:
- WeberYang
- Date:
- Thu Apr 12 00:55:59 2018 +0000
- Commit message:
- betaTesting
Changed in this revision
diff -r 000000000000 -r 6a62ef9495ec main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 12 00:55:59 2018 +0000
@@ -0,0 +1,220 @@
+#include "mbed.h"
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/String.h>
+#include<geometry_msgs/Twist.h> //set buffer larger than 50byte
+#include <std_msgs/Float32.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <iostream>
+#define Start 0xAA
+#define Address 0x7F
+#define ReturnType 0x00
+#define Clean 0x00
+#define Reserve 0x00
+#define End 0x55
+
+ Serial RS232 (PB_10, PB_11,115200); // This one works
+ std_msgs::String str_msg;
+ std_msgs::Float32 VelAngular_L;
+ std_msgs::Float32 VelAngular_R;
+ ros::Publisher pub_lmotor("pub_lmotor", &VelAngular_L);
+ ros::Publisher pub_rmotor("pub_rmotor", &VelAngular_R);
+
+ ros::NodeHandle nh;
+ geometry_msgs::Twist vel;
+ //float left;
+ //float right;
+ float Lrpm,Rrpm;
+ float ticks_since_target;
+ double timeout_ticks;
+
+ double w;
+ double rate;
+ double Dimeter;
+ float dx,dy,dr;
+ void init_variables();
+void Sendmessage(float Rrpm,float Lrpm);
+
+unsigned int CRC_Verify(unsigned char *cBuffer, unsigned int iBufLen)
+{
+ unsigned int i, j;
+ //#define wPolynom 0xA001
+ unsigned int wCrc = 0xffff;
+ unsigned int wPolynom = 0xA001;
+ /*---------------------------------------------------------------------------------*/
+ for (i = 0; i < iBufLen; i++)
+ {
+ wCrc ^= cBuffer[i];
+ for (j = 0; j < 8; j++)
+ {
+ if (wCrc &0x0001)
+ {
+ wCrc = (wCrc >> 1) ^ wPolynom;
+ }
+ else
+ {
+ wCrc = wCrc >> 1;
+ }
+ }
+ }
+ return wCrc;
+}
+
+ /*
+ 起始碼(1Byte): AA
+地址碼(1Byte): 01
+返回數據類型(1Byte):從站驅動器不返回數據 00
+故障清除(1Byte)默認發送: 00
+保留位(1Byte) 00
+A 馬達過載控制(1Byte)驅動器使能狀態 01
+B 馬達控制(1Byte) 01
+A 運行方向(1Byte)00:正轉,01反轉 00
+B 運行方向(1Byte)00:正轉,01反轉 00
+運行轉速(2Byte)是100–3000r/min, 12 34
+運行轉速(2Byte)是100–3000r/min, 12 34
+第 1 個 Byte:速度值高 8 位;
+第 2 個 Byte:速度值低 8 位;
+無效(2Byte)00:無效數據位。 00
+結束碼(1Byte)55:通訊結束編碼識別。 55
+校驗碼(2Byte)CRC16
+ */
+
+ // void spinOnce();
+ // void twistCallback(const geometry_msgs::Twist &twist_aux);
+
+
+void init_variables()
+{
+ dx = dy = dr =0;
+ w = 0.2 ;
+ rate = 50;
+ timeout_ticks = 2;
+ Dimeter = 0.15;
+}
+void Sendmessage(float Rrpm,float Lrpm)
+{
+ //RS232.printf("Wr = %.1f\n",Rrpm);
+ //RS232.printf("Wl = %.1f\n",Lrpm);
+unsigned char sendData[16];
+unsigned int tmpCRC;
+int motor1,motor2;
+
+ sendData[0] = Start;
+ sendData[1] = Address;
+ sendData[2] = ReturnType;
+ sendData[3] = Clean;
+ sendData[4] = Reserve;
+ sendData[5] = 0x01;//motor1Sevro ON
+ sendData[6] = 0x01;//motor2Sevro ON
+if (Rrpm>0){sendData[7] = 0x01;}else{sendData[7] = 0x00;}
+if (Lrpm>0){sendData[8] = 0x00;}else{sendData[8] = 0x01;}
+ motor1 = abs(Rrpm);
+ motor2 = abs(Lrpm);
+
+ sendData[9] = (motor1>>8);//motor1speedH
+ sendData[10] = (motor1 & 0xFF);//motor1speedL
+ sendData[11] = (motor2>>8);//motor2speedH
+ sendData[12] = (motor2 & 0xFF);//motor2speedL
+ sendData[13] = End;
+ tmpCRC = CRC_Verify(sendData, 14);
+ sendData[14] = (tmpCRC & 0xFF);
+ sendData[15] = (tmpCRC>>8);
+ int i;
+ for (i=0;i<16;i++)
+ {
+ RS232.printf("%c",sendData[i]);
+ }
+ RS232.printf("\r\n");
+}
+void TwistToMotors()
+{
+ float right,left;
+ double vel_data[2];
+ right = ( 1.0 * dx ) + (dr * w /2);
+ left = ( 1.0 * dx ) - (dr * w /2);
+ vel_data[0] = right*rate/Dimeter/60*1000;
+ vel_data[1] = left*rate/Dimeter/60*1000;
+ Sendmessage(vel_data[0],vel_data[1]);
+ VelAngular_R.data = vel_data[0];
+ VelAngular_L.data = vel_data[1];
+ //if(VelAngular_R.data >2000 || VelAngular_L.data>2000){
+ //}
+ //else{
+ pub_rmotor.publish( &VelAngular_R );
+ pub_lmotor.publish( &VelAngular_L );
+ //}
+ //RS232.printf("Wr = %.1f\n",vel_data[0]);
+ //RS232.printf("Wl = %.1f\n",vel_data[1]);
+ ticks_since_target += 1;
+
+}
+
+void messageCb(const geometry_msgs::Twist &msg)
+{
+ ticks_since_target = 0;
+ dx = msg.linear.x;
+ dy = msg.linear.y;
+ dr = msg.angular.z;
+ //RS232.printf("dx,dy,dr= %.1f,%.1f,%.1f\n",dx,dy,dr);
+ TwistToMotors();
+}
+
+void ACT( const std_msgs::String& ACT) {
+char sendData[16];
+//sendData = ACT;
+int motor1,motor2;
+
+ sendData[0] = Start;
+ sendData[1] = Address;
+ sendData[2] = ReturnType;
+ sendData[3] = Clean;
+ sendData[4] = Reserve;
+ sendData[5] = 0x00;//motor1Sevro OFF
+ sendData[6] = 0x00;//motor2Sevro OFF
+ motor1 = 0;
+ motor2 = 0;
+ sendData[9] = (motor1>>8);//motor1speedH
+ sendData[10] = (motor1 & 0xFF);//motor1speedL
+ sendData[11] = (motor2>>8);//motor2speedH
+ sendData[12] = (motor2 & 0xFF);//motor2speedL
+ sendData[13] = End;
+
+
+ sendData[14] = Start;
+ sendData[15] = Start;
+ int i;
+ for (i=0;i<16;i++)
+ {
+ RS232.printf("%c",sendData[i]);
+ }
+ RS232.printf("\r\n");
+}
+ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("cmd_vel", &messageCb);
+ros::Subscriber<std_msgs::String> sub_action("ACT", &ACT );
+
+ Timer t;
+int main(int argc, char **argv)
+{
+ long vel_timer = 0;
+ init_variables();
+ t.start();
+ nh.initNode();
+ nh.advertise(pub_lmotor);
+ nh.advertise(pub_rmotor);
+ nh.subscribe(cmd_vel_sub);
+ nh.subscribe(sub_action);
+
+ while (1)
+ {
+ if (t.read_ms() > vel_timer)
+ {
+ vel_timer = t.read_ms() + 500;
+ }
+ nh.spinOnce();
+
+ wait_ms(10);
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r 6a62ef9495ec mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 12 00:55:59 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/7130f322cb7e \ No newline at end of file
diff -r 000000000000 -r 6a62ef9495ec ros_lib_kinetic.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Thu Apr 12 00:55:59 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f