 
 void moveMotor()
 {
-  long starttime = micros();
   bool reversed = 0;
   correctedOutput = Output;
 

}

/* MOSFET STATES */
void state0(double pwmfreq)
{
  PORT->Group[PORTA].OUTSET.reg = (1<<20);
  PORT->Group[PORTA].OUTSET.reg = (1<<19);
//  PORT->Group[PORTA].OUTSET.reg = 110000000000000000000; //P2N3
}

void state1(double pwmfreq)
{
  PORT->Group[PORTA].OUTSET.reg = (1<<15);
  PORT->Group[PORTA].OUTSET.reg = (1<<19);
//  PORT->Group[PORTA].OUTSET.reg = 10001000000000000000; //P1N3
}

void state2(double pwmfreq)
{
  PORT->Group[PORTA].OUTSET.reg = (1<<15);
  PORT->Group[PORTA].OUTSET.reg = (1<<16);
//  PORT->Group[PORTA].OUTSET.reg = 11000000000000000; //P1N2
}

void state3(double pwmfreq)
{
  PORT->Group[PORTA].OUTSET.reg = (1<<7);
  PORT->Group[PORTA].OUTSET.reg = (1<<16);
//  PORT->Group[PORTA].OUTSET.reg = 10000000010000000; //P3N2
}

void state4(double pwmfreq)
{
  PORT->Group[PORTA].OUTSET.reg = (1<<7);
  PORT->Group[PORTA].OUTSET.reg = (1<<18);
//  PORT->Group[PORTA].OUTSET.reg = 1000000000010000000; //P3N1
}

void state5(double pwmfreq)
{
  PORT->Group[PORTA].OUTSET.reg = (1<<20);
  PORT->Group[PORTA].OUTSET.reg = (1<<18);
//  PORT->Group[PORTA].OUTSET.reg = 101000000000000000000; //P2N1
}


//    analogWrite(motorN1, pwmfreq);
//}

uint16_t readCurrent() //read current for active mosfet
{
  uint16_t coilCurrent = 0;
  if ((state == 0) || (state == 1))
    coilCurrent = analogRead(current0);
  if ((state == 2) || (state == 3))
