//Crappy Trapazoidal commutation per Joystick input
//Written by E. Vartanians on 1/31/2024
//To be run on Arduino Feather 32u4


int counter =0;
int ComStep = 1;
int MaxDutyCycle = 90;
int DutyCycle = 15;
int MinDutyCycle = 8;
int dir = 1;
int dwellTimeus = 4000;    //noload 800us at 15% was empirically characterized
int ComutationMode = 1;   //0 = align, 1 = openloop, 2 = closed loop
int PWMpulseNumInComm = 0;
int PWMPulsesToSkip = 0;

void setup() {
  // put your setup code here, to run once:
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(5, OUTPUT);pinMode(6, OUTPUT);pinMode(9, OUTPUT);pinMode(10, OUTPUT);pinMode(13, OUTPUT);pinMode(12, OUTPUT);
  digitalWrite(5,HIGH);digitalWrite(9,HIGH);digitalWrite(12,HIGH);

  pinMode(A4,INPUT);
  
  pinMode(23, OUTPUT);
  // start serial port at 9600 bps and wait for port to open:
  Serial.end();
  Serial.begin(9600);

  delay(2000);

  ResetTheRegs();
  SetupPWMRegs();

  commutate(6,10);    //align the rotor to commutation position 1
  delayMicroseconds(10);
  PORTF=0x01;PORTF=0x00;
  commutate(0,100);    //deadband
  StartTimer0(dwellTimeus);
}

void loop() {
  int gas = 0;
  
  while(1){ 

  if(ComutationMode == 2){
    gas = floor(analogRead(A4)/22);
    DutyCycle =15 +  map (gas, 23,46,0,75);
    Serial.println(DutyCycle);
  }else {
    Serial.println(ComutationMode);
  }
  
  }
}

ISR(TIMER0_COMPA_vect){        // ISR for open loop commutation

      if(ComutationMode==1){
        if(ComStep==6){ComStep=1;}else{ComStep=ComStep+1;}
        commutate(ComStep,DutyCycle);
        if(counter > 10){
          counter =0;
          EndTimer0();
          ComutationMode=2;
          PWMPulsesToSkip=1;           
        }
      }

      counter = counter + 1;
/*      if (ComutationMode ==2){
          if(counter > 70){
            counter =0;
            if(dir==1){DutyCycle = DutyCycle +1; if(DutyCycle>MaxDutyCycle){DutyCycle=MaxDutyCycle;dir=0;}}
            if(dir==0){DutyCycle = DutyCycle -1; if(DutyCycle<MinDutyCycle){DutyCycle=MinDutyCycle;dir=1;}}
//            Serial.println(DutyCycle);
          }
      }        
*/
}

ISR(TIMER4_COMPA_vect){        // ISR for when PWM of halfbridge A is switching
    
    if(((PINC & 128) == 128)){    //when A half bridge is PWMing
      PWMpulseNumInComm+=1;
      delayMicroseconds(10);
      if (ComStep == 2 ){    //are we in comstep 2? With A PWMing it means driving AC coil so this is where we'll be checking BEMF of B
        if((PWMpulseNumInComm > PWMPulsesToSkip) & ((PINF & 64) == 64)){         //Is BEMF of B above zero crossing? (in comstep 2 coil B BEMF is rising after having been low in comstep 1). AND that with making sure we're past the PWMPulsesToSkip
            if(ComutationMode == 2){
              ComStep = 3;
              PWMpulseNumInComm=0;
              commutate(3,DutyCycle);
            }
        }  
      }
      if (ComStep == 1 ){    //are we in comstep 1? With A PWMing it means driving AB coil so this is where we'll be checking BEMF of C
        if((PWMpulseNumInComm > PWMPulsesToSkip) & ((PINF & 32) == 0)){         //Is BEMF of C below zero crossing? (in comstep 1 coil C BEMF is falling after having been high in comstep 6). AND that with making sure we're past the PWMPulsesToSkip
            if(ComutationMode == 2){
              ComStep = 2;
              PWMpulseNumInComm=0;
              commutate(2,DutyCycle);
            }
        }
      }      
    }  
}
ISR(TIMER4_COMPB_vect){        // ISR for when PWM of halfbridge B is switching

     if((PINB & 64) == 64){    //when B half bridge is PWMing
      PWMpulseNumInComm+=1;
      delayMicroseconds(10);
      if (ComStep == 4 ){    //are we in comstep 4? With B PWMing it means driving BA coil so this is where we'll be checking BEMF of C
        if((PWMpulseNumInComm > PWMPulsesToSkip) & ((PINF & 32) == 32)){         //Is BEMF of C above zero crossing? (in comstep 4 coil C BEMF is rising after having been low in comstep 3)
            if(ComutationMode == 2){
              ComStep = 5;
              PWMpulseNumInComm=0;
              commutate(5,DutyCycle);
            }
        }  
      }
      if (ComStep == 3 ){    //are we in comstep 3? With B PWMing it means driving BC coil so this is where we'll be checking BEMF of A
        if((PWMpulseNumInComm > PWMPulsesToSkip) & ((PINF & 128) == 0)){         //Is BEMF of A below zero crossing? (in comstep 3 coil A BEMF is falling after having been high in comstep 2)
            if(ComutationMode == 2){
              ComStep = 4;
              PWMpulseNumInComm=0;
              commutate(4,DutyCycle);
            }
        }
      }      
    }  
}
ISR(TIMER4_COMPD_vect){        // ISR for when PWM of halfbridge C is switching
 
     if((PIND & 128) == 128){    //when C half bridge is PWMing
      PWMpulseNumInComm+=1;
      delayMicroseconds(10); 
      if (ComStep == 6 ){    //are we in comstep 6? With C PWMing it means driving CB coil so this is where we'll be checking BEMF of A
        if((PWMpulseNumInComm > PWMPulsesToSkip) & ((PINF & 128) == 128)){         //Is BEMF of A above zero crossing? (in comstep 6 coil A BEMF is rising after having been low in comstep 5)
            if(ComutationMode == 2){
              ComStep = 1;
              PWMpulseNumInComm=0;
              commutate(1,DutyCycle);
            }
        }  
      }
      if (ComStep == 5 ){    //are we in comstep 5? With C PWMing it means driving CA coil so this is where we'll be checking BEMF of B
        if((PWMpulseNumInComm > PWMPulsesToSkip) & ((PINF & 64) == 0)){         //Is BEMF of B below zero crossing? (in comstep 5 coil B BEMF is falling after having been high in comstep 4)
            if(ComutationMode == 2){
              ComStep = 6;
              PWMpulseNumInComm=0;
              commutate(6,DutyCycle);
            }
        }
      }     
    } 
}

void commutate(int step, int DutyCycle){
  PORTF=0x01;PORTF=0x00; //everytime we change comutation we bang
  int mappedDutyCycle = map (DutyCycle, 0,100,0,255);
  switch (step){
    case 0:
      OCR4A=0x00;OCR4B=0x00;OCR4D=0x00;digitalWrite(5,HIGH);digitalWrite(9,HIGH);digitalWrite(12,HIGH);TIMSK4 = 0x00;break;
    case 1:   //coil AB, C is free
      OCR4A=mappedDutyCycle;OCR4B=0x00;OCR4D=0x00;digitalWrite(5,HIGH);digitalWrite(9,LOW);digitalWrite(12,HIGH);TIMSK4 = 0x40;break; //TIMSK4 is D-A-B-x
    case 2:   //Coil AC, B is free
      OCR4A=mappedDutyCycle;OCR4B=0x00;OCR4D=0x00;digitalWrite(5,HIGH);digitalWrite(9,HIGH);digitalWrite(12,LOW);TIMSK4 = 0x40;break;
    case 3:   //Coil BC, A is free
      OCR4A=0x00;OCR4B=mappedDutyCycle;OCR4D=0x00;digitalWrite(5,HIGH);digitalWrite(9,HIGH);digitalWrite(12,LOW);TIMSK4 = 0x20;break;
    case 4:   //Coild BA, C is free
      OCR4A=0x00;OCR4B=mappedDutyCycle;OCR4D=0x00;digitalWrite(5,LOW);digitalWrite(9,HIGH);digitalWrite(12,HIGH);TIMSK4 = 0x20;break;
    case 5:   //Coil CA, B is free
      OCR4A=0x00;OCR4B=0x00;OCR4D=mappedDutyCycle;digitalWrite(5,LOW);digitalWrite(9,HIGH);digitalWrite(12,HIGH);TIMSK4 = 0x80;break;
    case 6:   //Coil CB, A is free
       OCR4A=0x00;OCR4B=0x00;OCR4D=mappedDutyCycle;digitalWrite(5,HIGH);digitalWrite(9,LOW);digitalWrite(12,HIGH);TIMSK4 = 0x80;break;                                  
    default:
      break;
  }
}

void EndTimer0(){
  TCCR0B = (TCCR0B & 0b11111000);   //this sets the clock prescalar to 0 which stops the timer.
}

void StartTimer0(int microseconds){
  //set up timer for 6ms dwelltime
OCR0A = floor(microseconds/33) + 1;
TCCR0A = _BV(WGM01);      //CTC
TIMSK0 = _BV(OCIE0A);   //interrupt is enabled
TCCR0B = _BV(CS02);    //CSn2 sets prescalar as 1024. That's 33us per timer increment (so resolution) with max of 33us * 0xFF = 8.44ms
}

void SetupPWMRegs(){

//set up the timers for 128us period pwm.
TCCR4D = _BV(WGM40);     //0x01. phase correct PWM mode please.
TCCR4C = _BV(COM4D1) | _BV(PWM4D);  //D timer counters will appear on the pin
TCCR4A = _BV(COM4A1) | _BV(PWM4A) | _BV(COM4B1) | _BV(PWM4B);  //A  timer counters will appear on the pin
TCCR4B = _BV(CS41);     // prescale clock by half to acheive 128us period
OCR4C = 0xFF; //sets the TOP of the counter.
OCR4A = 0x00; //sets when the timer counter trips
OCR4B = 0x00; //sets when the timer counter trips
OCR4D = 0x00; //sets when the timer counter trips

}
void ResetTheRegs(){
  TCCR1A=0;
  TCCR1B=0;
  TCCR1C=0;
  TIMSK1=0;
  TCNT1=0;
  OCR1A=0;
  
  TCCR0A=0;
  TCCR0B=0;
  TIMSK0=0;
  TCNT0=0;
  OCR0A=0;
  
  TCCR4A=0;
  TCCR4B=0;
  TCCR4C=0;
  TCCR4D=0;
  OCR4A=0;
  OCR4B=0;
  OCR4C=0;
  OCR4D=0;
  TIMSK4=0;
  
}