Diff: main.cpp

Revision:

1:f6a9a3a7455d

Parent:

0:450a7208c682

--- a/main.cpp	Tue May 03 08:02:58 2022 +0000
+++ b/main.cpp	Wed May 04 15:41:36 2022 +0000
@@ -1,153 +1,126 @@
 #include "mbed.h"
-#include "CAN.h"
-#include "PS4_Controller.h"
 #include "MDDS30_oit.h"
 #include "URF.h"
-
-PS4_controller PS4(PC_12,PD_2);
-
-Serial pc(USBTX, USBRX, 115200);
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Float32.h>
+#include <sensor_msgs/Joy.h>
+#include <sensor_msgs/LaserScan.h>
+#include <std_msgs/String.h>
 
-URF usdRight(D6);
-URF usdLeft(D8);
+ros::NodeHandle nh;
 
-Ticker update;
+URF usdRight(D6); // 右側に取り付ける超音波センサー
+URF usdLeft(D8);  // 左側に取り付ける超音波センサー
+
+Ticker update; // よくわからないけど、「24回/秒」処理されるってことかしら。
 
 DigitalOut led(LED1);
 DigitalOut led2(PC_4);
- 
-DigitalIn btn(USER_BUTTON);
+DigitalIn btn(USER_BUTTON); // USER_BUTTONってどこだろ
 MDDS30oit MD1(PC_10, PC_11);  //left:モーター番号1（右後輪）、right:モーター番号2（左後輪）
 MDDS30oit MD2(PA_0, PA_1);  //left:モーター番号3（右前輪）、right:モーター番号4（左前輪）
 
+
+// --- 初期設定 --- //
+double speed_ang = 0.0;
+double speed_lin = 0.0;
+
+double w_r = 0.0;
+double w_l = 0.0;
+
+float duty[4]={0.0f};
+// --------------- //
+
+
+// --- ROS --- //
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg); // マイコンからROSにデータを飛ばすことができるかを試す実験
+char hello[13] = "hello world!"; 
+// ----------- //
+
+
+// 超音波センサー
 void inter(void)
 {
     usdRight.send();
     usdLeft.send();
 }
 
-int i;
-int right = 0, left = 0;
-float duty[4]={0.0f};
 
-void Yonrin(float x,float y)       //四輪オムニのpwm値出し
-{
+// 四輪オムニのPWM値出し
+void Yonrin(float x,float y){
     duty[0] =-x/sqrtf(2)+y/sqrtf(2);
     duty[1] =-x/sqrtf(2)-y/sqrtf(2);
     duty[2] = x/sqrtf(2)-y/sqrtf(2);
     duty[3] = x/sqrtf(2)+y/sqrtf(2);
-    //pc.printf("a=%f, b=%f, c=%f, d=%f\r\n",duty[0],duty[1],duty[2],duty[3]);
         
-        MD1.left(duty[0]);
-        MD1.right(duty[1]);
-        MD2.left(duty[2]);
-        MD2.right(duty[3]);
-        wait(0.01);
+    MD1.left(duty[0]);
+    MD1.right(duty[1]);
+    MD2.left(duty[2]);
+    MD2.right(duty[3]);
+    wait(0.01);
 }
 
-void Yonrin(float x)
-{
-        //pc.printf("senkai = %f\r\n",x);
-        
-        MD1.left(x);
-        MD1.right(x);
-        MD2.left(x);
-        MD2.right(x);
-        
-        wait(0.01);
+
+// SLAMで経路計画した結果、送られてくる移動方向の入力を受け取って機体を制御する関数
+void slamControl(const geometry_msgs::Twist& msg){
+  // cmd_velを取得して制御
+
+  speed_ang = 1 - msg.angular.z; // 左右 (出力が１になると、一気に加速してしまうので１よりも小さくなってほしいために調整）
+  speed_lin = 1 - msg.linear.x; // 上下
+  w_r = -0.25f * speed_ang; // これも手動で調整した結果
+  w_l = 0.25f * speed_lin;
+  
+  Yonrin(w_l, w_r); // 上下，左右
 }
 
-void Yonrin(int mode)
-{
-    switch(mode) {
-        case 0:
-        MD1.left(0);
-        MD1.right(0);
-        MD2.left(0);
-        MD2.right(0);
+
+// PS4などのコントローラで機体を制御する関数
+void joyControl(const sensor_msgs::Joy& joy){
+    // axes[0]:Lスティック（左右）, axes[1]:Lスティック（上下）
+    Yonrin(joy.axes[1] * 0.23f, joy.axes[0] * (-0.23f));
+}
+
+
+// LiDARで障害物を検知して機体を制御する関数
+void lidarControl(const sensor_msgs::LaserScan& lidar){
+    if (lidar.range_min >= 0.1f && lidar.range_max <= 0.5f){ // 0.1m以上〜0.5m以下の場所に障害物がある場合、停止。
+        while(1){
+            Yonrin(0, 0);
+            if (lidar.range_max > 0.5f){ // 障害物が0.5mよりも遠くにならないと、抜け出せないループ。
+                break;
+            }
+        }
     }
 }
 
-int main(){
-    
-    wait_ms(100);
-    
-    PS4.send_UNO(1);
+
+// --- ROS --- //
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &slamControl); // SLAM データを取得したら関数が呼び出される仕組み
+ros::Subscriber<sensor_msgs::Joy> sub2("joy", &joyControl); // PS4 同上
+//ros::Subscriber<sensor_msgs::LaserScan> sub3("scan", &lidarCb); // LiDARの制御は、Bufferサイズの関係でバグって今は使えない（単に送る情報量が多すぎる）
+// ----------- //
+
 
-    while(!PS4.connect()){
-        pc.printf("1");
-        led=!led;
-        PS4.read_PAD();
-        pc.printf("2");
-        wait_ms(500);
-    }
-    led = 1;
+int main(){
+    //setup();
+    nh.initNode(); // ノードを初期化
+    nh.getHardware()->setBaud(57600);
+    nh.advertise(chatter); // マイコンからROSにデータ送信するために必要な認証？のようなもの。(道を作っている）
+    nh.subscribe(sub); // ROSからデータ取得
+    nh.subscribe(sub2); // ROSからデータ取得
+    //nh.subscribe(sub3);
     
-    update.attach(&inter, 0.06);
-
-    while(1){
+    str_msg.data = hello;
+    chatter.publish( &str_msg ); // ここで実際にROSにデータを送信している
+    
+    
+    //void loop();
+    while (1){
         int dataRight = usdRight.read(mm);
         int dataLeft = usdLeft.read(mm);
-        pc.printf("Left:%d, Right:%d\n", dataLeft, dataRight);
-        //pc.printf("right: %d, left: %d\n", dataRight, dataLeft);
-
-        //right = 0;  //機体の右側に障害物なし
-        //left = 0;   //機体の左側に障害物なし
-
-        PS4.read_PAD();
-        if(PS4.button(PS))PS4.disconnect();
-        if(PS4.connect()){
-            if(PS4.button(R1)==1){                
-                if(PS4.analog(PS_LX)<100||PS4.analog(PS_LX)>146||PS4.analog(PS_LY)<100||PS4.analog(PS_LY)>146){
-                float x_duty = ((PS4.analog(PS_LX)-128.0f)/127.0f)*0.7f;
-                float y_duty = -((PS4.analog(PS_LY)-128.0f)/127.0f)*0.7f;
-                //pc.printf("x_duty=%f y_duty=%f\r\n", x_duty, y_duty);
-                Yonrin(x_duty, y_duty);
-                /*wait(1.0);
-                Yonrin(0);*/
-                }
-                else if(PS4.analog(PS_R2)>10){
-                    float x=-((PS4.analog(PS_R2)/255.0f))*0.7f;
-                    Yonrin(x);
-                }
-                else if(PS4.analog(PS_L2)>10){
-                    float x=((PS4.analog(PS_L2)/255.0f))*0.7f;
-                    Yonrin(x);
-                }
-                else if(PS4.button(UP)==1){
-                    Yonrin(0,0.5f);
-                }
-                else if(PS4.button(DOWN)==1){
-                    Yonrin(0,-0.5f);
-                }
-                else if(PS4.button(RIGHT)==1){
-                    if (dataRight >= 400){
-                        Yonrin(-0.5f,0);
-                    }
-                    else{
-                        if (dataRight <= 400 && dataRight >= 200){
-                            Yonrin(-0.3f, 0);
-                       }
-                    }
-                }
-                else if(PS4.button(LEFT)==1){
-                    if (dataLeft >= 400){
-                        Yonrin(0.5f,0);
-                    }else{
-                        if (dataLeft <= 400 && dataLeft >= 200){
-                            Yonrin(0.3f, 0);
-                        }
-                    }
-                    
-                }
-                else Yonrin(0);
-            }
-            else Yonrin(0);
-        }
-        else Yonrin(0);
+        nh.spinOnce(); // ROSはノードごとで処理をしているため、その処理を繰り返すためのオマジナイ
+        wait_ms(10);
     }
-    /*if(PS4.button(PS)){
-        PS4.disconnect();
-    }
-    Yonrin(0);*/
 }
\ No newline at end of file