init

Dependencies:   mbed ros_lib_kinetic

main.cpp

Committer:
surpace0924
Date:
2019-10-29
Revision:
0:b1907fbc1696

File content as of revision 0:b1907fbc1696:

#include "mbed.h"
#include <ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int32.h>

float TH = 0;
double a[9];
bool onLine[7] = {false};
int binarized_data = 0;
void messageCb(const std_msgs::Int32 &msg)
{
    binarized_data = msg.data;
}

Ticker flipper;
ros::NodeHandle nh;
std_msgs::Float32MultiArray send_data;
ros::Publisher pub_to_ros("line_sensor/y", &send_data);
ros::Subscriber<std_msgs::Int32> sub("line_sensor/binarized/y", &messageCb);


void flip() {
    for(int i = 0; i < 9; i++)
    {
        send_data.data[i] = a[i];
    }

    // ROSへデータを送信
    pub_to_ros.publish(&send_data);
}

AnalogIn sensor[] = {
    AnalogIn(PA_7),
    AnalogIn(PA_6),
    AnalogIn(PA_5),
    AnalogIn(PA_4),
    AnalogIn(PB_0),
    AnalogIn(PB_1),
    AnalogIn(PA_3),
    AnalogIn(PA_1),
    AnalogIn(PA_0)};

DigitalOut leds[] = {
    DigitalOut(PA_9),
    DigitalOut(PA_10),
    DigitalOut(PA_8),
    DigitalOut(PB_7),
    DigitalOut(PB_6),
    DigitalOut(PF_0),
    DigitalOut(PF_1)};

int main()
{
    nh.initNode();
    nh.advertise(pub_to_ros);
    nh.subscribe(sub);
    
    // 送信パケットの初期化
    send_data.data_length = 9;
    send_data.data = (float *)malloc(sizeof(float) * send_data.data_length);
    for (int i = 0; i < send_data.data_length; i++)
        send_data.data[i] = 0.0;

    // 送信関数を一定周期で呼び出す
    flipper.attach(&flip, 0.02);
    
    while (1)
    {
        for (int i = 0; i < 9; i++)
        {
            a[i] = sensor[i];
        }
        for (int i = 0; i < 7; i++)
        {   
            leds[i] = (int)((binarized_data >> i) & 1);
        }

        nh.spinOnce();
    }
}