/*このスケッチを丸ごとコピーするだけでは再現できません。
特に倒立は、フレームの形状や使用部品及び部品配置によってバランスが異なり、
安定させるには機体に合わせた設定値の変更が必要です。
------------------------------------------------------------------------------------*/
#include "ESP8266WiFi.h"
#include "WiFiUDP.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
WiFiUDP UDP;
char recbuff[2];
IPAddress myIP(192, 168, 4, 1);
IPAddress HOSTIP (192, 168, 4, 2);
const char *ssid = "WiFi_BR";
const char *password = "123456789";
int Val_x, Val_y ;
int rx, ry;
MPU6050 accelgyro;
int16_t ax, ay, az;
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 -1.0 //* Adjustment required
#define RMotor_offset 0 //* Adjustment required
#define LMotor_offset 0 //* Adjustment required
#define pi 3.14159
#define min(X, Y) (((X)<(Y))?(X):(Y))
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;
int timeChange;
int IN1 = 14;
int IN2 = 13;
int ENA = 12;
int IN3 = 2;
int IN4 = 16;
int ENB = 15;
int CONNECT_LED = 0;
void setup() {
Wire.begin();
accelgyro.initialize();
WiFi.softAP(ssid, password);
WiFi.config(myIP, WiFi.gatewayIP(), WiFi.subnetMask());
UDP.begin(8000); // setting UDP port
Serial.begin(115200);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(CONNECT_LED, OUTPUT);
delay(100);
}
void WiFi_Receive() {
if (UDP.parsePacket() > 0) {
UDP.read(recbuff, 2);
UDP.flush();
}
int buff;
buff = recbuff[1];
//Serial.print("recbuff: "); Serial.println(buff);
switch (recbuff[0]) {
case 'X':
rx = buff;
break;
case 'Y':
ry = buff;
break;
default:
rx = ry = 130;
}
//Serial.print("rx: "); Serial.print(rx);
//Serial.print("ry: "); Serial.println(ry);
if (ry >= 145) {
Run_Speed_K = map(ry, 145, 255, 0, 100);
Run_Speed_K = Run_Speed_K / 20; //* 20 Adjustment required
Run_Speed = Run_Speed + Run_Speed_K;
}
else if (ry <= 110) {
Run_Speed_K = map(ry, 110, 2, 0, -100);
Run_Speed_K = Run_Speed_K / 20; //* 20 Adjustment required
Run_Speed = Run_Speed + Run_Speed_K;
}
else {
Run_Speed_K = 0;
}
if (rx >= 145) {
Turn_Speed = map(rx, 145, 255, 0, 20); //* 20 Adjustment required
}
else if (rx <= 110) {
Turn_Speed = map(rx, 110, 2, 0, -20); //* 20 Adjustment required
}
else {
Turn_Speed = 0;
}
}
void loop() {
WiFi_Receive();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ay, az) * 180 / pi + Angle_offset);
omega = Gyr_Gain * (gx + Gry_offset);
if (abs(r_angle) < 45) {
myPID();
PWMControl();
}
else {
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
if (WiFi.status() != WL_CONNECTED) {
digitalWrite(0, HIGH); // WiFi CONNECT_LED OFF
} else {
digitalWrite(0, LOW ); // WiFi CONNECT_LED ON
}
}
void myPID() {
kp = 30; //* Adjustment required
ki = 0.15; //* Adjustment required
kd = 900.0; //* Adjustment required
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;
}
}
void PWMControl() {
if (LOutput > 0) {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
}
else if (LOutput < 0) {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
}
else {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, HIGH);
}
if (ROutput > 0) {
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
else if (ROutput < 0) {
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
else {
digitalWrite(IN3, HIGH);
digitalWrite(IN4, HIGH);
}
analogWrite(ENA, min(800, abs(LOutput * 4) + LMotor_offset)); //max1023
analogWrite(ENB, min(800, abs(ROutput * 4) + RMotor_offset)); //max1023
}
#include "ESP8266WiFi.h"
#include "WiFiUDP.h"
#include "OSCMessage.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
IPAddress myIP(192, 168, 4, 1);
IPAddress HOSTIP (192, 168, 4, 2);
const char *ssid = "WiFi_TJ";
const char *password = "123456789";
int localPort = 8000;
WiFiUDP Udp;
MPU6050 accelgyro;
int16_t ax, ay, az;
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 -1.0 //* Adjustment required
#define RMotor_offset 0 //* Adjustment required
#define LMotor_offset 0 //* Adjustment required
#define pi 3.14159
#define min(X, Y) (((X)<(Y))?(X):(Y))
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;
int timeChange;
int IN1 = 14;
int IN2 = 13;
int ENA = 12;
int IN3 = 2;
int IN4 = 16;
int ENB = 15;
int CONNECT_LED = 0;
void setup() {
Wire.begin();
accelgyro.initialize();
WiFi.softAP(ssid, password);
IPAddress myIP = WiFi.softAPIP();
Serial.print("AP IP address: "); Serial.println(myIP);
Serial.println("Starting udp");
Udp.begin(localPort);
Serial.print("Local port: "); Serial.println(Udp.localPort());
Serial.begin(115200);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(CONNECT_LED, OUTPUT);
delay(100);
}
void OSCMsgReceive() {
OSCMessage msgIN;
int size;
if ((size = Udp.parsePacket()) > 0) {
while (size--)
msgIN.fill(Udp.read());
if (!msgIN.hasError()) {
msgIN.route("/1/xy1", xy_pad);
}
}
}
void xy_pad(OSCMessage &msg, int addrOffset) {
int rx = msg.getFloat(0);
int ry = msg.getFloat(1);
//Serial.print("rx: "); Serial.print(rx);
//Serial.print("ry: "); Serial.println(ry);
if (ry >= 145) {
Run_Speed_K = map(ry, 145, 255, 0, 100);
Run_Speed_K = Run_Speed_K / 20; //20 * Adjustment required
Run_Speed = Run_Speed + Run_Speed_K;
}
else if (ry <= 110) {
Run_Speed_K = map(ry, 110, 0, 0, -100);
Run_Speed_K = Run_Speed_K / 20; //20 * Adjustment required
Run_Speed = Run_Speed + Run_Speed_K;
}
else {
Run_Speed_K = 0;
}
if (rx >= 145) {
Turn_Speed = map(rx, 145, 255, 0, 20);//20 * Adjustment required
}
else if (rx <= 110) {
Turn_Speed = map(rx, 110, 0, 0, -20);//-20 * Adjustment required
}
else {
Turn_Speed = 0;
}
}
void loop() {
OSCMsgReceive();
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
r_angle = (atan2(ay, az) * 180 / pi + Angle_offset);
omega = Gyr_Gain * (gx + Gry_offset);
if (abs(r_angle) < 45) {
myPID();
PWMControl();
}
else {
analogWrite(ENA, 0);
analogWrite(ENB, 0);
}
if (WiFi.status() != WL_CONNECTED) {
digitalWrite(0, HIGH); // WiFi CONNECT_LED OFF
} else {
digitalWrite(0, LOW ); // WiFi CONNECT_LED ON
}
}
void myPID() {
kp = 30; //* Adjustment required
ki = 0.15; //* Adjustment required
kd = 900.0; //* Adjustment required
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;
}
}
void PWMControl() {
if (LOutput > 0) {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
}
else if (LOutput < 0) {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
}
else {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, HIGH);
}
if (ROutput > 0) {
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
else if (ROutput < 0) {
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
else {
digitalWrite(IN3, HIGH);
digitalWrite(IN4, HIGH);
}
analogWrite(ENA, min(800, abs(LOutput * 4) + LMotor_offset)); //max1023
analogWrite(ENB, min(800, abs(ROutput * 4) + RMotor_offset)); //max1023
}
|