abeの研究に利用:枚数判別システムを動作させるために必要となる

Dependencies:   ros_lib_indigo mbed

main.cpp

Committer:
_ai_
Date:
2021-02-24
Revision:
0:8589ed2aac65

File content as of revision 0:8589ed2aac65:

#include "mbed.h"
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Float32.h>

#define photoIC_R 3300  //受光IC抵抗


////クラス宣言
AnalogIn        intensity_of_light(A0);       //受光素子による抵抗変化の電圧値
DigitalOut      light_sw(D8);                 //発光素子を光らせるためのスイッチング(トランジスタ)


////プロトタイプ宣言
void init_func(void);       //初期化を行うための関数
void ir_led_switch_cb(const std_msgs::Int8&);


////グローバル変数
float analogIn_light_voltage=0.0;    //マイコンデジタルピンにかかる電圧


////ros msg
std_msgs::Int8 IR_LED_swich;               //ubuntuからIRLEDをON or OFFするかを決定するためのもの
std_msgs::Float32 photo_diode_value;    //photo_diodeの値をubuntuへと送るためのもの


////ros sub and pub
ros::NodeHandle n1;
ros::Subscriber<std_msgs::Int8> IR_LED_switch("/micon_ir_led_swich",&ir_led_switch_cb);
ros::Publisher photo_diode_pub("/micon_photo_diode_value", &photo_diode_value);


//-----------------------  ここまで 設定 ---------------------------------------//


//関数・初期値の初期化
void init_func(void)
{    
    n1.getHardware()->setBaud(115200);
    n1.initNode();

    n1.advertise(photo_diode_pub);
    n1.subscribe(IR_LED_switch);

    light_sw.write(0);
}


//LEDのON OFFを入れ替えるcb関数
void ir_led_switch_cb(const std_msgs::Int8& data_)
{
    light_sw = data_.data;
}


///// main program
int main(void)
{
    init_func();
    wait(1.0);

    for(;;) {
        n1.spinOnce();
        analogIn_light_voltage = intensity_of_light.read()*3.3;                   //0.0~1.0のレンジを0.0~3.3[V]のレンジに変更
        photo_diode_value.data =  analogIn_light_voltage;
        photo_diode_pub.publish(&photo_diode_value);
        wait(0.01);
    }
}