mega.cpp

#include <Arduino.h>
#include <BasicStepperDriver.h>

const int EAR_DIR = 48, EAR_STEP = 46, EAR_ENABLE = 62; // Z
const int EAR_02_DIR = 28, EAR_02_STEP = 26, EAR_02_ENABLE = 24; // E0
const int NOSE_UP_DIR = 34, NOSE_UP_STEP = 36, NOSE_UP_ENABLE = 30; // E1
const int NOSE_01_DIR = A7, NOSE_01_STEP = A6, NOSE_01_ENABLE = A2; // Y (left)
const int NOSE_02_DIR = A1, NOSE_02_STEP = A0, NOSE_02_ENABLE = 38; // X (right)

const int MOTOR_STEPS = 150;
const int RPM = 200;
const int MICROSTEPS = 5;

const int TRIGGER = 47;

BasicStepperDriver ear(MOTOR_STEPS, EAR_DIR, EAR_STEP, EAR_ENABLE);
BasicStepperDriver ear_02(MOTOR_STEPS, EAR_02_DIR, EAR_02_STEP, EAR_02_ENABLE);
BasicStepperDriver nose_up(MOTOR_STEPS, NOSE_UP_DIR, NOSE_UP_STEP, NOSE_UP_ENABLE);
BasicStepperDriver nose_01(MOTOR_STEPS, NOSE_01_DIR, NOSE_01_STEP, NOSE_01_ENABLE);
BasicStepperDriver nose_02(MOTOR_STEPS, NOSE_02_DIR, NOSE_02_STEP, NOSE_02_ENABLE);

void setup() {
  Serial.begin(115200);
  Serial2.begin(9600); // TX2 on D17
  
  ear.begin(RPM, MICROSTEPS);
  ear_02.begin(RPM, MICROSTEPS);
  nose_up.begin(150, MICROSTEPS);
  nose_01.begin(150, MICROSTEPS);
  nose_02.begin(150, MICROSTEPS);
}

void loop() {
  if (!Serial2.available() && !Serial.available()) return;
  if ((Serial2.available() && Serial2.read() != 's') || 
      (Serial.available() && Serial.read() != 's')) return;

  Serial.println("!\n");
  Serial2.println("!\n");

  nose_up.enable();
  nose_01.enable();
  nose_02.enable();
  ear.enable();
  ear_02.enable();
  
  nose_up.rotate(180);
  nose_01.rotate(-20);
  nose_02.rotate(20);
  ear.rotate(-30);
  ear_02.rotate(-30);
  
  delay(50);

  nose_up.rotate(-180);
  nose_01.rotate(20);
  nose_02.rotate(-20);
  ear.rotate(30);
  ear_02.rotate(30);

  nose_up.disable();
  nose_01.disable();
  nose_02.disable();
  ear.disable();
  c1ear_02.disable();
  
  delay(50);
}