roboarm.ino

typedef struct {
  uint16_t step_pin;
  uint16_t dir_pin;
  uint16_t enable_pin;
  bool is_stopped;
} Stepper;

typedef enum {
  Forward, Backward
} Direction;

Stepper yaw = {54, 55, 38};
Stepper arm = {60, 61, 56};
Stepper wrist = {46, 48, 62};

void setupStepper(Stepper* s) {
  pinMode(s->step_pin, OUTPUT);
  pinMode(s->dir_pin, OUTPUT);
  pinMode(s->enable_pin, OUTPUT);

  digitalWrite(s->enable_pin, LOW);
}

void driveStepper(Stepper* s, Direction dir, uint16_t _delay) {
  s->is_stopped = false;
  digitalWrite(s->dir_pin, dir == Forward ? HIGH : LOW);

  digitalWrite(s->step_pin, HIGH);
  delayMicroseconds(_delay);
  digitalWrite(s->step_pin, LOW);
}

void stopStepper(Stepper* s) {
  if (s->is_stopped)
    return;
  s->is_stopped = true;
  digitalWrite(s->dir_pin, LOW);
  digitalWrite(s->step_pin, LOW);
}

void setup() {
  // put your setup code here, to run once:
  setupStepper(&yaw);
  setupStepper(&arm);
  setupStepper(&wrist);
}

unsigned long last_time = 0;
uint16_t index = 0;

uint16_t change_state_delay = 1000;

void loop() {
  unsigned long m = millis();
  
  if (m - last_time > change_state_delay) {
    index++;
    last_time = m;
  }

  if (index > 5) {
    index = 0;
  }

  switch (index) {
    case 0:
      // Drive yaw forward for 2.5 secs
      stopStepper(&wrist);
      stopStepper(&arm);
      
      change_state_delay = 2500;
      
      driveStepper(&yaw, Forward, 50);
    break;
    case 1:
      // Drive yaw backward for 2.5 secs
      stopStepper(&wrist);
      stopStepper(&arm);
      
      change_state_delay = 2500;
      
      driveStepper(&yaw, Backward, 50);
    break;
    case 2:
      // Raise arm up for 1 sec
      stopStepper(&yaw);
      stopStepper(&wrist);
      
      change_state_delay = 1000;
      
      driveStepper(&arm, Forward, 100);
    break;
    case 3:
      // Put arm down for 1 sec
      stopStepper(&yaw);
      stopStepper(&wrist);
      
      change_state_delay = 1000;
      driveStepper(&arm, Backward, 100);
    break;
    case 4:
      change_state_delay = 2000;
      stopStepper(&yaw);
      stopStepper(&arm);

      driveStepper(&wrist, Forward, 50);
    break;
    case 5:
      change_state_delay = 2000;
      stopStepper(&yaw);
      stopStepper(&arm);

      driveStepper(&wrist, Backward, 50);
    break;
  }
}