Cylinder Fight

// #include <Arduino.h>
// #include <FS.h>
#include <M5AtomS3.h>
// #include <MPU6886.h>
// #include <SPIFFS.h>
// #include <M5Unified.h>
#include <atoms3joy.h>
#include <Toio.h>

Toio toio;

#define RESO10BIT (4096)
#define MAX_TOIOCORE_TARGET_NUM 29
ToioCoreTargetPos targetPos[MAX_TOIOCORE_TARGET_NUM];
#define MAX_TOIOCORECUBE_NUM 9
ToioCore *toiocore[MAX_TOIOCORECUBE_NUM];
size_t toiocore_num = 0;

String str;
int cmd[100], cmd2[3];
float pitch[10];
float old_pitch[10];
int sp = 0;
int dial = 0;
float aaa;
static unsigned long oootime, dddtime;
float yaw;
int steering[2];
int stop_flag[2];

// 初期化の外に置く
float sp2[2] = { 0, 0 };         // 各toioの現在のスピード
float target_sp2[2] = { 0, 0 };  // 各toioの目標スピード
float acceleration = 0.9;        //0.5;       // スムーズな変化のための係数（小さいほど滑らか）
int toio_x[2], toio_y[2], toio_angle[2];

int counter1 = 0, counter2 = 0, move = 0;
float Throttle;
float aileron, elevator, rudder;
uint16_t aileron_bias = 2048;
uint16_t elevator_bias = 2048;
uint16_t rudder_bias = 2048;
uint16_t Throttle_bias = 2048;
short xstick = 0;
short ystick = 0;
volatile uint8_t Loop_flag = 0;
float Timer = 0.0;
float dTime = 0.01;
uint8_t Timer_state = 0;
uint8_t StickMode = 2;
uint32_t espnow_version;
volatile uint8_t proactive_flag = 0;
unsigned long stime, etime, dtime = 0;
uint8_t axp_cnt = 0;
uint8_t disp_counter = 0;

// 周期カウンタ割り込み関数
hw_timer_t *timer = NULL;
void IRAM_ATTR onTimer() {
  Loop_flag = 1;
  // Timer = Timer + dTime;
}

void setup() {
  M5.begin();
  Serial2.begin(115200, SERIAL_8N1, 2, 1);
  Wire1.begin(38, 39, 400 * 1000);
  M5.update();
  M5.Lcd.setRotation(2);
  M5.Lcd.setTextFont(1);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setCursor(4, 2);

  M5.Lcd.fillScreen(BLACK);
  joy_update();

  THROTTLE = LEFTY;
  AILERON = RIGHTX;
  ELEVATOR = RIGHTY;
  RUDDER = LEFTX;
  ARM_BUTTON = LEFT_STICK_BUTTON;
  FLIP_BUTTON = RIGHT_STICK_BUTTON;
  MODE_BUTTON = RIGHT_BUTTON;
  OPTION_BUTTON = LEFT_BUTTON;

  byte error, address;
  int nDevices;

  ////////////////////////////////////////////////////////
  USBSerial.println("Scanning... Wire1");

  nDevices = 0;
  for(address = 1; address < 127; address++) {
    Wire1.beginTransmission(address);
    error = Wire1.endTransmission();

    if(error == 0) {
      USBSerial.print("I2C device found at address 0x");
      if(address < 16)
        USBSerial.print("0");
      USBSerial.print(address, HEX);
      USBSerial.println("  !");

      nDevices++;
    } else if(error == 4) {
      USBSerial.print("Unknown error at address 0x");
      if(address < 16)
        USBSerial.print("0");
      USBSerial.println(address, HEX);
    }
  }
  if(nDevices == 0)
    USBSerial.println("No I2C devices found\n");
  else
    USBSerial.println("done\n");

  // Scan Toio Core Cubes in 5 seconds.
  // 5 秒間 Toio Core Cube をスキャン
  USBSerial.printf("Scanning your toio core...\n");
  std::vector<ToioCore *> toiocore_list = toio.scan(3);

  // Exit if could not found any cubes.
  // 見つからなければ終了
  toiocore_num = toiocore_list.size();
  if (toiocore_num == 0) {
    USBSerial.printf("No toio Core Cube was found. Turn on your Toio Core Cube, then press the reset button of your Toio Core Cube.\n");
    return;
  }
  USBSerial.printf("found %u toio core cubes.\n", toiocore_num);

  for (uint i = 0; i < toiocore_num; i++) {
    // Assign the ToioCore object of the Toio Core Cubes found.
    // 見つかった Toio Core Cube の ToioCore オブジェクト
    toiocore[i] = toiocore_list.at(i);
    // Print the Toio Core cube's device name and MAC address.
    // Toio Core のデバイス名と MAC アドレスを表示
    USBSerial.printf("%u: %s (%s)\n", i, toiocore[i]->getName().c_str(), toiocore[i]->getAddress().c_str());
    targetPos[i] = { 0, 0, 0, 0 };

    // Start BLE connection.
    // BLE 接続開始
    USBSerial.printf("Connecting...\n");

    if (!toiocore[i]->connect()) {
      USBSerial.printf("%u: Failed to establish a BLE connection.\n", i);
      return;
    }
    USBSerial.printf("%u: Connected.\n", i);
    // Set Connection event callback.
    // Connection イベントのコールバックをセット
    USBSerial.printf("set cannection callback %u %p\n", i, toiocore[i]);
    toiocore[i]->onConnection([i](bool state) {
      USBSerial.printf("%u: Connection Event %s\n", i, state ? "Connected   " : "Disconnected");
    });

    // 設定変更の応答
    ToioCoreConfigurationResponse res;

    // コネクションインターバルの最小値、最大値をセット
    toiocore[i]->setConnectionInterval(8, 8);  // 8 = 10ms
    res = toiocore[i]->getConfigurationResponse();
    USBSerial.printf("setConnectionInterval response %02x %02x %02x\n", res.infoType, res.config.reserved, res.config.response);

    // Identification sensor ID notification settings
    // 読み取りセンサーの ID 通知設定
    toiocore[i]->setIDnotificationSettings(1, 0x00);  // 1 = 10ms
    res = toiocore[i]->getConfigurationResponse();
    USBSerial.printf("IDnotificationSettings response %02x %02x %02x\n", res.infoType, res.config.reserved, res.config.response);

    // 識別センサーID不在通知設定
    // 読み取りセンサーの ID missed 通知設定
    toiocore[i]->setIDmissedNotificationSettings(10);  // 10 ▶ 50
    res = toiocore[i]->getConfigurationResponse();
    USBSerial.printf(" IDmissedNotificationSettings 応答 %02x %02x %02x\n", res.infoType, res.config.reserved, res.config.response);

    // モーター制御の応答イベントのコールバックをセット
    // toiocore[i]->onMotor([i](ToioCoreMotorResponse motor_res) {});

    // Posture angle detection settings
    // 姿勢角検出の設定
    // toiocore->setPostureAngleDetectionSettings(1, NotifyChangesOnly, AngleTypeEuller); // 姿勢をオイラー角(int16)で検出
    //toiocore->setPostureAngleDetectionSettings(1, NotifyChangesOnly, AngleTypeQuaternion); // 姿勢を四元数(float)で検出
    toiocore[i]->setPostureAngleDetectionSettings(1, 0x00, AngleTypeHighPrecisionEuller);  // 姿勢を高精度オイラー角(float)で検出

    // Set ID notification event callback.
    // 読み取りセンサーのイベントのコールバックをセット
    toiocore[i]->onIDReaderData([i](ToioCoreIDData id_data) {
      // USBSerial.printf("IDReader Event ");
      if (id_data.type == ToioCoreIDTypePosition) {
        // toio_x[i] = id_data.position.cubePosX;
        // toio_y[i] = id_data.position.cubePosY;
        toio_angle[i] = id_data.position.cubeAngleDegree;
        stop_flag[i] = false;
      }else{
        stop_flag[i] = true;
      }
    });

    // Set motion event, magnetic sensor event, posture angle event callbacks.
    // Motion イベントと磁気センサーと姿勢角のコールバックをセット
    toiocore[i]->onMotion(
      nullptr,
      nullptr,
      [i](ToioCorePostureAngle angle) {
        // USBSerial.printf("Posture Angle High precision Euler(float32) Event roll=%f, pitch=%f, yaw=%f\n",
        //               angle.eulerf.roll, angle.eulerf.pitch, angle.eulerf.yaw);
        pitch[i] = angle.eulerf.pitch;
      });

    // Set indicator color.
    // LED色設定
    uint j = i + 1;
    toiocore[i]->turnOnLed((j & 0x01) ? 64 : 0, (j & 0x02) ? 64 : 0, (j & 0x04) ? 64 : 0);
  }

  // 割り込み設定
  timer = timerBegin(1, 80, true);
  timerAttachInterrupt(timer, &onTimer, true);
  timerAlarmWrite(timer, 5000, true); // 1000μs = 1ms
  timerAlarmEnable(timer);
  delay(100);
  stime = micros();

  stop_flag[0] = true;
  stop_flag[1] = true;
}

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void loop() {
  uint16_t _throttle; // = getThrottle();
  uint16_t _aileron;  // = getAileron();
  uint16_t _elevator; // = getElevator();
  uint16_t _rudder;   // = getRudder();

  while(Loop_flag == 0){
  }
  Loop_flag = 0;
  etime = stime;
  stime = micros();
  dtime = stime - etime;
  M5.update();
  joy_update();
  toio.loop();

  _throttle = getThrottle();
  _aileron = getAileron();
  _elevator = getElevator();
  _rudder = getRudder();

  // 量産版
  Throttle = -(float)(_throttle - Throttle_bias) / (float)(RESO10BIT * 0.5);
  aileron = (float)(_aileron - aileron_bias) / (float)(RESO10BIT * 0.5);
  elevator = (float)(_elevator - elevator_bias) / (float)(RESO10BIT * 0.5);
  rudder = (float)(_rudder - rudder_bias) / (float)(RESO10BIT * 0.5);

  Throttle = Throttle + 0.02;
  aileron = aileron - 0.04;
  elevator = elevator + 0.01;
  rudder = rudder - 0.05;

  //********************************* */

  int steering1 = 0, steering2 = 0;
  static int o_steering1, o_steering2;
  if(aileron > 0.2 || aileron < -0.2)
    steering1 = (int)(aileron * 5);
  if(rudder > 0.2 || rudder < -0.2)
    steering2 = (int)(rudder * 5);
  
  steering[0] = steering1;
  steering[1] = steering2;

  sp = 90;  // 100固定

  if(o_steering1 != steering1 || o_steering2 != steering2) {
    o_steering1 = steering1;
    o_steering2 = steering2;
  }

  for (int i = 0; i < 2; i++) {
    if (pitch[i] != old_pitch[i]) {
      if (stop_flag[i] == true) {
        toiocore[i]->controlMotor(1, 0, 1, 0);
      } else {
        int max_offset = 7;
        int min_offset = -max_offset;
        float speed_offset = mapFloat(pitch[i], 6.7 - 10, 6.7 + 10, min_offset, max_offset);
        if (speed_offset > max_offset) speed_offset = max_offset;
        if (speed_offset < min_offset) speed_offset = min_offset;
        yaw = steering[i];
        if (yaw == 0){
          if(toio_angle[i] > 89) yaw = 1;
          if(toio_angle[i] < 89) yaw = -1;
        }
        // モーター出力
        toiocore[i]->controlMotor(1, sp + speed_offset - yaw, 1, sp + speed_offset);
      }

      old_pitch[i] = pitch[i];
      if(i = 0) delay(5);
      dddtime = micros() - oootime;
      oootime = micros();
      // USBSerial.printf("%2.2f %d %f \n", dddtime / 1000.0, i, pitch[i]);
    }
  }
  // USBSerial.printf("%d %d\n", steering1, steering2);

  //********************************* */
  
  M5.Lcd.setCursor(0, 2 + 0 * 17);
  M5.Lcd.printf("%5.1f\n", dtime / 1000.0);
  M5.Lcd.printf("%5.1f %3d\n", pitch[0], toio_angle[0]);
  M5.Lcd.printf("%5.1f %3d\n", pitch[1], toio_angle[1]);
  
}