In progress parachute system

gistfile1.ino

#include "Bounce2.h"
#include "Servo.h"


#define SERVO_PIN 9
#define READY_PIN 13
#define ARM_PIN 12

#define SERVO_READY 0
#define SERVO_DEPLOY 180
#define VELOCITY_THRESHOLD 15

Servo parachute;
Bounce arm;
bool armed = false;
bool button_state;
int max_velocity;
int current_velocity;

void setup()
{
  pinMode(ARM_PIN, INPUT);
  digitalWrite(ARM_PIN, HIGH);  // Enable pull up resistor
  
  parachute.attach(SERVO_PIN);
  parachute.write(SERVO_READY);
  
  arm.attach(ARM_PIN);
  arm.interval(5);
}

void loop_armed()
{
  // Get current velocity
  
  // Wait until we launch
  if (max_velocity < VELOCITY_THRESHOLD) {
    max_velocity = current_velocity;
    return;
  }
  if (max_velocity <= current_velocity)
  {
    // Going up
    max_velocity = current_velocity;
  } else if (abs(max_velocity - current_velocity) <= VELOCITY_THRESHOLD) {
    // Hit Apogee, deploy shoot, go into unarmed mode
    parachute.write(SERVO_DEPLOY);
    armed = false;
  }
}

void loop_unarmed()
{
  digitalWrite(READY_PIN, HIGH);
  delay(100);
  digitalWrite(READY_PIN, LOW);
}

void loop()
{
  arm.update();
  button_state = arm.read() == 1;
  
  if (button_state != armed) {
    if (button_state) {
      // Transition to armed state
      digitalWrite(READY_PIN, HIGH);
      max_velocity = 0;
      // reinitialize accel
    } else {
      // Transition to unarmed state
    }    
    armed = button_state;
  }
  
  armed ? loop_armed() : loop_unarmed();
}