Revision 1:f6a9a3a7455d, committed 2022-05-04

Comitter:

yakifrog

Date:

Wed May 04 15:41:36 2022 +0000

Parent:

0:450a7208c682

Child:

2:87caac24d995

Commit message:

aaaa

Changed in this revision

--- a/PS4_to_Arduino.lib	Tue May 03 08:02:58 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/teams/OITRP_2022/code/PS4_to_Arduino/#3d6274f8c368

--- a/URF.cpp	Tue May 03 08:02:58 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,74 +0,0 @@
-#include "URF.h"
-
-#include "mbed.h"
-
-URF::URF(PinName URF_PIN)
-    : _event(URF_PIN)
-    , _cmd(URF_PIN)
-    , _timer()
-{
-    _event.rise(callback(this, &URF::_start));
-    _event.fall(callback(this, &URF::_stop));
-    _METER_PER_SECOND = 334.0;    // m/s
-    
-    _Busy = false;
-    
-    _cmd.output();
-    _cmd.write(0);
-    
-    _timer.stop();
-    _timer.reset();
-}
-
-void URF::_start(void)
-{
-//    _Valid = false;  // start the timere, and invalidate the current time.
-    _Busy = true;
-    _timer.reset();
-    _timer.start();
-}
-
-void URF::_stop(void)
-{
-//    _Valid = true;  // When it stops, update the time
-    _Busy = false;
-    _timer.stop();
-    _Time = _timer.read_us();
-}
-
-void URF::send(void)
-{
-    if(_Busy){
-        _timer.stop();
-        _Time = _timer.read_us();
-        return ;
-    }
-    _cmd.output();
-    _cmd.write(1);
-    wait_us(10);
-    _cmd.write(0);
-    _cmd.input();
-}
-
-void URF::Set_MpS(float MpS)
-{
-    _METER_PER_SECOND = MpS;
-}
-
-int URF::read(int scale)
-// -1 means not valid.
-{
-//    if(_Valid && ~_Busy)
-        return int(((_Time*(_METER_PER_SECOND / float(scale)))/2.0f) + 0.5f);
-//    else
-//        return -1;
-}
-
-int URF::read(void)
-// -1 means not valid.
-{
-//    if(_Valid && ~_Busy)
-        return int(((_Time*(_METER_PER_SECOND / 1000.0f))/2.0f) + 0.5f);
-//    else
-//        return -1;
-}
\ No newline at end of file

--- a/URF.h	Tue May 03 08:02:58 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,99 +0,0 @@
-/* mbed URF Library
- * Copyright (c) 2007-2010 rosienej
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#ifndef MBED_URF_H
-#define MBED_URF_H
-
-#include "mbed.h"
-/** URF class, based on an InterruptIn pin, and a timer
- *  works with the UltrasonicRangeFinder sensor (Parallax PING))), HC-SR04, etc...)
- *
- * Example:
- * @code
- * // Continuously send URFs and read the sensor
- * #include "mbed.h"
- * #include "URF.h"
- *
- * URF Ping(p21);
- *
- * int main() {
- *     int range;
-
- *     while(1) {
- *
- *        Ping.send();
- *        wait_ms(30);
- *        range = Ping.read(mm);
- *     }
- * }
- * @endcode
- */
-
-#define mm 1000
-#define cm 100
-
-class URF
-{
-public:
-    /** Create a URF object connected to the specified InterruptIn pin
-     *
-     * @param URF_PIN InterruptIn pin to connect to
-     */
-    URF(PinName URF_PIN);
-
-    /** Sends a URF
-     *
-     * @param none
-     */
-    void send(void);
-
-    /** Set the meter per second, default 334.0 m/s
-     *
-     * @param Meter per second in m/s
-     */
-    void Set_MpS(float SoS_ms);
-
-    /** Read the result in centimeters
-      *
-      * @param none
-      */
-    int read(int scale);
-    int read(void);
-
-protected:
-
-    InterruptIn     _event;
-    DigitalInOut    _cmd;
-    Timer           _timer;
-
-//    bool  _Valid;
-    bool  _Busy;
-    int   _Time;
-    float _METER_PER_SECOND; /* in milliseconds */
-    float _mm_per_us;
-
-    void  _start(void);
-    void  _stop(void);
-
-};
-
-#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/URF.lib	Wed May 04 15:41:36 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/OITRP_2022/code/URF/#6954fb95ce22

--- a/URF_check.txt	Tue May 03 08:02:58 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,87 +0,0 @@
-#include "mbed.h"
-#include "URF.h"
-
-Serial pc(USBTX, USBRX, 115200);
-
-URF usdRight(D6);
-URF usdLeft(D8);
-
-Ticker update;
-
-DigitalOut led(LED1);
-DigitalOut led2(PC_4);
- 
-DigitalIn btn(USER_BUTTON);
-
-void inter(void)
-{
-    usdRight.send();
-    usdLeft.send();
-}
-
-void URF(int dataRight, int dataLeft){
-    if(dataRight>100 && dataRight<=230){
-        pc.printf("right slow\n");
-        //Yonrin(-0.3f, 0);
-        if(dataLeft>100 && dataLeft<=230){
-            pc.printf("left slow\n");
-            //Yonrin(0.3f, 0);
-        }
-        else if(dataLeft<=100){
-            pc.printf("left stop\n");
-            //Yonrin(0);
-        }
-    }
-    else if(dataRight<=100){
-        pc.printf("right stop\n");
-        //Yonrin(0);
-        if(dataLeft>100 && dataLeft<=230){
-            pc.printf("left slow\n");
-            //Yonrin(0.3f, 0);
-        }
-        else if(dataLeft<=100){
-            pc.printf("left stop\n");
-            //Yonrin(0);
-        }
-    }
-    else if(dataLeft>100 && dataLeft<=230){
-        pc.printf("left slow\n");
-        //Yonrin(0.3f, 0);
-        if(dataRight>100 && dataRight<=230){
-            pc.printf("right slow\n");
-            //Yonrin(-0.3f, 0);
-        }
-        else if(dataRight<=100){
-            pc.printf("right stop\n");
-            //Yonrin(0);
-        }
-    }
-    else if(dataLeft<=100){
-        pc.printf("left stop\n");
-        //Yonrin(0);
-        if(dataRight>100 && dataRight<=230){
-            pc.printf("right slow\n");
-            //Yonrin(-0.3f, 0);
-        }
-        else if(dataRight<=100){
-            pc.printf("right stop\n");
-            //Yonrin(0);
-        }
-    }
-}
-
-int main(){
-    pc.printf("check1\n");
-
-    update.attach(&inter, 0.06);
-
-    while(1){
-        int dataRight = usdRight.read(mm);
-        int dataLeft = usdLeft.read(mm);
-        //pc.printf("right: %d, left: %d\n", dataRight, dataLeft);
-        
-        if(dataRight<=230 || dataLeft<=230){
-            URF(dataRight,dataLeft);
-        }
-    }
-}
\ No newline at end of file

--- 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ros_lib_melodic.lib	Wed May 04 15:41:36 2022 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/garyservin/code/ros_lib_melodic/#da82487f547e