記事一覧

rosserial で M5Atom Lite WiFi経由で サーボを動かす

rosserial で M5Atom Lite WiFi経由で サーボを動かす



こんにちはRoboTakaoです。前回、rosserial+M5AtomでWiFi経由でLチカをしました。
今回は、サーボをうぼかしたいと思います。

rosserial18.png


ターミナルで

$ roscore
$ rosrun rosserial_python serial_node.py tcp

このタイミングでM5Atomを起動

$ rostopic pub servo std_msgs/UInt16 170 //170はサーボの角度

rosserial19.png

うまくサーボが回転しました。



スケッチ


/*
* rosserial Subscriber Example
* Blinks an LED on callback
*/
#include "M5Atom.h"
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <WiFi.h>

const uint8_t Srv0 = 25;//GPIO No.
const uint8_t srv_CH0 = 0; //チャンネル
const double PWM_Hz = 50; //PWM周波数
const uint8_t PWM_level = 16; //PWM 16bit(0~65535)

int pulseMIN = 1640; //0deg 500μsec 50Hz 16bit
int pulseMAX = 8190; //180deg 2500μsec 50Hz 16bit

int cont_min = 0;
int cont_max = 180;
int SrvAng = 4914; //90deg

bool IMU6886Flag = false;

const char* ssid = "XXXXXXXXXXXXX";
const char* password = "XXXXXXXXXXXX";

WiFiClient client;
IPAddress server(192, 168, 10, 7); //ROS core IP adress

class WiFiHardware {
public:
WiFiHardware() {};
void init() {
client.connect(server, 11411);
}
int read() {
return client.read();
}
void write(uint8_t* data, int length) {
for (int i = 0; i < length; i++)
client.write(data[i]);
}
unsigned long time() {
return millis(); // easy; did this one for you
}
};

ros::NodeHandle_ nh;

void servo_cb( const std_msgs::UInt16& cmd_msg){
SrvAng = map(cmd_msg.data, cont_min, cont_max, pulseMIN, pulseMAX);
ledcWrite(srv_CH0, SrvAng);
}

ros::Subscriber sub("servo", servo_cb);

void setupWiFi() {
WiFi.begin(ssid, password);
Serial.print("\nConnecting to "); Serial.println(ssid);
uint8_t i = 0;
while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
if (i == 21) {
Serial.print("Could not connect to"); Serial.println(ssid);
while (1) delay(500);
}
Serial.print("Ready! Use ");
Serial.print(WiFi.localIP());
Serial.println(" to access client");
}

void setup()
{
Serial.begin(115200);
// void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
M5.begin(true, false, true);
setupWiFi();

pinMode(Srv0, OUTPUT);

//モータのPWMのチャンネル、周波数の設定
ledcSetup(srv_CH0, PWM_Hz, PWM_level);

//モータのピンとチャンネルの設定
ledcAttachPin(Srv0, srv_CH0);

ledcWrite(srv_CH0, SrvAng);

nh.initNode();
nh.subscribe(sub);
}

void loop()
{
nh.spinOnce();
delay(1);
}

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/

スポンサーリンク