DFRobot様よりメカナムホイールをいただきましたので、ラジコンカーを製作してみました。

Mecanum Wheel Kit (48mm - 4 Wheels) - DFRobot

www.dfrobot.com

思想

非常によいメカナムホイールが手に入ったので、これをどう利用しようかとウキウキで考えました。

長男くんはずっとLEGOが大好きで毎日いろいろ製作しています。

長男くんのレゴ作品集

そこで、折角ですのでLEGOで色々カスタマイズできるラジコンにしようと思います。

構成

メカナムホイールはレゴを接続できる360°回転サーボで回すことにしました。

2159.0￥ |4ピース/ロット9グラムサーボオタクサーボ360度回転サーボマイクロ: ビットレゴロボットスマートカーdiyグリーンオレンジgeekservo|Home Automation Modules| - AliExpress

aliexpress.com

サーボとしても低価格で非常に良い買い物をしました♪

ホイールとサーボの接続のために3Dプリンタで治具を製作しました。

サーボの制御にはM5StickCとM5StickC 8Servos Hatを用いました。

M5StickC 8Servos Hat - スイッチサイエンス

スイッチサイエンス

　
M5StickCとM5StickC 8Servos Hatによるビークル制御は以前試しているので慣れたものです。

M5StickC 8Servos Hat でラジコンを堪能

車体

車体はLEGOで構築しました。

Blynk設定

ここではスマホアプリのblynkでBLE通信で車体を動かします。

新規プロジェクトを作成します。ハードウェアはESP32 Dev Boardを選択。Conection TypeにはBLEを指定します。
AUTH TOKENはArduinoコード生成時に使用します (アカウント登録したメールに送信されます)。

ウィジェットとしてBLEウィジェットとNumeric InputウィジェットとJoystickウェジットを配置します。

Numeric InputウェジットはV1を使用します。
メカナムホイールの回転速度を決定します (0~90)。

JoystickウェジットはバーチャルピンV0を使用します。

ジョイスティックの設定はx軸とy軸の出力をMERGEしてヴァーチャルピンV0に出力させ、それぞれ値は-100~100としました。

AUTO RETURNはONにしてジョイスティックはタップ移動後離すと自動的に中央に戻します。
ROTATE ON TILTはOFFにてスマホの回転に依存せずのx, y軸を固定とします。

メカナムホイール動作

Arduinoコード

ジョイスティックのx, y値をM5StickCで受けて角度に変換してディスプレイ表示しています。
角度のディスプレイ表示は以下のM5StickCのサンプルコードTFT_Pie_Chartを利用しました。

8Servos Hatのサンプルコードを元に制御しております。　

#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT

#include <M5StickC.h>
#include "IIC_servo.h"

#include <BlynkSimpleEsp32_BLE.h>
#include <BLEDevice.h>
#include <BLEServer.h>

// You should get Auth Token in the Blynk App.
// Go to the Project Settings (nut icon).
char auth[] = "BlynkアプリのYourAuthTokenを入力";

int x = 0, y = 0;
int thFL, thFR, thRL, thRR;
float th = 0.0;

int stopValue = 90;
int Speed = 30;

#define DEG2RAD 0.0174532925
byte inc = 0;
unsigned int col = 0;


//ヴァーチャルピンV0 
BLYNK_WRITE(V0) {
  x = param[0].asInt();
  y = param[1].asInt();

  th = atan2(y,-x) *180.0/M_PI;
  if(th < 0){
    th += 360;
  }
  Serial.print("th : ");
  Serial.println(th);
  
  fillSegment(40, 80, 0, 360, 35, TFT_BLACK);
}

BLYNK_WRITE(V1) {
  Speed = param.asInt();
  Serial.print("Speed : ");
  Serial.println(Speed);
}


void setup(){
  // Debug console
  Serial.begin(115200);
  Serial.println("Waiting for connections...");

  Blynk.setDeviceName("Blynk");
  Blynk.begin(auth);

  M5.begin();
  M5.Axp.ScreenBreath(10);
  M5.Lcd.fillScreen(TFT_BLACK);

  M5.Lcd.println(" ");
  M5.Lcd.println(" Power ON");

  IIC_Servo_Init();     //sda  0     scl  26
  Servo_angle_set(1,stopValue);
  Servo_angle_set(4,stopValue);
  Servo_angle_set(5,stopValue);
  Servo_angle_set(8,stopValue);

  while(Blynk.connect() == false){
    Serial.print(".");
  }
  
  M5.Lcd.print(" BLE Connect");
}

void loop(){
  Blynk.run();
  
  if(x != 0 && y != 0){
    fillSegment(40, 80, int(th) - 90, 5, 35, TFT_GREEN);

    //前進
    if(th > 70.0 && th < 110.0){
      thFL = stopValue + Speed;
      thFR = stopValue - Speed;
      thRL = stopValue + Speed;
      thRR = stopValue - Speed;

    //後進
    }else if(th > 250.0 && th < 290.0){
      thFL = stopValue - Speed;
      thFR = stopValue + Speed;
      thRL = stopValue - Speed;
      thRR = stopValue + Speed;

    //左
    }else if(th > 340.0 || th < 20.0){
      thFL = stopValue - Speed;
      thFR = stopValue - Speed;
      thRL = stopValue + Speed;
      thRR = stopValue + Speed;

    //右
    }else if(th > 160.0 && th < 200.0){
      thFL = stopValue + Speed;
      thFR = stopValue + Speed;
      thRL = stopValue - Speed;
      thRR = stopValue - Speed;

    //左上
    }else if(th > 25.0 && th < 65.0){
      thFL = stopValue;
      thFR = stopValue - Speed;
      thRL = stopValue + Speed;
      thRR = stopValue;

    //左下
    }else if(th > 295.0 && th < 335.0){
      thFL = stopValue - Speed;
      thFR = stopValue;
      thRL = stopValue;
      thRR = stopValue + Speed;

    //右上
    }else if(th > 115.0 && th < 155.0){
      thFL = stopValue + Speed;
      thFR = stopValue;
      thRL = stopValue;
      thRR = stopValue - Speed;

    //右下
    }else if(th > 205.0 && th < 245.0){
      thFL = stopValue;
      thFR = stopValue + Speed;
      thRL = stopValue - Speed;
      thRR = stopValue;
      
    //停止
    }else{ 
      thFL = stopValue;
      thFR = stopValue;
      thRL = stopValue;
      thRR = stopValue;
    }

    Servo_angle_set(8, thFL);
    Servo_angle_set(4, thFR);
    Servo_angle_set(5, thRL);
    Servo_angle_set(1, thRR);

    
    

    
  }else{
    fillSegment(40, 80, 0, 360, 35, TFT_BLACK);
    Servo_angle_set(1,stopValue);
    Servo_angle_set(4,stopValue);
    Servo_angle_set(5,stopValue);
    Servo_angle_set(8,stopValue);
  }
}



// #########################################################################
// Draw circle segments
// #########################################################################

// x,y == coords of centre of circle
// start_angle = 0 - 359
// sub_angle   = 0 - 360 = subtended angle
// r = radius
// colour = 16 bit colour value

int fillSegment(int x, int y, int start_angle, int sub_angle, int r, unsigned int colour)
{
  // Calculate first pair of coordinates for segment start
  float sx = cos((start_angle - 90) * DEG2RAD);
  float sy = sin((start_angle - 90) * DEG2RAD);
  uint16_t x1 = sx * r + x;
  uint16_t y1 = sy * r + y;

  // Draw colour blocks every inc degrees
  for (int i = start_angle; i < start_angle + sub_angle; i++) {

    // Calculate pair of coordinates for segment end
    int x2 = cos((i + 1 - 90) * DEG2RAD) * r + x;
    int y2 = sin((i + 1 - 90) * DEG2RAD) * r + y;

    M5.Lcd.fillTriangle(x1, y1, x2, y2, x, y, colour);

    // Copy segment end to sgement start for next segment
    x1 = x2;
    y1 = y2;
  }
}


// #########################################################################
// Return the 16 bit colour with brightness 0-100%
// #########################################################################
unsigned int brightness(unsigned int colour, int brightness)
{
  byte red   = colour >> 11;
  byte green = (colour & 0x7E0) >> 5;
  byte blue  = colour & 0x1F;

  blue =  (blue * brightness)/100;
  green = (green * brightness)/100;
  red =   (red * brightness)/100;

  return (red << 11) + (green << 5) + blue;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

#define BLYNK_PRINT Serial

#define BLYNK_USE_DIRECT_CONNECT

#include <M5StickC.h>

#include "IIC_servo.h"

#include <BlynkSimpleEsp32_BLE.h>

#include <BLEDevice.h>

#include <BLEServer.h>

// You should get Auth Token in the Blynk App.

// Go to the Project Settings (nut icon).

char auth[] = "BlynkアプリのYourAuthTokenを入力";

int x = 0, y = 0;

int thFL, thFR, thRL, thRR;

float th = 0.0;

int stopValue = 90;

int Speed = 30;

#define DEG2RAD 0.0174532925

byte inc = 0;

unsigned int col = 0;

//ヴァーチャルピンV0

BLYNK_WRITE(V0) {

x = param[0].asInt();

y = param[1].asInt();

th = atan2(y,-x) *180.0/M_PI;

if(th < 0){

th += 360;

}

Serial.print("th : ");

Serial.println(th);

fillSegment(40, 80, 0, 360, 35, TFT_BLACK);

}

BLYNK_WRITE(V1) {

Speed = param.asInt();

Serial.print("Speed : ");

Serial.println(Speed);

}

void setup(){

// Debug console

Serial.begin(115200);

Serial.println("Waiting for connections...");

Blynk.setDeviceName("Blynk");

Blynk.begin(auth);

M5.begin();

M5.Axp.ScreenBreath(10);

M5.Lcd.fillScreen(TFT_BLACK);

M5.Lcd.println(" ");

M5.Lcd.println(" Power ON");

IIC_Servo_Init(); //sda 0 scl 26

Servo_angle_set(1,stopValue);

Servo_angle_set(4,stopValue);

Servo_angle_set(5,stopValue);

Servo_angle_set(8,stopValue);

while(Blynk.connect() == false){

Serial.print(".");

}

M5.Lcd.print(" BLE Connect");

}

void loop(){

Blynk.run();

if(x != 0 && y != 0){

fillSegment(40, 80, int(th) - 90, 5, 35, TFT_GREEN);

//前進

if(th > 70.0 && th < 110.0){

thFL = stopValue + Speed;

thFR = stopValue - Speed;

thRL = stopValue + Speed;

thRR = stopValue - Speed;

//後進

}else if(th > 250.0 && th < 290.0){

thFL = stopValue - Speed;

thFR = stopValue + Speed;

thRL = stopValue - Speed;

thRR = stopValue + Speed;

//左

}else if(th > 340.0 || th < 20.0){

thFL = stopValue - Speed;

thFR = stopValue - Speed;

thRL = stopValue + Speed;

thRR = stopValue + Speed;

//右

}else if(th > 160.0 && th < 200.0){

thFL = stopValue + Speed;

thFR = stopValue + Speed;

thRL = stopValue - Speed;

thRR = stopValue - Speed;

//左上

}else if(th > 25.0 && th < 65.0){

thFL = stopValue;

thFR = stopValue - Speed;

thRL = stopValue + Speed;

thRR = stopValue;

//左下

}else if(th > 295.0 && th < 335.0){

thFL = stopValue - Speed;

thFR = stopValue;

thRL = stopValue;

thRR = stopValue + Speed;

//右上

}else if(th > 115.0 && th < 155.0){

thFL = stopValue + Speed;

thFR = stopValue;

thRL = stopValue;

thRR = stopValue - Speed;

//右下

}else if(th > 205.0 && th < 245.0){

thFL = stopValue;

thFR = stopValue + Speed;

thRL = stopValue - Speed;

thRR = stopValue;

//停止

}else{

thFL = stopValue;

thFR = stopValue;

thRL = stopValue;

thRR = stopValue;

}

Servo_angle_set(8, thFL);

Servo_angle_set(4, thFR);

Servo_angle_set(5, thRL);

Servo_angle_set(1, thRR);

}else{

fillSegment(40, 80, 0, 360, 35, TFT_BLACK);

Servo_angle_set(1,stopValue);

Servo_angle_set(4,stopValue);

Servo_angle_set(5,stopValue);

Servo_angle_set(8,stopValue);

}

// #########################################################################

// Draw circle segments

// #########################################################################

// x,y == coords of centre of circle

// start_angle = 0 - 359

// sub_angle = 0 - 360 = subtended angle

// r = radius

// colour = 16 bit colour value

int fillSegment(int x, int y, int start_angle, int sub_angle, int r, unsigned int colour)

{

// Calculate first pair of coordinates for segment start

float sx = cos((start_angle - 90) * DEG2RAD);

float sy = sin((start_angle - 90) * DEG2RAD);

uint16_t x1 = sx * r + x;

uint16_t y1 = sy * r + y;

// Draw colour blocks every inc degrees

for (int i = start_angle; i < start_angle + sub_angle; i++) {

// Calculate pair of coordinates for segment end

int x2 = cos((i + 1 - 90) * DEG2RAD) * r + x;

int y2 = sin((i + 1 - 90) * DEG2RAD) * r + y;

M5.Lcd.fillTriangle(x1, y1, x2, y2, x, y, colour);

// Copy segment end to sgement start for next segment

x1 = x2;

y1 = y2;

}

// #########################################################################

// Return the 16 bit colour with brightness 0-100%

// #########################################################################

unsigned int brightness(unsigned int colour, int brightness)

{

byte red = colour >> 11;

byte green = (colour & 0x7E0) >> 5;

byte blue = colour & 0x1F;

blue = (blue * brightness)/100;

green = (green * brightness)/100;

red = (red * brightness)/100;

return (red << 11) + (green << 5) + blue;

}

動作

レゴカーできた。
あとは長男くんに好きにカスタマイズいただこう。#レゴ #LEGO #Blynk #メカナムホイール pic.twitter.com/1aFs8EihGW

— HomeMadeGarbage (@H0meMadeGarbage) January 3, 2021

おわりに

憧れのメカナムホイールを用いてレゴカーができました！

あとは長男くんに自由にカスタマイズしていただきましょう。

コメントはこちらからコメントをキャンセル

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。

思想

構成

車体

Blynk設定

メカナムホイール動作

Arduinoコード

動作

おわりに

関連記事

コメントはこちらから コメントをキャンセル

コメントはこちらからコメントをキャンセル