File content as of revision 0:aaf22a04f350:

#include "mbed.h"
#include "USBSerial.h"
#include "IAP.h"

#define SERIAL //comment for USB operation, uncomment for serial
//#define DEBUG 1
#define CALIBRATE 0
#define FUNCTION_CHECK 0

// longest user command we will accept
#define MAX_COMMAND_LEN 64

// where to start reading stored values from the EEPROM
#define BASE_ADDRESS 1

// temperature control stuff

// fixed values
#define CONTROL_INTERRUPT 1000 // every 1ms
#define MAX_DUTY 100 // This is both the number of times the heater_control interrupt will run per control period and the max amount of interrupts the heater triac can be on.

// adjustable values
#define DEFAULT_FILTER 0.05 // first order digital filter parameter, should be [0.0 - 1]. less means filter more, a value of 1 should turn it off
//#define INITIAL_DUTY 25 // 25% duty generally results in a few degrees under 57C output for the ambient temperatures I've been testing at
#define DEFAULT_INITIAL_DUTY 45 
#define DEFAULT_NOMINAL_DUTY 7 // 11% duty is a good set point for Beta init (testing at 20C ambient)
//#define SETPOINT 435 // this results in roughly 57C at the tines on the Alphas (really 53)
#define DEFAULT_SETPOINT 463 //445 -> 53.1-54.1//475 -> 58-59.5// 500 -> 62.5 // 505 -> 59 //475 -> 53C // 445 // 505 seems right for second unit //535-> first unit //527 // this results in roughly 57C at the tines on the Betas
#define DEFAULT_TOLERANCE 2 // if we are within this many ADC counts of the setpoint don't make any changes to the duty cycle
//#define DUTY_TOO_LARGE 40 // If the duty gets larger than this we have problems (Alphas)
#define DEFAULT_DUTY_TOO_LARGE 70 // If the duty gets larger than this we have problems (Betas) -> 65 duty should result in roughly 62C tip temps
//#define DUTY_TOO_LARGE 85 // For testing
//#define DEFAULT_TEMP_LIMIT_LOWER 50 // If the temperature measures too low during operation we are either doing a bad job or have a faulty temperature sensor
#define DEFAULT_TEMP_LIMIT_LOWER 0 // For testing
//#define TEMP_LIMIT_UPPER 485 // No burning allowed (Alphas)
#define DEFAULT_TEMP_LIMIT_UPPER 625 // No burning allowed (Betas)

float filter = DEFAULT_FILTER;
uint8_t initial_duty = DEFAULT_INITIAL_DUTY;
uint8_t nominal_duty = DEFAULT_NOMINAL_DUTY;
uint16_t setpoint = DEFAULT_SETPOINT;
uint8_t tolerance = DEFAULT_TOLERANCE;
uint8_t duty_too_large = DEFAULT_DUTY_TOO_LARGE;
uint16_t temp_limit_lower = DEFAULT_TEMP_LIMIT_LOWER;
uint16_t temp_limit_upper = DEFAULT_TEMP_LIMIT_UPPER;

enum State {
    IDLE,
    WAIT_FOR_TIP,
    INITIAL_RAMP,
    ACTIVE,
    ERROR,
};

enum Heater_State {
    ON,
    OFF,
};

// not used yet
enum Error {
    NONE,
    TEMP_TOO_LOW,
    TEMP_TOO_HIGH,
    DUTY_TOO_LARGE,
};

// Heater control pins
DigitalOut fan(P0_22);
DigitalOut heater_pin(P1_27);
AnalogIn temp_sense(P0_16);

// LED pins
DigitalInOut empty_led(P0_10);
DigitalInOut fuel_gage_1(P0_9);
DigitalInOut fuel_gage_2(P0_8);
DigitalInOut fuel_gage_3(P1_21);
DigitalInOut fuel_gage_4(P1_31);
DigitalOut tip_light(P1_14);

DigitalIn tip_sensor(P0_14);

#define ON_TIME_S 2700L // The device is active for 45 minutes
//#define ON_TIME_S 150L
#define RED_LED_ON_TIME_S 300L // The red LED is active for the last 5 minutes
//#define RED_LED_ON_TIME_S 30L
#define TIP_FLASH_INTERVAL_S 20L // How often the tip will flash
//Serial pc(0x1f00, 0x2012, 0x0001, false);

#ifdef SERIAL
Serial pc(P0_19, P0_18); // tx, rx
#else
USBSerial pc(0x1f00, 0x2012, 0x0001);
#endif

Ticker control_interrupt;
Ticker check_limits_interrupt;
Ticker temperature_interrupt;
Ticker tick_interrupt;

volatile Heater_State heater = OFF;
volatile State state = WAIT_FOR_TIP;
volatile Error error_state = NONE;

volatile uint8_t duty = DEFAULT_INITIAL_DUTY;
volatile float temperature = 0.0;
unsigned long start_time = 0L;
unsigned long current_time = 0L;
bool connected = false;
volatile uint32_t millis = 0;

void loop(void);
void calibrate(bool first);
void interpret(char parameter, int value);
void spin_lights(bool first);

//INTERRUPT - increment this every ms since the normal mbed timer returns an int and real code needs an unsigned type
void tick(void) {millis++;}

uint32_t get_time(void){
    uint32_t copy = 0;
    __disable_irq();
    copy =  millis;
    __enable_irq();
    return copy;
}

uint8_t get_duty(void){
  __disable_irq();
  uint8_t duty_copy = duty;
  __enable_irq();
  return duty_copy;
}

void set_duty(uint8_t new_duty){
  __disable_irq();
  duty = new_duty;
  __enable_irq();
}

float get_temperature(void){
  __disable_irq();
  float temperature_copy = temperature;
  __enable_irq();
  return temperature_copy;
}

//pull the settings saved to flash into memory
void read_settings(void){
    uint8_t address = BASE_ADDRESS;
    uint16_t filter_temp = 0;
    
    IAP iap;  

    iap.read_eeprom((char*)address, (char*)&filter_temp, sizeof(filter_temp));
    address += sizeof(filter_temp);
    filter = (float)filter_temp/100;
    iap.read_eeprom((char*)address, (char*)&initial_duty, sizeof(initial_duty));
    address += sizeof(initial_duty);
    iap.read_eeprom((char*)address, (char*)&setpoint, sizeof(setpoint));
    address += sizeof(setpoint);
    iap.read_eeprom((char*)address, (char*)&tolerance, sizeof(tolerance));
    address += sizeof(tolerance);
    iap.read_eeprom((char*)address, (char*)&duty_too_large, sizeof(duty_too_large));
    address += sizeof(duty_too_large);
    iap.read_eeprom((char*)address, (char*)&temp_limit_lower, sizeof(temp_limit_lower));
    address += sizeof(temp_limit_lower);
    iap.read_eeprom((char*)address, (char*)&temp_limit_upper, sizeof(temp_limit_upper));
    address += sizeof(temp_limit_upper);
    iap.read_eeprom((char*)address, (char*)&nominal_duty, sizeof(nominal_duty));
    address += sizeof(nominal_duty);
}

//write settings to persistent memory
void write_settings(void){
 IAP iap;
 uint8_t address = 1; //BASE_ADDRESS;
 uint16_t filter_temp = 0;
    
 filter_temp = (int)(filter*100);
 
 iap.write_eeprom((char*)&filter_temp, (char*)address, sizeof(filter_temp));
 address += sizeof(filter_temp);
 iap.write_eeprom((char*)&initial_duty, (char*)address, sizeof(initial_duty));
 address += sizeof(initial_duty);
 iap.write_eeprom((char*)&setpoint, (char*)address, sizeof(setpoint));
 address += sizeof(setpoint);
 iap.write_eeprom((char*)&tolerance, (char*)address, sizeof(tolerance));
 address += sizeof(tolerance);
 iap.write_eeprom((char*)&duty_too_large, (char*)address, sizeof(duty_too_large));
 address += sizeof(duty_too_large);
 iap.write_eeprom((char*)&temp_limit_lower, (char*)address, sizeof(temp_limit_lower));
 address += sizeof(temp_limit_lower);
 iap.write_eeprom((char*)&temp_limit_upper, (char*)address, sizeof(temp_limit_upper));
 address += sizeof(temp_limit_upper);
 iap.write_eeprom((char*)&nominal_duty, (char*)address, sizeof(nominal_duty));
 address += sizeof(nominal_duty);
}

//parse commands. commands take the form of a character followed by a number, delimited by "\r|\n|;"
void getInput(void){
  static int i = 0;
  static char parameter = '_';
  static char buffer[MAX_COMMAND_LEN + 1];
  int value = 0;
  //char *endp = NULL;
  
  if(!connected) return;
  
  // listen for commands coming in
#ifdef SERIAL
  while (pc.readable()){
      char ch = pc.getc();
#else
  while (pc.available()){
      char ch = pc._getc();
#endif
    if((ch == '\r' || ch == ';' || ch == '\n') && parameter != '_'){
      if(i > 1){
        buffer[i-1] = 0;
        value = atoi(buffer);
      }
      pc.printf("%c, %d", parameter, value);
      //Serial.println("not _");
      interpret(parameter, value);
      parameter = '_';
      buffer[0] = 0;
      i=0;
      break;
    }
    else{
      if(i==0) parameter = ch;
      else buffer[i-1] = ch;
      i++;
    }
    
    if(ch == '_' || ch == '\r' || ch == ';' || ch == '\n'){
      parameter = '_';
      buffer[0] = 0;
      i=0;
    }
  }
}

//print usage info
void usage(void){
    if(!connected)return;
    pc.printf("\r\nCommands are a character followed by a number.\r\n");
    wait_ms(1);
    pc.printf("Available commands are:\r\n");
    wait_ms(1);
    pc.printf("'c': duty cap [1-100]\r\n");
    wait_ms(1);
    pc.printf("'d': set the initial duty used on startup [1-100]\r\n");
    wait_ms(1);
    pc.printf("'f': set the smoothing filter for the temperature sensor.");
    wait_ms(1);
    pc.printf(" smaller is more smoothing. [1-100]\r\n");
    wait_ms(1);
    pc.printf("'l': lower temperature limit. arbitrary units. [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'r': reset all parameters to default values.\r\n");
    wait_ms(1);
    pc.printf("'n': set the nominal duty used on control cycle start [1-100]\r\n");
    wait_ms(1);
    pc.printf("'s': setpoint. same units as upper and lower temperature");
    wait_ms(1);
    pc.printf(" limit [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'t': tolerance. no control action within this band. same ");
    wait_ms(1);
    pc.printf("units as upper and lower temperature limit [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'u': upper temperature limit. arbitrary units. [1-1000]\r\n");
    wait_ms(1);
    pc.printf("'w': write the current control parameters to permanent memory\r\n");
    wait_ms(1);
    pc.printf("\r\n\r\n");
    wait_ms(1);
    
    pc.printf("Values can be modified by entering a ");
    pc.printf("command followed by a number.");
    wait_ms(1);
    pc.printf(" For example, entering 'c50' <enter> would limit the duty");
    wait_ms(1);
    pc.printf(" to 50%\r\n\r\n");
    wait_ms(1);

    pc.printf("Status of individual variables can be queried by entering ");
    wait_ms(1);
    pc.printf("that command name (no number afterwards), or typing any ");
    wait_ms(1);
    pc.printf("unrecognised command (which prints this message).\r\n\r\n");
    wait_ms(1);
}

void print_active_settings(void){
    if(!connected)return;
    pc.printf("Duty capped at: %u%\r\n", duty_too_large);
    wait_ms(1);
    pc.printf("Initial duty is: %u\r\n", initial_duty);
    wait_ms(1);
    pc.printf("Nominal duty is: %u\r\n", nominal_duty);
    wait_ms(1);
    pc.printf("Temperature filter is: %d\r\n", (int)(filter * 100));
    wait_ms(1);
    pc.printf("Lower temperature limit is: %u\r\n", temp_limit_lower);
    wait_ms(1);
    pc.printf("Upper temperature limit is: %u\r\n", temp_limit_upper);
    wait_ms(1);
    pc.printf("Setpoint is: %u\r\n", setpoint);
    wait_ms(1);
    pc.printf("Tolerance is %u\r\n\r\n", tolerance);
    wait_ms(1);
}

//interpret a user command
void interpret(char parameter, int value){
  switch(parameter){
  case 'c':
    if(value != 0) duty_too_large = value;
    pc.printf("Duty cap is %u\r\n", duty_too_large);
    break;
  case 'd':
    if(value != 0) initial_duty = value;
    pc.printf("Initial duty is %u\r\n", initial_duty);
    break;
  case 'f':
    if(value != 0) filter = ((float)value) / 100.0;
    pc.printf("Filter is %d\r\n", (int)(filter * 100));
    break;
  case 'r':
    filter = DEFAULT_FILTER;
    initial_duty = DEFAULT_INITIAL_DUTY;
    nominal_duty = DEFAULT_NOMINAL_DUTY;
    setpoint = DEFAULT_SETPOINT;
    tolerance = DEFAULT_TOLERANCE;
    duty_too_large = DEFAULT_DUTY_TOO_LARGE;
    temp_limit_lower = DEFAULT_TEMP_LIMIT_LOWER;
    temp_limit_upper = DEFAULT_TEMP_LIMIT_UPPER;
    write_settings();
    pc.printf("All parameters reset to default values:\r\n");
    print_active_settings();
    break; 
  case 'l':
    if(value != 0) temp_limit_lower = value;
    pc.printf("Lower temperature limit is %u\r\n", temp_limit_lower);
    break;
  case 'n':
    if(value != 0) nominal_duty = value;
    pc.printf("Nominal duty is %u\r\n", nominal_duty);
    break;
  case 's':
    if(value != 0) setpoint = value;
    pc.printf("Setpoint is %u\r\n", setpoint);
    break;
  case 't':
    if(value != 0) tolerance = value;
    pc.printf("Tolerance is %u\r\n", tolerance);
    break;
  case 'u':
    if(value != 0) temp_limit_upper = value;
    pc.printf("Upper temperature limit is %u\r\n", temp_limit_upper);
    break;
  case 'w':
    write_settings();
    pc.printf("Wrote the current control parameters to memory.\r\n");
    break;


  default:
    usage();    
    print_active_settings();
    break;
  }
}

//put everything into a low power/off state. control loop will be unaffected by this command, so that will also need to be disabled before the heater will stay off
void all_off(){
  heater_pin = 0;
  heater = OFF; //need to treat this as volatile
  fan = 0;
  empty_led.input(); //
  fuel_gage_1.input(); // = 1;
  fuel_gage_2.input(); // = 1;
  fuel_gage_3.input(); // = 1;
  fuel_gage_4.input(); // = 1;
  tip_light = 0;
}

// run this to manually check the hardware works
// probably best to disable the heater on the first try
void functional_check(void){
  all_off();  
  if(connected)pc.printf("Tip light\r\n");
  tip_light = 1;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Empty\r\n");
  empty_led.output();
  empty_led = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 1\r\n");
  fuel_gage_1.output();
  fuel_gage_1 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 2\r\n");
  fuel_gage_2.output();
  fuel_gage_2 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 3\r\n");
  fuel_gage_3.output();
  fuel_gage_3 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fuel Gage 4\r\n");
  fuel_gage_4.output();
  fuel_gage_4 = 0;
  wait_ms(1000);
  all_off();
  if(connected)pc.printf("Fan\r\n");
  fan = 1;
  wait_ms(5000);
  if(connected)pc.printf("Heater\r\n");
  heater_pin = 1;
  
  while(1){
    if(connected)pc.printf("Temp: %u\r\n", temp_sense.read_u16()>>6);
    wait_ms(50);
    tip_light = 1;
    wait_ms(50);
    tip_light = 0;
  }
}

// INTERRUPT - called every 4ms, samples the ADC and filters the value with a low pass first order digital filter
void get_temp(void){
  // not bothering with units, this means we need to keep the update constant at once every 4ms or we will need to set the filter again
  //temperature = filter * (temp_sense.read_u16()>>6) + (1 - filter) * temperature;
  // temperature is now in degrees C (value is at sensor, not at tips)
  temperature = filter * (51.282 * (temp_sense.read()*3.3 - 0.4)) + (1 - filter) * temperature;
}

// INTERRUPT - called every millisecond to handle maintaining the on/off state of the triac
void heater_control(void){
  static uint8_t count = 0;
  
  // count up once per interrupt.
  // safety: check to make sure we haven't exceeded MAX_DUTY safety limit
  // after duty is reached for this control period, turn the heat off 
  count++;
  
  if(heater == ON && fan.read()){
    if(count > MAX_DUTY){
      count = 0;
      heater_pin = 1;
    }
    if(count > duty)heater_pin = 0;
  }
  else heater_pin = 0;
}

// bail, something is wrong
void abort(bool error){
  uint16_t period = 1000;
  if(error) period = 250;
  // turn everything off, leave the fan on for a few seconds and flash the red LED
  control_interrupt.detach();
  all_off();
  empty_led.input();
  fan = 1;
  wait_ms(3000);
  fan = 0;
  while(1){
    empty_led.output();
    empty_led = 0;
    heater_pin = 0;
    wait_ms(period);
    heater_pin = 0;
    empty_led.input();
    heater_pin = 0;
    wait_ms(period);
  }
}

// INTERRUPT - monitor stuff that might indicate an error condition
void check_limits(void){
// need to move printing to the main loop
  if(duty >= duty_too_large){
    if(connected){
        pc.printf("Error!!! Duty cycle has become unreasonably ");
        pc.printf("large, Aborting.\r\n");
    }
    abort(true);
  }
   
  if(get_temperature() > temp_limit_upper){
    if(connected)pc.printf("Error!!! Temperature is too high, Aborting.\r\n");
    abort(true);
  }
  
  if((state == ACTIVE || state == INITIAL_RAMP) && (get_temperature() < temp_limit_lower)){
    if(connected){
        pc.printf("%f\r\n", get_temperature());
        pc.printf("Error!!! Abnormally low temperature detected. ");
        pc.printf("Please check the sensor, Aborting.\r\n");
    }
    abort(true);
  }
}

// values pulled from the MCP9701T-E/LT datasheet
// 19.5mV/deg-C, 400mV @ 0C
float adc_to_temp(float adc_value){
 //return 0.195 * adc_value - 
 return 0.196 * adc_value - 31.322;
}

void init(void){
#ifdef SERIAL
  pc.baud(115200);
  connected = true;
#else
  pc.connect();
  connected = pc.vbusDetected();
#endif

  if(connected)pc.printf("hello\r\n");
  
  tick_interrupt.attach(&tick, 0.001);
  control_interrupt.attach(&heater_control, 0.001);
  temperature_interrupt.attach(&get_temp, 0.004);
  if(!CALIBRATE)check_limits_interrupt.attach(&check_limits, 0.5);

  read_settings();
  set_duty(initial_duty);
  all_off();
}

// tip neeeds to be present before we can start the cycle
// later we will also abort the cycle if the tip is removed
void do_wait_for_tip(bool first){
  unsigned long time = get_time();
  unsigned long time_increment = time % 1800;
  
  if(first){
    if(connected)pc.printf("Looking for tip\r\n");  
  }  
    if(!tip_sensor){ //tip present is low == present
      tip_light = 1;
      fuel_gage_1.output();
      fuel_gage_1 = 0;
      fuel_gage_2.output();
      fuel_gage_2 = 0;
      fuel_gage_3.output();
      fuel_gage_3 = 0;
      fuel_gage_4.output();
      fuel_gage_4 = 0;
      empty_led.input();
      state = INITIAL_RAMP;
      if(connected)pc.printf("Found the tip\r\n");
      return;
    }
        
    if(time_increment < 300){
        fuel_gage_1.input(); // = 1;
        fuel_gage_2.input(); // = 1;
        fuel_gage_3.input(); // = 1;
        fuel_gage_4.input(); // = 1;
    }
    if(time_increment > 600){
      fuel_gage_1.output();
      fuel_gage_1 = 0;
    }
    if(time_increment > 900){
      fuel_gage_2.output();
      fuel_gage_2 = 0;
    }
    if(time_increment > 1200){
      fuel_gage_3.output();
      fuel_gage_3 = 0;
    }
    if(time_increment > 1500){
      fuel_gage_4.output();
      fuel_gage_4 = 0;
    }
}

//This should quickly take us up from ambient to the setpoint.
void do_initial_ramp(bool first){
  // set duty to initial_duty and wait to reach the setpoint to break out
  static uint32_t start_time = 0;
  static uint32_t last_print = get_time();
  
  if(first){
    start_time = get_time();
    fan = 1;
    set_duty(initial_duty);
    heater = ON;
    if(connected)pc.printf("Initial ramp up. Duty will be held constant until setpoint is reached.\r\n");
  }
  
  if(get_time() - start_time > 60000){
     if(connected)pc.printf("Took too long to reach setpoint, aborting.\r\n");
     abort(true);
  }
  
  if(get_time() - last_print > 5000){
    if(connected)pc.printf("Duty: %u, Temp: %f, Time: %.2fs\r\n", get_duty(), get_temperature(), (float)get_time()/1000);
    last_print = get_time();
  } 
  
  if(get_temperature() > setpoint - tolerance){
    //now we are roughly up to temperature
    set_duty(nominal_duty);
    state = ACTIVE; 
    return;
  }
}

void do_treatment_cycle(bool first){
  static uint32_t start_time = 0;
  static uint32_t control_timer = 0;
  uint8_t duty_copy;
  float temperature_copy;
  
  if(first){
    start_time = get_time();
    control_timer = start_time;
  }
  
  uint32_t current_time = get_time() - start_time;
  
  // check if we're done
  if(current_time >= ON_TIME_S * 1000L){
    if(connected)pc.printf("Done!\r\n");
    abort(false);
    state = IDLE;
  }
  
  //if(!tip->in_place())abort(false);
   
  if(current_time - control_timer > 5000){ // run the control loop every 5 seconds
    control_timer = current_time;
    duty_copy = get_duty();
    temperature_copy = get_temperature();
    if(temperature_copy > setpoint + tolerance) duty_copy--;
    if(temperature_copy < setpoint - tolerance) duty_copy++;
    set_duty(duty_copy);
    
    if(connected)pc.printf("Duty: %u, Temp: %f, Time: %.2fs\r\n", get_duty(), get_temperature(), ((float)get_time() - (float)start_time)/1000);
  }
}

void print_state(void){
    if(!connected)return;
    switch(state){
        case IDLE:
          printf("IDLE\r\n");
          break;
        case WAIT_FOR_TIP:
          printf("WAIT_FOR_TIP\r\n");
          break;
        case INITIAL_RAMP:
          printf("INITIAL_RAMP\r\n");
          break;
        case ACTIVE:
          printf("ACTIVE\r\n");
          break;
        case ERROR:
          printf("ERROR\r\n");
          break;
        default: break;
    }
}

int main(){
  static State last_state = IDLE;
  init();
  //if(FUNCTION_CHECK) functional_check();
  if(CALIBRATE){
      calibrate(true);
      while(1)calibrate(false);
  }
  
  while(1){
    getInput();    
    
    bool state_change = false;
    if(state != last_state){
        state_change = true;
        last_state = state;
        print_state();
    }
      
   switch(state){
      case IDLE:
        // later put a check to exit state per on/off switch
        break;
      case WAIT_FOR_TIP:
        do_wait_for_tip(state_change);
        break;
      case INITIAL_RAMP:
        do_initial_ramp(state_change);
        spin_lights(state_change);
        break;
      case ACTIVE:
        do_treatment_cycle(state_change);
        spin_lights(false);
        break;
      case ERROR:
        abort(true);
        break;
      default: break;
    }   
  }
}

void spin_lights(bool first){
  static uint32_t start_time = 0;
  
  if(first) start_time = get_time();
  uint32_t current_time = get_time() - start_time;
  
  uint32_t tip_time = current_time % 23000;
  if(tip_time < 20000) tip_light = 1;
  else tip_light = (tip_time % 100 < 50)?1:0;
  //if(current_time > 4600) current_time = 0;
  
  // handle fule gage LEDs
  // this should be more directly linked to the treatment stop condition
  uint32_t step = (ON_TIME_S - RED_LED_ON_TIME_S)/5;
  step *= 1000;
  
  if(current_time > step) fuel_gage_4.input();
  else{
    fuel_gage_4.output();
    fuel_gage_4 = 0;
  }
  if(current_time > 2*step) fuel_gage_3.input();
  else{
    fuel_gage_3.output();
    fuel_gage_3 = 0;
  }
  if(current_time > 3*step) fuel_gage_2.input();
  else{
    fuel_gage_2.output();
    fuel_gage_2 = 0;
  }
  if(current_time > 4*step) fuel_gage_1.input();
  else{
    fuel_gage_1.output();
    fuel_gage_1 = 0;
  }
  if(current_time > 5*step){
      empty_led.output();
      empty_led = 0;
  }
  else empty_led.input();
}

void calibrate(bool first){
  static uint32_t start_time = 0;
  static uint32_t control_timer = 0;
  uint8_t duty_copy;
  
  if(first){
    start_time = get_time();
    control_timer = start_time;
    fan = 1;
    set_duty(5);
    heater = ON;
  }
  
  uint32_t current_time = get_time() - start_time;
  
  if(current_time - control_timer > 15000){ // increase the duty by 5% every 15 seconds
    if(connected)pc.printf("Duty: %u, Temp: %f, Time: %.2fs\r\n", get_duty(), get_temperature(), ((float)get_time() - (float)start_time)/1000);
    control_timer = current_time;
    duty_copy = get_duty();
    duty_copy += 5;
    // check if we're done
    if(duty > 75 && connected){
      set_duty(1);
      pc.printf("Done!\r\n");
      abort(false);
    }
    set_duty(duty_copy);
  }    
}

/*
void calibrate(void){
  // this is really not necessary because all we care about is the temperature we need to hold the sensor at,
  // which we are assuming directly correlates to output air temp.
  // in reality it will probably be affected by ambient temperature,
  // humidity, air flow rate, elevation (air density), and other factors 
  static uint16_t count = 0;
  static uint8_t duty_copy = 5; //inital duty
  float temperature_copy;
  static int tester_count = 0;

  current_time = get_time();
  count++;

  check_limits();

  if(count > 200){ // run the control loop every second
    count = 0;
    tester_count++;
    if(tester_count % 30 == 0){ //30 seconds each duty cycle
      duty_copy += 5;
      if(duty > 85)abort(false);
      if(connected)pc.printf("Duty: %u\r\nTemps:\r\n", duty_copy);
    }
    __disable_irq();
    duty = duty_copy;
    temperature_copy = temperature;
    __enable_irq();
  
    if(DEBUG && (tester_count % 30) > 24)pc.printf("\t%f\t%u\t%f\r\n", temperature_copy, temp_sense.read_u16(), temp_sense.read());
  }
}*/