Diff: main.cpp

Revision:

0:27046fed2426

Child:

1:417488dfc3d0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 13 09:31:05 2019 +0000
@@ -0,0 +1,227 @@
+/*
+ * rosserial Motor Shield Example
+ *
+ * This tutorial demonstrates the usage of the
+ * Seeedstudio Motor Shield
+ * http://www.seeedstudio.com/depot/motor-shield-v20-p-1377.html
+ *
+ * Source Code Based on:
+ * https://developer.mbed.org/teams/shields/code/Seeed_Motor_Shield/
+ */
+
+#include "mbed.h"
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include "QEI.h"
+#include <std_msgs/String.h>
+
+// マクロ定義(ピン配置)
+#define D0  PA_3
+#define D1  PA_2
+#define D2  PA_10
+#define D3  PB_3
+#define D4  PB_5
+#define D5  PB_4
+#define D6  PB_10
+#define D7  PA_8
+#define D8  PA_9
+#define D9  PC_7
+#define D10 PB_6
+#define D11 PA_7
+#define D12 PA_6
+#define D13 PA_5
+// アナログ
+#define A0  PA_0
+#define A1  PA_1
+#define A2  PA_4
+#define A3  PB_0
+#define A4  PC_1
+#define A5  PC_0
+
+#define B1  PC_13
+// その他マクロ定義
+#define SAMPLING_TIME           0.01    // 100Hz
+#define ENC_PULSE_PER_METER     1965
+#define KP_LEFT     0.70
+#define KI_LEFT     0
+#define KD_LEFT     0
+#define KP_RIGHT    0.75
+#define KI_RIGHT    0
+#define KD_RIGHT    0
+#define SPEED_LEFT  (double)(get_enc_left / 19.65)
+#define SPEED_RIGHT (double)(get_enc_right / 19.65)
+
+// 入出力モード、各種モードの設定
+BusIn sw(B1);
+BusIn in(D5, D6, D11, D12);
+PwmOut motor_left(D4);
+PwmOut motor_right(D10);
+DigitalOut left(D3);
+DigitalOut right(D9);
+QEI enc_left(D5, D6, NC, 48, QEI::X4_ENCODING);
+QEI enc_right(D12, D11, NC, 48, QEI::X4_ENCODING);
+Ticker control;
+ros::NodeHandle nh;
+
+// グローバル変数の宣言
+int get_enc_left;
+int get_enc_right;
+int enc_center;
+double SpeedIntegral_left = 0;
+double iSpeedBefore_left = 0;
+double TargetSpeed_left;
+double pwm_left;
+double SpeedIntegral_right = 0;
+double iSpeedBefore_right = 0;
+double TargetSpeed_right;
+double pwm_right;
+char str_left[13] = "mode left   ";
+char str_right[13] = "mode right  ";
+char str_forward[13] = "mode forward";
+char str_back[13] = "mode back   ";
+char str_nutral[13] = "mode nutral ";
+// 関数のプロトタイプ宣言
+void function(void);
+void motor_speed_left( void );
+void motor_speed_right( void );
+void messageCb(const geometry_msgs::Twist& msg);
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+ros::Subscriber<geometry_msgs::Twist> sub("diff_drive_controller/cmd_vel", &messageCb);
+
+Timer t;
+
+int main()
+{
+  
+  in.mode(PullUp);
+    control.attach(&function, SAMPLING_TIME);       //台車の速度制御用のタイマー関数を設定
+    
+  t.start();
+  long vel_timer = 0;
+  nh.getHardware()->setBaud(57600);
+  nh.initNode();
+  nh.subscribe(sub);
+  nh.advertise(chatter);
+  while (1)
+  {
+    if (t.read_ms() > vel_timer)
+    {
+      // 停止
+        TargetSpeed_left = 0;
+        TargetSpeed_right = 0;
+        motor_left = 0;
+        motor_right = 0;
+        vel_timer = t.read_ms() + 500;    // 500ms毎に実行
+    }
+    nh.spinOnce();
+    motor_left = pwm_left;
+    motor_right = pwm_right;
+    wait_ms(1);
+  }
+}
+
+// 割り込み関数
+void function(void){
+    motor_speed_left();
+    motor_speed_right();
+    }
+
+void motor_speed_left( void ){
+    double out;
+    get_enc_left = enc_left.getPulses();
+    enc_left.reset();
+    double div_speed_left = TargetSpeed_left - SPEED_LEFT;
+    out = KP_LEFT * div_speed_left;
+    if(out < 0){
+        out *= -1;
+        left = 1;
+    } else {
+        left = 0;        
+    }
+    pwm_left = out;
+} 
+void motor_speed_right( void ){
+    double out;
+    get_enc_right = enc_right.getPulses();
+    enc_right.reset();
+    double div_speed_right = TargetSpeed_right - SPEED_RIGHT;
+    out = KP_RIGHT * div_speed_right;
+    if(out < 0){
+        out *= -1;
+        right = 1;
+    } else {
+        right = 0;        
+    }
+    pwm_right = out;
+}
+
+void messageCb(const geometry_msgs::Twist& msg){
+  double tmp = 0;
+  
+  TargetSpeed_left = msg.linear.x;
+  TargetSpeed_right = msg.linear.x;
+  tmp = 0;
+  
+  if(msg.angular.z < 0){
+      tmp = 1 - (-msg.angular.z / 3);
+      TargetSpeed_left = msg.linear.x;
+      TargetSpeed_right = msg.linear.x * tmp;
+  } else if(msg.angular.z > 0){
+      tmp = 1 - (msg.angular.z / 3);
+      TargetSpeed_left = msg.linear.x * tmp;
+      TargetSpeed_right = msg.linear.x;
+  } else {
+      TargetSpeed_right = msg.linear.x;
+      TargetSpeed_right = msg.linear.x;
+  }
+  motor_left = pwm_left;
+  motor_right = pwm_right;
+  
+  /*
+  if (msg.angular.z == 0 && msg.linear.x == 0){
+    // 停止
+    TargetSpeed_left = 0;
+    TargetSpeed_right = 0;
+    motor_left = pwm_left;
+    motor_right = pwm_right;
+    str_msg.data = str_nutral;
+    chatter.publish( &str_msg );
+  } else {
+    if (msg.angular.z < 0){
+      // 右旋回
+        TargetSpeed_left = 1.0;
+        TargetSpeed_right = 0;
+        motor_left = pwm_left;
+        motor_right = pwm_right;
+        str_msg.data = str_right;
+        chatter.publish( &str_msg );
+    } else if (msg.angular.z > 0){
+      // 左旋回
+        TargetSpeed_left = 0;
+        TargetSpeed_right = 1.0;
+        motor_left = pwm_left;
+        motor_right = pwm_right;
+        str_msg.data = str_left;
+        chatter.publish( &str_msg );
+    } else if (msg.linear.x < 0){
+      // 後進
+        TargetSpeed_left = -1.0;
+        TargetSpeed_right = -1.0;
+        motor_left = pwm_left;
+        motor_right = pwm_right;
+        str_msg.data = str_back;
+        chatter.publish( &str_msg );
+    } else if (msg.linear.x > 0){
+      // 前進
+        TargetSpeed_left = 1.0;
+        TargetSpeed_right = 1.0;
+        motor_left = pwm_left;
+        motor_right = pwm_right;
+        str_msg.data = str_forward;
+        chatter.publish( &str_msg );
+    }
+  }
+  */
+}
\ No newline at end of file