raspberry pi pico RC car servo
import time
import network
import socket
from machine import Pin, PWM
pwm_f_r = PWM(Pin(15), freq=50)
pwm_f_l = PWM(Pin(16), freq=50)
pwm_b_r = PWM(Pin(10), freq=50)
pwm_b_l = PWM(Pin(21), freq=50)
#stop
duty_r_stop = int(65535 * 0)
duty_l_stop = int(65535 * 0)
#go
duty_r_go = int(65535 * 0.05)
duty_l_go = int(65535 * 0.10)
#back
duty_r_back = int(65535 * 0.10)
duty_l_back = int(65535 * 0.05)
#right
duty_r_right = int(65535 * 0.10)
duty_l_right = int(65535 * 0.10)
#left
duty_r_left = int(65535 * 0.05)
duty_l_left = int(65535 * 0.05)
def connect_to_wifi(ssid, password):
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
wlan.connect(ssid, password)
max_wait = 20
while max_wait > 0:
if wlan.status() < 0 or wlan.status() >= 3:
break
max_wait -= 1
print('wait connecting...')
time.sleep(1)
if wlan.status() != 3:
raise RuntimeError('fail connected')
else:
print('finish connected')
status = wlan.ifconfig()
print('IPaddress = ' + status[0])
# your router Wi-Fi and SSID
ssid = "your_ssid"
password = "your_pass"
# Wi-Fi connect
connect_to_wifi(ssid, password)
# HTML page
html = """
<!DOCTYPE html>
<html>
<head>
<meta name="viewport" content="width=device-width, initial-scale=1">
<style>
html {
font-family: Helvetica;
text-align: center;
}
button {
color: white;
padding: 15px 32px;
font-size: 16px;
margin: 4px 2px;
cursor: pointer;
border: none;
border-radius: 15px;
}
.blue { background-color: #364ff4; }
.red { background-color: #ff2f00; }
button, form { display: inline-block; text-align: center; }
</style>
</head>
<body>
<h1>Raspberry Pi Pico W RC</h1>
<form>
<button class="blue" name="pwm" value="left" type="submit">LEFT</button>
<button class="blue" name="pwm" value="go" type="submit">GO</button>
<button class="blue" name="pwm" value="back" type="submit">BACK</button>
<button class="blue" name="pwm" value="right" type="submit">RIGHT</button>
<button class="red" name="pwm" value="stop" type="submit">STOP</button>
</form>
<p>%s</p>
</body>
</html>
"""
# Initialize pwm state
pwmState = "PWM State Unknown"
# Start configuring socket
addr = socket.getaddrinfo("0.0.0.0", 80)[0][-1]
s = socket.socket()
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(addr)
s.listen(1)
print("listening on", addr)
# wait for connection from client
while True:
try:
# Accept connections from clients
cl, addr = s.accept()
print("client connected from", addr)
# Receive requests from clients
request = cl.recv(1024)
request = str(request)
# find 'pwm=on' and 'pwm=off' positions from request
pwm_stop = request.find("pwm=stop")
pwm_left = request.find("pwm=left")
pwm_go = request.find("pwm=go")
pwm_back = request.find("pwm=back")
pwm_right = request.find("pwm=right")
# Control pwm by position of pwm=left' or pwm=go' or 'pwm=right' string
if pwm_stop == 8:
print("pwm stop")
pwm_f_r.duty_u16(duty_r_stop)
pwm_b_r.duty_u16(duty_r_stop)
pwm_f_l.duty_u16(duty_l_stop)
pwm_b_l.duty_u16(duty_l_stop)
if pwm_go == 8:
print("pwm go")
pwm_f_r.duty_u16(duty_r_go)
pwm_b_r.duty_u16(duty_r_go)
pwm_f_l.duty_u16(duty_l_go)
pwm_b_l.duty_u16(duty_l_go)
if pwm_back == 8:
print("pwm back")
pwm_f_r.duty_u16(duty_r_back)
pwm_b_r.duty_u16(duty_r_back)
pwm_f_l.duty_u16(duty_l_back)
pwm_b_l.duty_u16(duty_l_back)
if pwm_left == 8:
print("pwm left")
pwm_f_r.duty_u16(duty_r_left)
pwm_b_r.duty_u16(duty_r_left)
pwm_f_l.duty_u16(duty_l_left)
pwm_b_l.duty_u16(duty_l_left)
if pwm_right == 8:
print("pwm right")
pwm_f_r.duty_u16(duty_r_right)
pwm_b_r.duty_u16(duty_r_right)
pwm_f_l.duty_u16(duty_l_right)
pwm_b_l.duty_u16(duty_l_right)
# Create a response and send it to the client
response = "HTTP/1.0 200 OK\r\nContent-type: text/html\r\n\r\n" + (html % pwmState)
cl.send(response)
cl.close()
# Close the connection with the client if an error occurs
except OSError as e:
cl.close()
print("connection closed")
Raspberry pi pico WiFi PWM
import time
import network
import socket
from machine import Pin, PWM
pwm = PWM(Pin(16), freq=500)
duty = 0
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
wlan.connect(ssid, password)
max_wait = 10
while max_wait > 0:
if wlan.status() < 0 or wlan.status() >= 3:
break
max_wait -= 1
print('wait connecting...')
time.sleep(1)
if wlan.status() != 3:
raise RuntimeError('fail connected')
else:
print('finish connected')
status = wlan.ifconfig()
print('IPaddress = ' + status[0])
password = "your password"
# Wi-Fi connect
# HTML page
html = """
<!DOCTYPE html>
<html>
<head>
<meta name="viewport" content="width=device-width, initial-scale=1">
<style>
html {
font-family: Helvetica;
text-align: center;
}
button {
color: white;
padding: 15px 32px;
font-size: 16px;
margin: 4px 2px;
cursor: pointer;
border: none;
border-radius: 15px;
}
.green { background-color: #f44336; }
.red { background-color: #00BFFF; }
button, form { display: inline-block; text-align: center; }
</style>
</head>
<body>
<h1>Raspberry Pi Pico W</h1>
<form>
<button class="green" name="pwm" value="on" type="submit">ON</button>
<button class="red" name="pwm" value="off" type="submit">OFF</button>
</form>
<p>%s</p>
</body>
</html>
"""
# Initialize pwm state
pwmState = "PWM State Unknown"
# Start configuring socket
addr = socket.getaddrinfo("0.0.0.0", 80)[0][-1]
s = socket.socket()
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind(addr)
s.listen(1)
print("listening on", addr)
# wait for connection from client
while True:
try:
# Accept connections from clients
cl, addr = s.accept()
print("client connected from", addr)
# Receive requests from clients
request = cl.recv(1024)
request = str(request)
# find 'pwm=on' and 'pwm=off' positions from request
pwm_on = request.find("pwm=on")
pwm_off = request.find("pwm=off")
# Control pwm by position of pwm=on' or 'pwm=off' string
if pwm_on == 8:
print("pwm on")
duty = int((65535 * 50) / 100)
pwm.duty_u16(duty) # pwm > 0
if pwm_off == 8:
print("pwm off")
duty = 0
pwm.duty_u16(duty) # pwm = 0
# Set the current state of PWM
pwmState = "State: OFF" if duty == 0 else "State: ON"
# Create a response and send it to the client
response = "HTTP/1.0 200 OK\r\nContent-type: text/html\r\n\r\n" + (html % pwmState)
cl.send(response)
cl.close()
# Close the connection with the client if an error occurs
except OSError as e:
cl.close()
print("connection closed")
blink.cpp_src_pico
/** * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. * * SPDX-License-Identifier: BSD-3-Clause */ #include "pico/stdlib.h" int main() { #ifndef PICO_DEFAULT_LED_PIN #warning blink example requires a board with a regular LED #else const uint LED_PIN = PICO_DEFAULT_LED_PIN; gpio_init(LED_PIN); gpio_set_dir(LED_PIN, GPIO_OUT); while (true) { gpio_put(LED_PIN, 1); sleep_ms(250); gpio_put(LED_PIN, 0); sleep_ms(250); } #endif
Motion Detect with Gyro sensor
#define SV_Motor_x_PIN 32 #define SV_Motor_y_PIN 26 #define SV_Motor_z_PIN 14 #include<math.h> #define Deg2rad 3.1415 /180 #include <MadgwickAHRS.h> //MPU9250 #include <Wire.h> const int MPU_addr=0x68; // I2C address of the MPU-6050 int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ; //Actual unitization scale factor of acquired data float SF_Acc = 16384; //acceleration scale factor (digit/g) float SF_Gy = 131; //Gyro scale factor (digit/dps) float SF_Mg = 500; //magnetic scale factor(500☛0.6かも。。) float SF_Tmp = 333.87; //temperature scale factor float g2mpss = 9.80665; //conversion from G to m/s2 float deg2rad = 3.14159265/180; //from degree to radian Madgwick MadgwickFilter; //servo motor define duty min and max float duty_min = 25.6; // at 0 deg float duty_max = 122.9; // at 180 deg float duty_active = duty_max - duty_min; float duty_mid = (duty_min + duty_max)/2; // at 90 deg ///タイマー割込み 参考https://55life555.blog.fc2.com/blog-entry-3194.html volatile int timeCounter1; hw_timer_t *timer1 = NULL; portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; void IRAM_ATTR onTimer1(){ portENTER_CRITICAL_ISR(&timerMux); timeCounter1++; portEXIT_CRITICAL_ISR(&timerMux); } void setup() { //SVmotor pinMode(SV_Motor_x_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT pinMode(SV_Motor_y_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT pinMode(SV_Motor_z_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT ledcSetup(0, 50, 10); ledcSetup(1, 50, 10); ledcSetup(2, 50, 10); ledcAttachPin(SV_Motor_x_PIN,0); ledcAttachPin(SV_Motor_y_PIN,1); ledcAttachPin(SV_Motor_z_PIN,2); ////////////////////////////Highly needed module Serial.begin(115200); //////////////////////////// //MPU9250 set up Wire.begin(); Wire.beginTransmission(MPU_addr); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); setupMPU9255(); //MadgwickFilter MadgwickFilter.begin(10);//filtering frequency 100Hz //タイマー割込み timer1 = timerBegin(0, 80, true); timerAttachInterrupt(timer1, &onTimer1, true); timerAlarmWrite(timer1, 100000, true); timerAlarmEnable(timer1); } //value after sensor value scaling float AcX_SF; float AcY_SF; float AcZ_SF; float GyX_SF; float GyY_SF; float GyZ_SF; //変数 struct strc{ float AXIS_X; float AXIS_Y; float AXIS_Z; }; float ROLL, PITCH, YAW; void setupMPU9255() { Wire.beginTransmission(0x68); Wire.write(0x6B); Wire.write(0x00); Wire.endTransmission(); Wire.beginTransmission(0x68); Wire.write(0x1A); Wire.write(0x05); Wire.endTransmission(); Wire.beginTransmission(0x68); Wire.write(0x37); Wire.write(0x02); Wire.endTransmission(); Wire.beginTransmission(0x0C); Wire.write(0x0A); Wire.write(0x16); Wire.endTransmission(); delay(500); } //MPU9250 setup void readCompass() { Wire.beginTransmission(0x0C); Wire.write(0x02); Wire.endTransmission(); Wire.requestFrom(0x0C, 1); uint8_t ST1 = Wire.read(); if (ST1 & 0x01) { Wire.beginTransmission(0x0C); Wire.write(0x03); Wire.endTransmission(); Wire.requestFrom(0x0C, 7); uint8_t i = 0; uint8_t buf[7]; while (Wire.available()) { buf[i++] = Wire.read(); } if (!(buf[6] & 0x08)) { MgX = ((int16_t)buf[1] << 8) | buf[0]; MgY = ((int16_t)buf[3] << 8) | buf[2]; MgZ = ((int16_t)buf[5] << 8) | buf[4]; } } } void loop() { unsigned long time;//「time」をunsigned longで変数宣言, declared "time"as variable time = millis();//プログラム実行からの経過時間(ms)をtimeに返す,Returns from program execution lapsed time (ms) to "time" if (timeCounter1 > 0) { portENTER_CRITICAL(&timerMux); timeCounter1--; portEXIT_CRITICAL(&timerMux); ////MPU9250関連//// //読み出し, read Wire.beginTransmission(MPU_addr); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU_addr,16,true); // request a total of 14 registers AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) readCompass(); //Acc/g->m/s^2,Gy/dps->rad/s MadgwickFilter.update(GyX/SF_Gy, GyY/SF_Gy, GyZ/SF_Gy, AcX/SF_Acc, AcY/SF_Acc, AcZ/SF_Acc, MgX/SF_Mg, MgY/SF_Mg, MgZ/SF_Mg); ROLL = MadgwickFilter.getRoll(); PITCH = MadgwickFilter.getPitch(); YAW = MadgwickFilter.getYaw(); //for serial plot Serial.print(ROLL); Serial.print(","); Serial.print(PITCH); Serial.print(","); Serial.print(YAW); Serial.print("\n"); //convert from angle of sensor for servo control ROLL = duty_mid + ROLL/180 * duty_active; ROLL = min(max(ROLL, duty_min), duty_max); PITCH = duty_mid + PITCH/180 * duty_active; PITCH = min(max(PITCH, duty_min), duty_max); YAW = duty_mid + YAW/180 * duty_active; YAW = min(max(YAW, duty_min), duty_max); ledcWrite(0, ROLL); ledcWrite(1, PITCH); ledcWrite(2, YAW); delay(10); }
DIY RC : Simple Structure Air Plane
#include "BluetoothSerial.h" #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it #endif #define Brushless_Motor_PIN 25 #define SV_Motor_PIN 32 int i = 0; BluetoothSerial SerialBT; void setup() { //Bluetooth Serial.begin(115200); SerialBT.begin("ESP32test"); //Bluetooth device name Serial.println("The device started, now you can pair it with bluetooth!"); //Brushlessmotor pinMode(Brushless_Motor_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT ledcSetup(0, 1000, 8); //PWM channel is 0, Frequency 1000Hz, 8 bit(Duty with 256 levels of resolution, the value is 0-255) ledcAttachPin(Brushless_Motor_PIN,0); //Assign channel 0 to Brushless_Motor_PIN //SVmotor pinMode(SV_Motor_PIN, OUTPUT); //Brushless_Motor_PIN as OUTPUT ledcSetup(10, 50, 10); ledcAttachPin(SV_Motor_PIN,10); } uint8_t calculate_checksum(uint8_t *data) { uint8_t checksum = 0; checksum |= 0b11000000 & data[1]; checksum |= 0b00110000 & data[2]; checksum |= 0b00001100 & data[3]; checksum |= 0b00000011 & data[4]; return checksum; } void loop() { uint8_t recv_data[6]; if (SerialBT.available()) { SerialBT.readBytes(recv_data, 6); if (recv_data[0] != 'T') { Serial.print("Receive error!"); return; } if (recv_data[5] != calculate_checksum(recv_data)) { Serial.print("Decode error!"); return; } Serial.printf("left_x: %d, left_y: %d, right_x: %d, right_y: %d\n", recv_data[1], recv_data[2], recv_data[3], recv_data[4]); if((recv_data[2] < 50)&&(i < 200)){//Duty MAX About 0.5(≒124/255) ledcWrite(0, i++); if(recv_data[3] > 200){//turn left ledcWrite(10, 110); } if(recv_data[3] < 50){//turn left ledcWrite(10, 50); } if((recv_data[3] >= 50)&&(recv_data[3] <= 200)){//turn standard position ledcWrite(10, 80); } } if((recv_data[2] > 200)&&(i >= 1)){ ledcWrite(0, i--); if(recv_data[3] > 200){//turn left ledcWrite(10, 110); } if(recv_data[3] < 50){//turn left ledcWrite(10, 50); } if((recv_data[3] >= 50)&&(recv_data[3] <= 200)){//turn standard position ledcWrite(10, 80); } } else{ if(recv_data[3] > 200){//turn left ledcWrite(10, 110); } if(recv_data[3] < 50){//turn left ledcWrite(10, 50); } if((recv_data[3] >= 50)&&(recv_data[3] <= 200)){//turn standard position ledcWrite(10, 80); } } Serial.printf("i:%d\n", i); } delay(20); }
controller side source code is same with below.
DIY Drone - ESP32 [Version 2] #4 (How to detect Drone angle) Source code
#include "BluetoothSerial.h" #include<math.h> #define Deg2rad 3.1415 /180 #include <MadgwickAHRS.h> //MPU9250関連 #include <Wire.h> const int MPU_addr=0x68; // I2C address of the MPU-6050 int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ; //取得データの実単位化スケールファクター float SF_Acc = 16384; //加速度のスケールファクタ (digit/g) float SF_Gy = 131; //#ジャイロのスケールファクタ (digit/dps) float SF_Mg = 500; //磁気のスケールファクタ(500☛0.6かも。。) float SF_Tmp = 333.87; //温度のスケールファクタ float g2mpss = 9.80665; //Gをm/s2に変換 float deg2rad = 3.14159265/180; //ディグリーをラジアンに int i; #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it #endif BluetoothSerial SerialBT; Madgwick MadgwickFilter; ///タイマー割込み 参考https://55life555.blog.fc2.com/blog-entry-3194.html volatile int timeCounter1; hw_timer_t *timer1 = NULL; portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; void IRAM_ATTR onTimer1(){ portENTER_CRITICAL_ISR(&timerMux); timeCounter1++; portEXIT_CRITICAL_ISR(&timerMux); } void setup() { //Bluetooth Serial.begin(115200); SerialBT.begin("ESP32test"); //Bluetooth device name Serial.println("The device started, now you can pair it with bluetooth!"); //Brushlessmotor // pinMode(Brushless_Motor_PIN, OUTPUT);//Brushless_Motor_PIN as OUTPUT // ledcSetup(0, 1000, 8);//PWM channel is 0, Frequency 1000Hz, 8 bit(Duty with 256 levels of resolution, the value is 0-255) // ledcAttachPin(Brushless_Motor_PIN,0);//Assign channel 0 to Brushless_Motor_PIN //MPU9250 set up Wire.begin(); Wire.beginTransmission(MPU_addr); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); setupMPU9255(); //MadgwickFilter MadgwickFilter.begin(10);//filtering frequency 100Hz //タイマー割込み timer1 = timerBegin(0, 80, true); timerAttachInterrupt(timer1, &onTimer1, true); timerAlarmWrite(timer1, 100000, true); timerAlarmEnable(timer1); } ///通信データの正しさの確認 uint8_t calculate_checksum(uint8_t *data) { uint8_t checksum = 0; checksum |= 0b11000000 & data[1]; checksum |= 0b00110000 & data[2]; checksum |= 0b00001100 & data[3]; checksum |= 0b00000011 & data[4]; return checksum; } //センサー値スケール化後数値 float AcX_SF; float AcY_SF; float AcZ_SF; float GyX_SF; float GyY_SF; float GyZ_SF; //変数 struct strc{ float AXIS_X; float AXIS_Y; float AXIS_Z; }; float ROLL, PITCH, YAW; void setupMPU9255() { Wire.beginTransmission(0x68); Wire.write(0x6B); Wire.write(0x00); Wire.endTransmission(); Wire.beginTransmission(0x68); Wire.write(0x1A); Wire.write(0x05); Wire.endTransmission(); Wire.beginTransmission(0x68); Wire.write(0x37); Wire.write(0x02); Wire.endTransmission(); Wire.beginTransmission(0x0C); Wire.write(0x0A); Wire.write(0x16); Wire.endTransmission(); delay(500); } //MPU9250セットアップ関連 void readCompass() { Wire.beginTransmission(0x0C); Wire.write(0x02); Wire.endTransmission(); Wire.requestFrom(0x0C, 1); uint8_t ST1 = Wire.read(); if (ST1 & 0x01) { Wire.beginTransmission(0x0C); Wire.write(0x03); Wire.endTransmission(); Wire.requestFrom(0x0C, 7); uint8_t i = 0; uint8_t buf[7]; while (Wire.available()) { buf[i++] = Wire.read(); } if (!(buf[6] & 0x08)) { MgX = ((int16_t)buf[1] << 8) | buf[0]; MgY = ((int16_t)buf[3] << 8) | buf[2]; MgZ = ((int16_t)buf[5] << 8) | buf[4]; } } } void loop() { unsigned long time;//「time」をunsigned longで変数宣言, declared "time"as variable time = millis();//プログラム実行からの経過時間(ms)をtimeに返す,Returns from program execution lapsed time (ms) to "time" if (timeCounter1 > 0) { portENTER_CRITICAL(&timerMux); timeCounter1--; portEXIT_CRITICAL(&timerMux); ////MPU9250関連//// //読み出し, read Wire.beginTransmission(MPU_addr); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU_addr,16,true); // request a total of 14 registers AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) readCompass(); //実単位化(Acc/g->m/s^2,Gy/dps->rad/s) MadgwickFilter.update(GyX/SF_Gy, GyY/SF_Gy, GyZ/SF_Gy, AcX/SF_Acc, AcY/SF_Acc, AcZ/SF_Acc, MgX/SF_Mg, MgY/SF_Mg, MgZ/SF_Mg); ROLL = MadgwickFilter.getRoll(); PITCH = MadgwickFilter.getPitch(); YAW = MadgwickFilter.getYaw(); //書き出し Serial.print(ROLL); Serial.print(","); Serial.print(PITCH); Serial.print(","); Serial.print(YAW); Serial.print("\n"); //Serial.print(AcZ/SF_Acc*g2mpss); delay(10); /////////////////////////////////////////// uint8_t recv_data[6]; if (SerialBT.available()) { SerialBT.readBytes(recv_data, 6); if (recv_data[0] != 'T') { Serial.print("Receive error!"); return; } if (recv_data[5] != calculate_checksum(recv_data)) { Serial.print("Decode error!"); return; } Serial.printf("left_x: %d, left_y: %d, right_x: %d, right_y: %d\n", recv_data[1], recv_data[2], recv_data[3], recv_data[4]); if((recv_data[2] < 50)&&(i < 200)){//Duty MAX About 0.5(≒124/255) ledcWrite(0, i++); } if((recv_data[2] > 200)&&(i >= 1)){ ledcWrite(0, i--); } Serial.printf("i:%d\n", i); } delay(20); } }
