操舵プログラムメモ

#include <Arduino.h>
#include <WiFi.h>
#include <EEPROM.h>
#include <esp_now.h>
#include <ESP32-TWAI-CAN.hpp>
#include <ESP32Servo.h>

#define CAN_TX 6
#define CAN_RX 7

const int servo1Pin = 3;
const int servo2Pin = 5;

CanFrame rxFrame;

static const float RUDDER_RANGE = 60.0;   // ラダーの可動域（弧度法）
static const float ELEVATOR_RANGE = 60.0; // エレベーターの可動域

static uint8_t rudder_rotation = 0;
static uint8_t elevator_rotation = 0;

static float rudder_default = 0.0;
static float elevator_default = 0.0;

Servo servo1;
Servo servo2;

const int minUs = 500;
const int maxUs = 2500;

struct control_packet
{
  float rudder_default = 0.0;
  float elevator_default = 0.0;
  int rudder_analog = 2048;
  int elevator_analog = 2048;
  int control_flag = 0;
} Device;

uint8_t Address[] = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF};

// 受信コールバック
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len)
{
  if (memcmp(mac_addr, Address, 6) == 0)
  {
    memcpy(&Device, data, sizeof(control_packet));
    rudder_default = Device.rudder_default;
    elevator_default = Device.elevator_default;
    EEPROM.put<float>(0 * sizeof(float), rudder_default);
    EEPROM.put<float>(1 * sizeof(float), elevator_default);
    EEPROM.commit();
  }
}

static const int NEUTRAL_RANGE = 256;
static const int JOYSTICK_NEUTRAL = 2048;
static const double RANGE_CENTER = (4096.0 - NEUTRAL_RANGE) / 2;
float ToRotation(int x, float range)
{
  // 閾値以内であればジョイスティックは中央にあるものとみなす
  if (x >= JOYSTICK_NEUTRAL - NEUTRAL_RANGE / 2 && x <= JOYSTICK_NEUTRAL + NEUTRAL_RANGE / 2)
  {
    x = RANGE_CENTER;
  }
  // ニュートラルの左端と右端を接続して線形に変換する
  if (x > JOYSTICK_NEUTRAL)
  {
    x -= NEUTRAL_RANGE;
  }
  return (x - RANGE_CENTER) / RANGE_CENTER * (range / 2.0);
}

void setup()
{
  Serial.begin(115200);

  EEPROM.begin(1024);
  EEPROM.get<float>(0 * sizeof(float), rudder_default);
  EEPROM.get<float>(1 * sizeof(float), elevator_default);

  // ESP-NOW初期化
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK)
  {
    Serial.println("ESPNow Init Success");
  }

  esp_now_register_recv_cb(OnDataRecv);

  servo1.setPeriodHertz(50); // Standard 50hz servo
  servo2.setPeriodHertz(50); // Standard 50hz servo
  servo1.attach(servo1Pin, minUs, maxUs);
  servo2.attach(servo2Pin, minUs, maxUs);

  ESP32Can.begin(ESP32Can.convertSpeed(1), CAN_TX, CAN_RX, 10, 10);
}

void loop()
{
  static int rudder_analog = 0;
  static int elevator_analog = 0;

  if (ESP32Can.readFrame(rxFrame, 10))
  {
    if (rxFrame.identifier == 0x7E8 && rxFrame.data_length_code == 8)
    {
      rudder_analog = (rxFrame.data[0] << 8) | rxFrame.data[1];
      elevator_analog = (rxFrame.data[4] << 8) | rxFrame.data[5];
      Serial.println(String("Rudder(Analog): ") + rudder_analog);
      Serial.println(String("Elevator(Analog): ") + elevator_analog);
    }
  }

  // Override control signal
  if (Device.control_flag == 1)
  {
    rudder_analog = Device.rudder_analog;
    elevator_analog = Device.elevator_analog;
  }

  servo1.write(ToRotation(rudder_analog, RUDDER_RANGE) + 90.0);
  servo2.write(ToRotation(elevator_analog, ELEVATOR_RANGE) + 90.0);
}

Koki Mizumoto

#include <Arduino.h>
#include <ESP32-TWAI-CAN.hpp>

#define CAN_TX 6
#define CAN_RX 7

const int PIN[4] = {2, 3, 4, 5};

void setup()
{
  Serial.begin(115200);
  for (int i = 0; i < 4; i++)
  {
    pinMode(PIN[i], ANALOG);
  }

  ESP32Can.begin(ESP32Can.convertSpeed(1), CAN_TX, CAN_RX, 10, 10);
}

void loop()
{
  CanFrame obdFrame = {0};
  obdFrame.identifier = 0x7E8; // ID設定
  obdFrame.extd = 0;
  obdFrame.data_length_code = 8;
  for (int i = 0; i < 4; i++)
  {
    uint16_t data = analogRead(PIN[i]);
    obdFrame.data[i * 2] = (data >> 8);
    obdFrame.data[i * 2 + 1] = (data & 0xff);
  }
  ESP32Can.writeFrame(obdFrame); // timeout defaults to 1 ms

  delay(10);
}

Koki Mizumoto

#include <Arduino.h>
#include <EEPROM.h>
#include <esp_now.h>
#include <WiFi.h>

const int FLAG_PIN = 2;
const int BUTTON_PIN[4] = {5, 6, 4, 7};
const int JOYSTICK_PIN[2] = {0, 1};

static int ButtonPushTime[4] = {0, 0, 0, 0};
static int ButtonPushCount[4] = {0, 0, 0, 0};

struct control_packet
{
  float rudder_default = 0.0;
  float elevator_default = 0.0;
  int rudder_analog = 2048;
  int elevator_analog = 2048;
  int control_flag = 0;
} Device;

uint8_t broadcastAddress[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
esp_now_peer_info_t peerInfo;

// 送信コールバック
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02X:%02X:%02X:%02X:%02X:%02X",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.print("Last Packet Sent to: ");
  Serial.println(macStr);
  Serial.print("Last Packet Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}

void setup() {
  Serial.begin(115200);
  pinMode(FLAG_PIN, INPUT_PULLUP);
  for (int i = 0; i < 4; i++)
  {
    pinMode(BUTTON_PIN[i], INPUT_PULLUP);
  }
  for (int i = 0; i < 2; i++)
  {
    pinMode(JOYSTICK_PIN[i], ANALOG);
  }

  EEPROM.begin(1024);
  EEPROM.get<float>(0 * sizeof(float), Device.rudder_default);
  EEPROM.get<float>(1 * sizeof(float), Device.elevator_default);

  // ESP-NOW初期化
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESP-NOW Init Success");
  }

  // マルチキャスト用Slave登録
  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
  peerInfo.channel = 0;  
  peerInfo.encrypt = false;
  esp_err_t addStatus = esp_now_add_peer(&peerInfo);
  if (addStatus == ESP_OK) {
    // Pair success
    Serial.println("Pair success");
  }

  // ESP-NOWコールバック登録
  esp_now_register_send_cb(OnDataSent);
}

void loop() {
  if (digitalRead(FLAG_PIN) == LOW) {
    Device.control_flag = 0;
  } else {
    Device.control_flag = 1;
  }
  for (int i = 0; i < 4; i++) {
    if (digitalRead(BUTTON_PIN[i]) == LOW) {
      // 押したときそのもの
      if (ButtonPushTime[i] == 0) {
        ButtonPushTime[i] = millis();
        ButtonPushCount[i]++;
      }
      // 1秒以上連続かつその後100ミリ秒おきに
      if (millis() - ButtonPushTime[i] > 1000) {
        ButtonPushTime[i] += 100;
        ButtonPushCount[i]++;
      }
    }
    else
    {
      // 押し始めてから50ミリ秒以内は無視
      if (millis() - ButtonPushTime[i] > 50) {
        ButtonPushTime[i] = 0;
      }
    }
  }
  if (ButtonPushCount[0] - ButtonPushCount[1] != 0) {
    Device.rudder_default += (ButtonPushCount[0] - ButtonPushCount[1]) / 10.0;
    ButtonPushCount[0] = ButtonPushCount[1] = 0;
    EEPROM.put<float>(0 * sizeof(float), Device.rudder_default);
    EEPROM.commit();
  }
  if (ButtonPushCount[2] - ButtonPushCount[3] != 0) {
    Device.elevator_default += (ButtonPushCount[2] - ButtonPushCount[3]) / 10.0;
    ButtonPushCount[2] = ButtonPushCount[3] = 0;
    EEPROM.put<float>(1 * sizeof(float), Device.elevator_default);
    EEPROM.commit();
  }
  Device.rudder_analog = analogRead(JOYSTICK_PIN[0]);
  Device.elevator_analog = analogRead(JOYSTICK_PIN[1]);

  esp_err_t result = esp_now_send(peerInfo.peer_addr, (uint8_t*)&Device, sizeof(control_packet));
}

Koki Mizumoto

lib_deps = 
	madhephaestus/ESP32Servo@^3.0.5
	handmade0octopus/ESP32-TWAI-CAN@^1.0.1