main.ino

#include <ArduinoSTL.h>
#include <SPI.h>
#include <Servo.h>
#include <bitset>
#include <iostream>
#include <iterator>
#include <stdlib.h>
#include <vector>
byte data_head = 0x00;

const int SERVO1 = 7;
const int SERVO2 = 8;

const int PWM_MIN = 500;
const int PWM_MAX = 2400;

std::vector<byte> data_digital{0x00};
std::vector<byte> data_analog{0x00};
unsigned char getData = 0;
auto data_digital_it = data_digital.end() - 1;
auto data_analog_it = data_analog.end() - 1;

Servo sv1;
Servo sv2;

ISR(SPI_STC_vect) {
  getData = SPDR;
  // std::cout << static_cast<int>(getData) << std::endl;
  switch (data_head) {
  case 0x80: {
    if (std::distance(data_digital_it + 1, data_digital.end()) >= 2) {
      data_head = 0x00;
      if (((0x80 + *(data_digital_it + 1) + *(data_digital_it + 2)) & 0x3f) ==
          SPDR) {
        data_digital_it = data_digital.end() - 1;
      } else {
        data_digital.erase(data_digital_it + 1, data_digital.end());
      }
    } else {
      data_digital.push_back(static_cast<byte>(SPDR));
    }
    break;
  }

  case 0xC0: {
    if (std::distance(data_analog_it + 1, data_analog.end()) >= 6) {
      data_head = 0x00;
      if (((0x80 + *(data_analog_it + 1) + *(data_analog_it + 2) +
            *(data_analog_it + 3) + *(data_analog_it + 4) +
            *(data_analog_it + 5) + *(data_analog_it + 6)) &
           0x3f) == SPDR) {
        // Serial.write("success\n");
        data_analog_it = data_analog.end() - 1;
      } else {
        // Serial.write("reject\n");
        data_analog.erase(data_analog_it + 1, data_analog.end());
      }
    } else {
      data_analog.push_back(static_cast<byte>(SPDR));
    }
    break;
  }
  default:
    data_head = static_cast<byte>(SPDR);
    break;
  }
  SPDR = getData;
}

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

  pinMode(MISO, OUTPUT);
  // speed:16MHz/16=1MHz
  SPI.setClockDivider(SPI_CLOCK_DIV16);
  // LSBFIRST / MSBFIRST
  SPI.setBitOrder(MSBFIRST);
  //立ち上がりでラッチ
  SPI.setDataMode(SPI_MODE1);
  // turn on SPI in slave mode
  SPCR |= _BV(SPE);
  // now turn on interrupts
  SPI.attachInterrupt();

  sv1.attach(SERVO1, PWM_MIN, PWM_MAX);
  sv2.attach(SERVO2, PWM_MIN, PWM_MAX);
}

void loop() {
    // sv1.write(90);
    // sv2.write(90);
  if (data_analog.begin() != data_analog_it) {

        std::cout << static_cast<int>(*(data_analog.begin() + 2)) << " : "
         << static_cast<int>(*(data_analog.begin() + 3)) << ":"<<data_analog.size()<<std::endl;
    // sv1.write(static_cast<int>((static_cast<double>(*(data_analog.begin() + 2)) / 255.0) *
    //           180.0));
    // sv2.write(static_cast<int>((static_cast<double>(*(data_analog.begin() + 3)) / 255.0) *
    //           180.0));
    sv1.write(static_cast<int>(*(data_analog.begin() + 2)));
    sv2.write(static_cast<int>(*(data_analog.begin() + 3)));
    data_analog.erase(data_analog.begin(), data_analog_it);
  }
}