   set_pwm_duty(current_pwm);
 }
 
-void constant_voltage_control(int target_voltage) {
-  
-  int current_pwm = power_status.pwm_duty;
-  int target_difference = target_voltage - power_status.bat_volts;
-
-  // if target difference is large, use big steps
-  int pwm_delta = 1;
-  if (abs(target_difference) > 100) {
-    pwm_delta = 16;
-  }
-
-  // adjust pwm either up or down based on difference sign
-  if (target_difference < 0) {
-    current_pwm -= pwm_delta;
-  } else if (target_difference > 0) {
-    current_pwm += pwm_delta;
-  }
-  
-  set_pwm_duty(current_pwm);
-}
+void state_switch(charger_mode_t mode, int target = 0) {
+
+  switch (mode) {
+    case off:
+      TURN_OFF_MOSFETS;
+      power_status.mode = mode;
+      break;
+      
+    case const_volt:
+      TURN_ON_MOSFETS;
+      power_status.mode = mode;
+      power_status.target = target;
+      break;
+      
+    case const_power:
+      TURN_ON_MOSFETS;
+      power_status.mode = mode;
+      power_status.target = target;
+      break;
+
+    case mppt_on:
+      TURN_ON_MOSFETS;
+      power_status.mode = mode;
+      break;
+
+    default:
+      break;
+  }
+}
+
+// State machine for different modes
+void state_machine(void) {
+  switch (power_status.mode) {
+    case const_volt:
+      adjust_pwm(power_status.target - power_status.bat_volts);
+      break;
+    case const_power:
+      adjust_pwm(power_status.target - power_status.sol_watts);
+      break;
+    case off:
+      break; // do nothing if it is in off mode
+    // other cases: call mppt state-machine
+    default:
+      MPPT_state_machine();
+      break;
+  }
+}
+
+//------------------------------------------------------------------------------------------------------
+// This routine is automatically called at powerup/reset
+//------------------------------------------------------------------------------------------------------
+void setup()                               // run once, when the sketch starts
+{
+  Serial.begin(115200);                    // open the serial port at 9600 bps:
+  Timer1.initialize(20);                   // initialize timer1, and set a 20uS period
+  Timer1.pwm(PWM_PIN, 0);                  // setup pwm on pin 9, 0% duty cycle
+  Timer1.attachInterrupt(timer_callback);  // attaches timer_callback() as a timer overflow interrupt
+
+  pinMode(PWM_ENABLE_PIN, OUTPUT);         // sets the digital pin as output
+  TURN_OFF_MOSFETS;                        // turn on MOSFET driver chip
+  set_pwm_duty(PWM_START);
+
+  lpf_in_volts.Init();                     // initialize ADC low-pass filters
+  lpf_out_volts.Init();
+  lpf_in_amps.Init();
+
+  //state_switch(off);                       // start with charger state as off
+  state_switch(const_volt, 300);                       // start with charger state as off
+}
+
+//------------------------------------------------------------------------------------------------------
+// Main loop.
+// Right now the number of times per second that this main loop runs is set by how long the printing to 
+// the serial port takes. You can speed that up by speeding up the baud rate.
+// You can also run the commented out code and the charger routines will run once a second.
+//------------------------------------------------------------------------------------------------------
 
 void loop()                          // run over and over again
 {
   read_data();                         //read data from inputs
-  //MPPT_state_machine();                      //run the charger state machine
-
+  state_machine();                     //run the state machine
+
+  /*
   // update power info
-  static int current_power = 0;
+  static int current_target = 0;
   static unsigned int prev_seconds = 0;        // seconds value from previous pass
   if ((seconds - prev_seconds) > 4) {  
     prev_seconds = seconds;		// do this stuff once a second
 
-    if (current_power >= 400) {
-      current_power = 100;
+    if (current_target >= 400) {
+      current_target = 0;
     }
     else {
-      current_power += 100;
+      current_target = 400;
     }
-  }
-
-  //constant_voltage_control(current_power);
-  //constant_voltage_control(500);
-  //constant_power_control(current_power);
-  constant_power_control(150);
-
+
+    state_switch(const_power, current_target);
+    //state_switch(const_volt, current_target);
+  }*/
+  
   // diagnosistic prints
   static int print_counter = 0;
   print_counter++;
-  if (print_counter > 200) {
-    print_data(); 
+  if (print_counter > 50) {
+    print_data_json(); 
     print_counter = 0;
   }
-
-
-//    read_data();                         //read data from inputs
-//    MPPT_state_machine();
-//    print_data();                       //print data
-//  }
-}
-//------------------------------------------------------------------------------------------------------
+}
+//------------------------------------------------------------------------------------------------------

  Serial.print("  ");

  Serial.print("s_volts = ");
  //Serial.print(sol_volts,DEC);
  //Serial.print("  ");
  print_int100_dec2(power_status.sol_volts);
  Serial.print("  ");



//------------------------------------------------------------------------------------------------------
// global variables
enum charger_mode_t {off, mppt_on, auto_off, bulk, bat_float, const_volt, const_power};

struct system_states_t {
   int sol_amps;                         // solar amps in 10 mV (scaled by 100)
   int sol_volts;                        // solar volts in 10 mV (scaled by 100)
   int bat_volts;                        // battery volts in 10 mV (scaled by 100)
   int sol_watts;                        // solar watts in 10 mV (scaled by 100)
   uint16_t pwm_duty;                    // pwm target duty cycle, set by set_pwm_duty function
   int target;                           // voltage or power target
   charger_mode_t mode;                  // state of the charge state machine
} power_status;

unsigned int seconds = 0;             // seconds from timer routine
  
// enumerated variable that holds state for charger state machine
void set_pwm_duty(int pwm);

//------------------------------------------------------------------------------------------------------
// Circular buffer type for ADC averaging filter
class LowPassBuffer {
  public:
    const static int N = AVG_NUM;

    void Init(void) {
      data_len = 0;
