記事一覧

M5camera+M5Atom Liteでメカナムホイール

M5camera+M5Atom Liteでメカナムホイール



こんにちは、RoboTakaoです

前回までで、M5cameraの動作テストをしてみましたが、以前作ったM5Atom Matrix+M5StickC+M5UnitVで作ったメカナムホイールのM5StickC+M5UnitVから換装しました。

M5camera23.jpg

M5camera15.jpeg

ついでにM5Atom Matrix から M5Atom Liteに地味に換装も行っています。
(理由は単に買ったから…)


結線図

M5camera16.png


M5camera側は前回のスケッチを使用

まずはiPhoneのSafariでストリーミングを受けるようにしました。

M5AtomのスケッチはBlynkのジョイスティックを1個で操作できるように少し変更(といっても条件分けが複雑になってしまいましたが)

動作確認

少し遅延がありますが、まあまあ良い感じです。

M5camera20.jpeg

M5camera23.jpg



M5Atomのスケッチ



#define BLYNK_PRINT Serial

#include "M5Atom.h"
#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>

char auth[] = "XXXXXXXXXXXXXXXXXXXXXXXX"; //メールで送られるAuth Token
char ssid[] = "XXXXXXXXXXXX";
char pass[] = "XXXXXXXXXXXX";

const uint8_t Srv_RF = 22, Srv_RR = 19, Srv_LF = 23, Srv_LR = 33; //GPIO No.
const uint8_t srvCH_RF = 0, srvCH_RR = 1, srvCH_LF = 2, srvCH_LR = 3; //チャンネル
const double PWM_Hz = 50; //PWM周波数
const uint8_t PWM_level = 16; //PWM 16bit(0~65535)

int mRF = 0, mRR = 0, mLF = 0, mLR = 0;

int pulseMIN = 2295; //700μsec 50Hz 16bit
int pulseMAX = 7533; //2300μsec 50Hz 16bit

int cont_max = 100;
int cont_thres = 40;
int cont_thres2 = 10;

int JoyStickSW = 0;

//右ジョイスティックのデータ受信
BLYNK_WRITE(V0) {
int x = param[0].asInt();
int y = param[1].asInt();

if(abs(x) > cont_thres){
if(y > cont_thres){
if(JoyStickSW == 0){ //前輪旋回
mRF = -x;
mRR = 0;
mLF = x;
mLR = 0;
}else if(JoyStickSW == 1 && x > 0){ //右斜め前
mRF = 0;
mRR = x;
mLF = x;
mLR = 0;
}else if(JoyStickSW == 1 && x < 0){ //左斜め前
mRF = -x;
mRR = 0;
mLF = 0;
mLR = -x;
}
}else if(y < -cont_thres){
if(JoyStickSW == 0){ //後輪旋回
mRF = 0;
mRR = -x;
mLF = 0;
mLR = x;
}else if(JoyStickSW == 1 && x > 0){ //右斜め後
mRF = -x;
mRR = 0;
mLF = 0;
mLR = -x;
}else if(JoyStickSW == 1 && x < 0){ //左斜め後
mRF = 0;
mRR = x;
mLF = x;
mLR = 0;
}
}else{
if(JoyStickSW == 0){ //超信地旋回
mRF = -x;
mRR = -x;
mLF = x;
mLR = x;
}else if(JoyStickSW == 1){ //左右スライド
mRF = -x;
mRR = x;
mLF = x;
mLR = -x;
}
}
}else if(cont_thres2 < x && x < cont_thres){
if(y > cont_thres){
if(JoyStickSW == 0){ //右前信地旋回
mRF = 0;
mRR = 0;
mLF = y;
mLR = y;
}else if(JoyStickSW == 1){ //右斜め前
mRF = 0;
mRR = x;
mLF = x;
mLR = 0;
}
}else if(y < -cont_thres){
if(JoyStickSW == 0){ //右後信地旋回
mRF = y;
mRR = y;
mLF = 0;
mLR = 0;
}else if(JoyStickSW == 1){ //右斜め後
mRF = -x;
mRR = 0;
mLF = 0;
mLR = -x;
}
}
}else if(-cont_thres < x && x < -cont_thres2){
if(y > cont_thres){
if(JoyStickSW == 0){ //左前信地旋回
mRF = y;
mRR = y;
mLF = 0;
mLR = 0;
}else if(JoyStickSW == 1){ //左斜め前
mRF = -x;
mRR = 0;
mLF = 0;
mLR = -x;
}
}else if(y < -cont_thres){
if(JoyStickSW == 0){ //左後信地旋回
mRF = 0;
mRR = 0;
mLF = y;
mLR = y;
}else if(JoyStickSW == 1){ //左斜め後
mRF = 0;
mRR = x;
mLF = x;
mLR = 0;
}
}
}else if(abs(x) <= cont_thres2 && abs(y)>cont_thres){ //前後進
mRF = y;
mRR = y;
mLF = y;
mLR = y;
}else{
mRF = 0;
mRR = 0;
mLF = 0;
mLR = 0;
}
}

//左ジョイスティックのデータ受信
BLYNK_WRITE(V1) {
JoyStickSW = param.asInt();
Serial.println(JoyStickSW);
}

void motor_drive(int motor_RF, int motor_RR, int motor_LF, int motor_LR){
motor_RF = map(motor_RF, -cont_max, cont_max, pulseMIN, pulseMAX);
motor_RR = map(motor_RR, -cont_max, cont_max, pulseMIN, pulseMAX);
motor_LF = map(motor_LF, -cont_max, cont_max, pulseMIN, pulseMAX);
motor_LR = map(motor_LR, -cont_max, cont_max, pulseMIN, pulseMAX);

ledcWrite(srvCH_RF, motor_RF);
ledcWrite(srvCH_RR, motor_RR);
ledcWrite(srvCH_LF, motor_LF);
ledcWrite(srvCH_LR, motor_LR);
}

//void led_red(){
// M5.dis.drawpix(0, 0x000000); //なぜか最初のLEDが付かないためダミー
// for(int i=0; i<25; i++){
// M5.dis.drawpix(i, 0x00ff00); //red
// }
//}

void led_green(){
M5.dis.drawpix(0, 0xff0000);
// for(int i=0; i<25; i++){
// M5.dis.drawpix(i, 0xff0000); //green
// }
}

void setup() {
Serial.begin(151200);
M5.begin(true, false, true); // void M5Atom::begin(bool SerialEnable , bool I2CEnable , bool DisplayEnable )
pinMode(Srv_RF, OUTPUT);
pinMode(Srv_RR, OUTPUT);
pinMode(Srv_LF, OUTPUT);
pinMode(Srv_LR, OUTPUT);

Blynk.begin(auth, ssid, pass);

led_green();

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

//モータのピンとチャンネルの設定
ledcAttachPin(Srv_RF, srvCH_RF);
ledcAttachPin(Srv_RR, srvCH_RR);
ledcAttachPin(Srv_LF, srvCH_LF);
ledcAttachPin(Srv_LR, srvCH_LR);
}

void loop() {
M5.update();
Blynk.run();

motor_drive(-mRF, -mRR, mLF, mLR);
delay(10);
}


スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

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

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

スポンサーリンク