Diff: main.cpp

Revision:

0:4ff8854a49e8

diff -r 000000000000 -r 4ff8854a49e8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 12 00:57:02 2018 +0000
@@ -0,0 +1,281 @@
+#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.302;//0.2 ;//m
+    rate = 20;//50;
+    timeout_ticks = 2;
+    Dimeter = 0.127;//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;
+    float motor_rpm_r, motor_rpm_l;
+    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;
+    */
+   if (dx!=0)
+    {
+     if (dx>0)
+     { 
+       if (dr >=0)
+       {
+           motor_rpm_r=300+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+           motor_rpm_l=300;
+       }
+       else
+       {
+           motor_rpm_r=300;
+           motor_rpm_l=300-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+       }
+     }
+     else
+     {
+         if(dr>=0)
+         {
+          motor_rpm_r=(-300)-(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+          motor_rpm_l=(-300);
+         }
+         else
+         {
+             motor_rpm_r=(-300);
+             motor_rpm_l=(-300)+(dr*w/2)*rate/(Dimeter/2)/(2*3.1416)*60;
+         }
+     }
+    }
+    else
+    {
+        if(dr>=0)
+        {
+            motor_rpm_r=100;
+            motor_rpm_l=-100;
+        }
+        else
+        {
+            motor_rpm_r=-100;
+            motor_rpm_l=100;
+        }
+    }
+    vel_data[0]=motor_rpm_r;
+    vel_data[1]=motor_rpm_l; 
+    //================================================================ Original Version
+    /*if (dr>=0)
+    {
+        right = ( 1.0 * dx ) + (dr * w /2);
+        left = ( 1.0 * dx );
+    }
+    else
+    {
+        right = ( 1.0 * dx );
+        left = ( 1.0 * dx ) - (dr * w /2);
+    }
+        vel_data[0] = right*rate/(Dimeter/2)/(2*3.1416)*60;
+        vel_data[1] =  left*rate/(Dimeter/2)/(2*3.1416)*60;*/
+//===================================================================
+    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