Anders Skivspelare

main.ino

//This is a code made to run 3-phase PWM drive stage with 6-transistors, in other words 3 half bridge transistors with an Arduino UNO 
//This setup is made with User settings of DEAD TIME. So here the the dead time can be adjusted by user. const int dead_time = 10;//10 = 0.625 us
//The PWM work here with fix frequancy of 31.25kHz (Other settings may not work proper when use all 3 TIMERS, if you want more flexibilty use Arduino MEGA 2560 or some other plattform)
//Olle Welin [email protected]

// TL Transistor Low  side of half bridge pair
// TH Transistor High side of half bridge pair

// PWM assignement at Arduino UNO is by following:
//~6  TL, PH0, TMR0, pwm_ph0 data value
//~5  TH, PH0, TMR0, pwm_ph0 data value
//~9  TL, PH1, TMR1, pwm_ph1 data value
//~10 TH, PH1, TMR1, pwm_ph1 data value
//~11 TL, PH2, TMR2, pwm_ph2 data value
//~3  TH, PH2, TMR2, pwm_ph2 data value
#include <math.h>
//#define USE_PRINT_HALL //Turn OFF this switch this so save CPU time and get CORRECT SPEED
//#define USE_SINUS_PRINT_TABLE
void setup()
{
  pinMode(2, OUTPUT);   //Frequance Indicator
  pinMode(3, INPUT);   //PWM is set to input before PWM initialized to prevent start up short circuit
  pinMode(5, INPUT);   //PWM is set to input before PWM initialized to prevent start up short circuit
  pinMode(6, INPUT);   //PWM is set to input before PWM initialized to prevent start up short circuit
  pinMode(9, INPUT);   //PWM is set to input before PWM initialized to prevent start up short circuit
  pinMode(10, INPUT);   //PWM is set to input before PWM initialized to prevent start up short circuit
  pinMode(11, INPUT);   //PWM is set to input before PWM initialized to prevent start up short circuit
  pinMode(4, INPUT);    //HALL state 
  pinMode(7, INPUT);    //HALL state
  pinMode(8, INPUT);    //HALL state
  pinMode(12, OUTPUT);  //HALL OK
  pinMode(13, OUTPUT);  //SYNCED ROTOR LED "L" on Arduino. When this LED is Light then it tell that the rotor is in sync with HALL
  
  // start serial port at 9600 bps:
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for Leonardo only
  }
}
const int dead_time = 2;//10 = 0.625 us
const int half_dead_time = dead_time / 2;//
const int HALL_ERROR = 6;
int H_sens = 0;
int pwm_ph0=0;
int pwm_ph1=0;
int pwm_ph2=0;

inline void update_pwm(void)
{
 //Limit the pwm_phx so it take the dead time into account so it not will swap over when reach end limit of workable PWM range
 if(pwm_ph0 > (255 - half_dead_time))
 {
   pwm_ph0 = (255 - half_dead_time);
 }
 if(pwm_ph1 > (255 - half_dead_time))
 {
   pwm_ph1 = (255 - half_dead_time);
 }
 if(pwm_ph2 > (255 - half_dead_time))
 {
   pwm_ph2 = (255 - half_dead_time);
 }

 if(pwm_ph0 < half_dead_time)
 {
   pwm_ph0 = half_dead_time;
 }
 if(pwm_ph1 < half_dead_time)
 {
   pwm_ph1 = half_dead_time;
 }
 if(pwm_ph2 < half_dead_time)
 {
   pwm_ph2 = half_dead_time;
 }

 //All The PWM will run from 0xFF to 0x00 and up again to TOP 0xFF
 //The OCRnx will automatic update when TCNTn is on TOP 0xFF by hardware
 //Therefor it is sutible to change both OCRnx pair on a place where TCNT not update to ensure that the hardware automatic update NOT occur exact when only one of the two OCRnx have changed by this code
 while(TCNT0 > 0x7F)
 {
 }
 //Now it's safe to update OCR0x pair (connect to same transistor pair)
  OCR0A = (pwm_ph0 + half_dead_time);//~6 TL PH0 TMR0
  OCR0B = (pwm_ph0 - half_dead_time);//~5 TH PH0 TMR0
 while(TCNT1 > 0x7F)
 {
 }
 //Now it's safe to update OCR1x pair (connect to same transistor pair)
  OCR1A = (pwm_ph1 + half_dead_time);//~9 TL PH1 TMR1
  OCR1B = (pwm_ph1 - half_dead_time);//~10 TH PH1 TMR1
 while(TCNT2 > 0x7F)
 {
 }
 //Now it's safe to update OCR2x pair (connect to same transistor pair)
  OCR2A = (pwm_ph2 + half_dead_time);//~3 TH PH2 TMR2
  OCR2B = (pwm_ph2 - half_dead_time);//~11 TL PH2 TMR2
}
inline void sync_pwm_timers(void)
{
  GTCCR = (GTCCR & 0xFF) | 0x81;// Set bit Bit 7 – TSM: Timer/Counter Synchronization Mode
  //Writing the TSM bit to one activates the Timer/Counter Synchronization mode. In this mode, the value that is written
  //to the PSRASY and PSRSYNC bits is kept, hence keeping the corresponding prescaler reset signals asserted.
  //This ensures that the corresponding Timer/Counters are halted and can be configured to the same value without
  //the risk of one of them advancing during configuration. When the TSM bit is written to zero, the PSRASY and
  //PSRSYNC bits are cleared by hardware, and the Timer/Counters start counting simultaneously.
  TCNT0 = 00;//Syncronize PWM timers
  TCNT1 = 0x0000;//Syncronize PWM timers
  TCNT2 = 0x00;//Syncronize PWM timers
  GTCCR = (GTCCR & 0x7E);// Rest bit Bit 7 – TSM: Timer/Counter Synchronization Mode
}

inline void wait_TCN(void)
{
    //**************************************************************************************
    ///This two while wait will create a poll to create a Fix loop frequense of 31.25kHz
    while(TCNT0 < 0xEF)
    {
    }
    while(TCNT0 > 0xEF)
    {
    }
    /// End of poll of Fix loop frequense 31.25kHz
    //**************************************************************************************
}
inline int circulate_value(int table_index, int table_size)
{
  int circ_return=0;
    if(table_index > table_size-1)
    {
      table_index = table_index - table_size;
    }
    if(table_index < 0)
    {
      table_index = table_index + table_size;
    }
    circ_return = table_index;
  return circ_return;
}
inline int read_hall(void)
{
  
  if(digitalRead(4) == 1)
  {
    H_sens = H_sens | 0b00000001;
  }
  else
  {
    H_sens = H_sens & 0b11111110;
  }
  if(digitalRead(7) == 1)
  {
    H_sens = H_sens | 0b00000010;
  }
  else
  {
    H_sens = H_sens & 0b11111101;
  }
  if(digitalRead(8) == 1)
  {
    H_sens = H_sens | 0b00000100;
  }
  else
  {
    H_sens = H_sens & 0b11111011;
  }
  wait_TCN(); 

  int hall=0;
  if (H_sens == 0b00000111)
  {
    Serial.println("HALL ERROR!! ALL 111");
    digitalWrite(12, LOW);///Hall Failure
    hall = HALL_ERROR;
  }
  else
  {
    if (H_sens == 0b00000000)
    {
     Serial.println("HALL ERROR!! ALL 000");
     digitalWrite(12, LOW);///Hall Failure
     hall = HALL_ERROR;
    }
    else
    {
      digitalWrite(12, HIGH);///Hall OK
      
    }
  }
  if(H_sens == 0b00000101)
  {
    hall = 5;//0
  }
  if(H_sens == 0b00000001)
  {
    hall = 4;//1
  }
  if(H_sens == 0b00000011)
  {
    hall = 3;//2
  }
  if(H_sens == 0b00000010)
  {
    hall = 2;//3
  }
  if(H_sens == 0b00000110)
  {
    hall = 1;//4
  }
  if(H_sens == 0b00000100)
  {
    hall = 0;//5
  }
wait_TCN(); 
  
  return hall;
}
void loop()
{
Serial.println("Version 1.0");
  TCCR1A = 0xE1;//PWM, Phase Correct 8-bit TOP at 0xFF 
  TCCR0A = 0xE1;//PWM, Phase Correct 8-bit TOP at 0xFF 
  TCCR2A = 0xE1;//PWM, Phase Correct 8-bit TOP at 0xFF 
  TCCR2B = 0x01;//clkI/O/1 (No prescaling) 31.25kHz PWM frequency
  TCCR1B = 0x01;//clkI/O/1 (No prescaling) 31.25kHz PWM frequency
  TCCR0B = 0x01;//clkI/O/1 (No prescaling) 31.25kHz PWM frequency
  sync_pwm_timers();

  pwm_ph0=128;//128 = center. User PWM Range is value between (0 + half_dead_time) to (255 - half_dead_time)
  pwm_ph1=128;
  pwm_ph2=128;
  update_pwm();
  pinMode(3, OUTPUT);   //PWM output
  pinMode(5, OUTPUT);   //PWM output
  pinMode(6, OUTPUT);   //PWM output
  pinMode(9, OUTPUT);   //PWM output
  pinMode(10, OUTPUT);   //PWM output
  pinMode(11, OUTPUT);   //PWM output
  pinMode(2, OUTPUT);   //Frequance Indicator 
  pinMode(4, INPUT);    //HALL state 
  pinMode(7, INPUT);    //HALL state
  pinMode(8, INPUT);    //HALL state
  pinMode(12, OUTPUT);  //HALL OK
  pinMode(13, OUTPUT);  //SYNCED ROTOR LED "L" on Arduino. When this LED is Light then it tell that the rotor is in sync with HALL
  
digitalWrite(13, LOW);//Not Sync

  const float rps = 0.555555555556f;/// 100 turn / 180 sec
  const int table_size = 300;
  
 // float rot_step = 0.128f;/// (12 poles / 2) * rps * table_size / (31250Hz / 4)
 // float frequence = 7812.5;
  float rot_step = 0.256f;/// (12 poles / 2) * rps * table_size / (31250Hz / 8)
  float frequence = 3906.25;

  rot_step = 6 * rps * table_size / frequence;
  Serial.print("rot_step = ");
  Serial.println(rot_step, 8);
  const float two_pi = 6.28318530718;
  float rot_counter=0;
  float sinus=0;
  float pwm_max = 100;//Must be less then 128
  const float deg_120 = 2.09439510239f;///(1/3) * 2pi
  const float deg_240 = 4.18879020479f;///(2/3) * 2pi
//  const float rewind_counter_value = two_pi / 10;///How much will the comutation jump back or fourth if off track ocurre
  const float rewind_counter_value = rot_step * 1.2f;
  const int table_one_third = table_size / 3;
  const int table_one_sixth = table_size / 6;
  const int table_size_one_half = table_size / 2;
  const int max_offtrack_ang = table_size / 10;
  const int hall_rot_offset = 200;///0..table_size-1 Change this offset so it match the offset between the real magnetic feald and the HALL sensor based signal
  int off_track = 0;///0 = ON track. -1 = OFF track lagging. 1 = OFF track to ahead.
  int table[table_size];
  int hall_state=0;//0..5 state
  //Make sinus integer table
  for(int i = 0;i<table_size;i++)
  {
    sinus = sin(two_pi * i/table_size);///
    sinus = sinus * pwm_max; 
    sinus = sinus + 128.0f;
    table[i] = sinus; 
#ifdef USE_SINUS_PRINT_TABLE
    Serial.print("table[");
    Serial.print(i);
    Serial.print("] = ");
    Serial.println(table[i]);
#endif
  }
  int table_index1=0;
  int table_index2=0;
  int table_index3=0;
//  const long ramp_sec = 60;
  const long ramp_sec = 3;
  const float rot_step_ramp_inc = rot_step / (((float) frequence) * ((float)ramp_sec));

  Serial.print("rot_step_ramp_inc = ");
  Serial.println(rot_step_ramp_inc, 8);
  Serial.print("hall_rot_offset = ");
  Serial.println(hall_rot_offset);
  float rot_step_ramp = 0;
  long ramp_counter = 0;
  ramp_counter = (long) frequence;
  Serial.print("ramp_counter = ");
  Serial.println(ramp_counter);

  ramp_counter = ramp_counter * ramp_sec;
  Serial.print("ramp_counter = ");
  Serial.println(ramp_counter);
int estimated_rot_c = 0;///This is based on value from HALL sensor
int rot_c_diff = 0;///This is a circulate value 
int test_max = 0;

int print_counter =0;  
  while(1)
  {  
    digitalWrite(2, LOW);
    wait_TCN();    
    digitalWrite(2, HIGH); 
    if(rot_counter < table_size-1)
    {
      rot_counter = rot_counter + rot_step;
    }
    else
    {
      rot_counter = 0;
    }
    table_index1 = (int) rot_counter;  
    table_index1 = circulate_value(table_index1, table_size);//Make to a circulate value inside 0..to table_size-1 range
    table_index2 = table_index1 + table_one_third;
    wait_TCN();    
    table_index3 = table_index1 + table_one_third + table_one_third;

    table_index2 = circulate_value(table_index2, table_size);//Make to a circulate value inside 0..to table_size-1 range
    table_index3 = circulate_value(table_index3, table_size);//Make to a circulate value inside 0..to table_size-1 range
    pwm_ph2 = table[table_index1];
    pwm_ph1 = table[table_index2];
    pwm_ph0 = table[table_index3];
    wait_TCN();
    hall_state = read_hall();
 
    wait_TCN();    
    estimated_rot_c = hall_state * table_one_sixth + hall_rot_offset;
    estimated_rot_c = circulate_value(estimated_rot_c, table_size);//Make to a circulate value inside 0..to table_size-1 range
 
    rot_c_diff = table_index1 - estimated_rot_c;//Circulate value system
    rot_c_diff = circulate_value(rot_c_diff, table_size);//Make to a circulate value inside 0..to table_size-1 range

    wait_TCN(); 
    digitalWrite(13, LOW);//Turn off "OFF track" LED
    if(rot_c_diff < table_size_one_half)
    {
      ///The estimated_rot_c from HALL sensor is lagging behind the output PWM sinus wave
      if(rot_c_diff > max_offtrack_ang)
      {
        ///Off track, to much lagging of the motor
       // Serial.println("lagging ");
 
        digitalWrite(13, HIGH);//OFF track LED
 
        rot_counter = rot_counter - rewind_counter_value;///Rewind the rot_counter
      }
    }
    else
    {
       ///The estimated_rot_c from HALL sensor go before the output PWM sinus wave
      if(rot_c_diff < (table_size_one_half - max_offtrack_ang))
      {
        ///Off track, to far ahead on the motor
        Serial.println("Off track, to far ahead");
        digitalWrite(13, HIGH);//OFF track LED
       // rot_counter = rot_counter + rewind_counter_value;///Forward the rot_counter
        rot_counter = 0.0f;
    pwm_ph2 = 10;
    pwm_ph1 = 10;
    pwm_ph0 = 10;
            update_pwm();

    delay(10000);
      }
    }

     
    wait_TCN();    
    update_pwm();

#ifdef USE_PRINT_HALL
    if(print_counter > 2)
    {
      Serial.println(hall_state);
      
      print_counter = 0;
    }
    else
    {
      print_counter++;
    }
    if(print_counter == 1 )
    {
   // Serial.println(rot_counter);
    }
#endif
  }
}