倒立振子ロボット ESP8266 RemoteXY操作

倒立振子ロボットをスマホアプリ［RemoteXY］を使って操作しました。

(2019年製作時はスマホアプリ［TouchOSC］［Blynk］を使って操作)

●同ブログ関連記事：【RemoteXY使い方】

 

動画

参考サイト

●OLDMAKERS【https://oldmakers.blog.jp/archives/6388847.html】

回路図

RemoteXY設定

【Properties】【Configuration】設定変更

［Connection］WiFi access point

［Board］ESP8266 based board

［Module］WiFi on chip

【Apply】実行

【joystick】をスマホ画面に貼付け形・色・文字を入力

作成したら。

【Get source code】実行

コードをArduino IDE　エディタ画面に貼付てスケッチを行う。

Arduino IDE設定

プログラム

//////////////////////////////////////////////

//        RemoteXY include library          //

//////////////////////////////////////////////

// you can enable debug logging to Serial at 115200

//#define REMOTEXY__DEBUGLOG    

// RemoteXY select connection mode and include library 

#define REMOTEXY_MODE__ESP8266WIFI_LIB_POINT

#include <ESP8266WiFi.h>

#include <RemoteXY.h>

// RemoteXY connection settings 

#define REMOTEXY_WIFI_SSID "RemoteXY"

#define REMOTEXY_WIFI_PASSWORD "12345678"

#define REMOTEXY_SERVER_PORT 6377

// RemoteXY configurate  

#pragma pack(push, 1)

uint8_t RemoteXY_CONF[] =   // 19 bytes

  { 255,2,0,0,0,12,0,16,3,1,5,0,7,15,51,51,21,205,31 };

  

// this structure defines all the variables and events of your control interface 

struct {

    // input variables

  int8_t joystick_1_x; // from -100 to 100  

  int8_t joystick_1_y; // from -100 to 100  

    // other variable

  uint8_t connect_flag;  // =1 if wire connected, else =0 

} RemoteXY;

#pragma pack(pop)

/////////////////////////////////////////////

//           END RemoteXY include          //

/////////////////////////////////////////////

#include "Wire.h"

#include "I2Cdev.h"

#include "MPU6050.h"

//ジャイロセンサーの定義

MPU6050 accelgyro;

int16_t ax, ay, az;//加速度　int16_tは2バイトの符号付き整数 

int16_t gx, gy, gz;//角速度　同上

#define I2C_SDA  4

#define I2C_CLK  5

#define Gry_offset 0

#define Gyr_Gain 0.00763358

#define Angle_offset  0 //車体傾き調整

#define RMotor_offset 10 

#define LMotor_offset 10 

#define Driving_Angle 30

#define pi 3.14159

float kp, ki, kd;

float r_angle, f_angle, omega;

float Turn_Speed = 0, Turn_Speed_K = 0;

float Run_Speed = 0, Run_Speed_K = 0;

float LOutput, ROutput;

unsigned long preTime = 0;

float SampleTime = 0.08;

unsigned long lastTime;

float Input, Output;

float errSum, dErr, error, lastErr;

float gear_ratio = 2; //ギア比率

int timeChange;

//モータードライバDRV8833の接続

#define AIN1 12

#define AIN2 13

#define AIN3 14

#define AIN4 15

void RemoteXY_Receive() {

  RemoteXY_Handler ();  

   int y_pwm = RemoteXY.joystick_1_y; //go back

  if( y_pwm >= 50){

    Run_Speed_K = map(y_pwm, 50, 100, 0, 30);//y_pwm 50~100 →　0~30

    Run_Speed_K = Run_Speed_K / gear_ratio; 

    Run_Speed += Run_Speed_K;

  }

  else if( y_pwm <= -50)  {

    Run_Speed_K = map(y_pwm, -50,-100, 0, -30);//y_pwm　-50~-100 → 0~-30

    Run_Speed_K = Run_Speed_K / gear_ratio; 

    Run_Speed += Run_Speed_K;

  }

  else {

    Run_Speed_K = 0;

  }

  

  int x_pwm = RemoteXY.joystick_1_x; //turn left & right

  if( x_pwm >= 50){

    Turn_Speed = map(x_pwm, 50, 100, 0, 30);//x_pwm 50~100 →　0~30

    }

  else if( x_pwm <= -50)  {

    Turn_Speed = map(x_pwm, -50, -100, 0, -30);//x_pwm　-50~-100 → 0~-30

    }

  else {

    Turn_Speed = 0;

  }

}

void setup() {  

  RemoteXY_Init ();  

  Wire.begin();

  accelgyro.initialize();   

  pinMode(AIN1,OUTPUT);

  pinMode(AIN2,OUTPUT); 

  pinMode(AIN3,OUTPUT);

  pinMode(AIN4,OUTPUT);  

  delay(100);

}

void loop() {  

  RemoteXY_Receive();

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);// 各軸加速度と角速度を取得

  // r_angle = (atan2(ay, az) * 180 / pi + Angle_offset); //Roll

  // omega =  Gyr_Gain * (gx +  Gry_offset);

  r_angle = (atan(-ax/ sqrt(ay * ay + az * az)) * 180 / pi + Angle_offset); //Pitch

  omega =  Gyr_Gain * (gy +  Gry_offset);

  if (abs(r_angle) < Driving_Angle) {

    myPID();

    PWMControl();

  }

  else {

    analogWrite(AIN1, 0);

    analogWrite(AIN2, 0);

    analogWrite(AIN3, 0);

    analogWrite(AIN4, 0);    

  } 

}

void myPID() {

//PID係数

  kp = 20; 

  ki = 0.20;  

  kd = 500.0;  

// Filters datas to get the real gesture

//Calculating the output values using the PID values.

  unsigned long now = millis();

  float dt = (now - preTime) / 1000.0;

  preTime = now;

  float K = 0.8;

  float A = K / (K + dt);

  f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle;

  timeChange = (now - lastTime);

  if (timeChange >= SampleTime) {

    Input = f_angle;

    error = Input;

    errSum += error * timeChange;

    dErr = (error - lastErr) / timeChange;

    Output = kp * error + ki * errSum + kd * dErr;

    LOutput = Output + Run_Speed + Turn_Speed;

    ROutput = Output + Run_Speed - Turn_Speed;

    lastErr = error;

    lastTime = now;

    Serial.print(LOutput);

    Serial.print("  ,  ");

    Serial.println(ROutput);

  }

}

void PWMControl() {                                                     //小さい方をLrpmに返す

  float Lrpm = (min((float)900, abs(LOutput * 4) + LMotor_offset))*1.5; //モーターバランス

  float Rrpm = min((float)900, abs(ROutput * 4) + RMotor_offset);   

  if (LOutput > 0) {

    analogWrite(AIN1, Lrpm);

    analogWrite(AIN2, 0 );

  }

  else if (LOutput < 0) {

    analogWrite(AIN2,  Lrpm);

    analogWrite(AIN1, 0 );

  }

  else {

    analogWrite(AIN1, 1 );

    analogWrite(AIN2, 1 );

  }

  if (ROutput > 0) {

    analogWrite(AIN3, Rrpm);

    analogWrite(AIN4, 0 );

  }

  else if (ROutput < 0) {

    analogWrite(AIN4, Rrpm);

    analogWrite(AIN3, 0 );

  }

  else {

    analogWrite(AIN3, 1 );

    analogWrite(AIN4, 1 );

  }     

    Serial.print(Lrpm);

    Serial.print("  ,  ");

    Serial.println(Rrpm);

    

}