File content as of revision 1:f6a9a3a7455d:

#include "mbed.h"
#include "MDDS30_oit.h"
#include "URF.h"
#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>

ros::NodeHandle nh;

URF usdRight(D6); // 右側に取り付ける超音波センサー
URF usdLeft(D8);  // 左側に取り付ける超音波センサー

Ticker update; // よくわからないけど、「24回/秒」処理されるってことかしら。

DigitalOut led(LED1);
DigitalOut led2(PC_4);
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();
}


// 四輪オムニの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);
        
    MD1.left(duty[0]);
    MD1.right(duty[1]);
    MD2.left(duty[2]);
    MD2.right(duty[3]);
    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); // 上下，左右
}


// 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;
            }
        }
    }
}


// --- 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サイズの関係でバグって今は使えない（単に送る情報量が多すぎる）
// ----------- //


int main(){
    //setup();
    nh.initNode(); // ノードを初期化
    nh.getHardware()->setBaud(57600);
    nh.advertise(chatter); // マイコンからROSにデータ送信するために必要な認証？のようなもの。(道を作っている）
    nh.subscribe(sub); // ROSからデータ取得
    nh.subscribe(sub2); // ROSからデータ取得
    //nh.subscribe(sub3);
    
    str_msg.data = hello;
    chatter.publish( &str_msg ); // ここで実際にROSにデータを送信している
    
    
    //void loop();
    while (1){
        int dataRight = usdRight.read(mm);
        int dataLeft = usdLeft.read(mm);
        nh.spinOnce(); // ROSはノードごとで処理をしているため、その処理を繰り返すためのオマジナイ
        wait_ms(10);
    }
}