arduino pwm output frequencys

ronco · Post by **ronco** » Mon Jan 02, 2012 5:31 pm

Hi,

i made some code to change timer 1 and 2's frequencys..
with this we can use higher or lower frequencys as 488Hz fore each pin.

for example .. i use some jamara 6 A ESCs now with 550Hz and it seems to work well

maybe there are also other applications where this could be usefull

for now it works only for pro mini...

to use it you need to delete or uncomment the writeMotors function in Output and put this in setup:

Code: Select all

  #if defined(PROMINI)
    init_Soft_PWM();
  #endif

and heres the code

.. save it to a new file in MWC directory or put it into Output

Code: Select all


#define SOFT_PWM_MOTORS // to activate

#define PWM_0_PIN       9
#define PWM_0_PIN_HIGH  PORTB |= 1<<1;
#define PWM_0_PIN_LOW   PORTB &= ~(1<<1);

#define PWM_1_PIN       10
#define PWM_1_PIN_HIGH  PORTB |= 1<<2;
#define PWM_1_PIN_LOW   PORTB &= ~(1<<2);

#define PWM_2_PIN       11
#define PWM_2_PIN_HIGH  PORTB |= 1<<3;
#define PWM_2_PIN_LOW   PORTB &= ~(1<<3);

#define PWM_3_PIN       3
#define PWM_3_PIN_HIGH  PORTD |= 1<<3;
#define PWM_3_PIN_LOW   PORTD &= ~(1<<3);


// pwm pulse values
int16_t soft_PWM[4] = {125,125,125,125};

/*
PWM Frequency
1  = 3,906 kHz
2  = 1,953 kHz
3  = 1,3 kHz
4  = 976,5 Hz
5  = 781,2 Hz
6  = 651 Hz
7  = 558 Hz
8  = 488,2 Hz // default
10 = 390,6 Hz
20 = 195,3 Hz
40 = 97,6 Hz
80 = 48,8 Hz
 x = 3906/x hz 
*/
int8_t soft_PWM_Frequency[4] = {7,7,7,7};


void writeMotors() { //[1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #endif
  #if defined(PROMINI)
    for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
      #if defined(SOFT_PWM_MOTORS)
        soft_PWM[i] = motor[i]>>3;
      #else
        analogWrite(PWM_PIN[i], motor[i]>>3);
      #endif
  #endif
}

// init
void init_Soft_PWM(){
  #if defined(PROMINI) && NUMBER_MOTOR > 0 && defined(SOFT_PWM_MOTORS)
    pinMode(PWM_0_PIN, OUTPUT);
    TCCR2A = 0;
    //Timer 2 to 4kHz
    TCCR2B = TCCR2B & 0b11111000 | 0x02;
    TIMSK2 |= (1<<OCIE2A);
    #if NUMBER_MOTOR > 1
      pinMode(PWM_1_PIN, OUTPUT);
      TIMSK2 |= (1<<OCIE2B);
    #endif
    
    #if NUMBER_MOTOR > 2
      pinMode(PWM_2_PIN, OUTPUT);
      TCCR1A = 0;
      //Timer 1 to 4kHz
      TCCR1B = TCCR1B & 0b11111000 | 0x02;
      TIMSK1 |= (1<<OCIE1A); 
      #if NUMBER_MOTOR > 3
        pinMode(PWM_3_PIN, OUTPUT);
        TIMSK1 |= (1<<OCIE1B);
      #endif
    #endif
  #endif  
}

#if defined(PROMINI) && NUMBER_MOTOR > 0 && defined(SOFT_PWM_MOTORS)
  ISR(TIMER2_COMPA_vect) {
    static uint8_t divi = 0;
    static uint8_t state = 0;
    if(state == 0){
      divi = soft_PWM_Frequency[0]*2;
      PWM_0_PIN_HIGH;
      OCR2A   += soft_PWM[0];
      state = 1;
    }else if(state > 0 && state < divi){
      OCR2A   += soft_PWM[0];
      state ++;
    }else if(state == divi){
      PWM_0_PIN_LOW;
      OCR2A   += (255-soft_PWM[0]);
      state = (divi+1); 
    }else if(state > divi){
      OCR2A   += (255-soft_PWM[0]);
      state ++;
      if(state == (divi*2)){
         state = 0;
      }
    } 
  }
  #if NUMBER_MOTOR > 1
    ISR(TIMER2_COMPB_vect) {
      static uint8_t divi = 0;
      static uint8_t state = 0;
      if(state == 0){
        divi = soft_PWM_Frequency[1]*2;
        PWM_1_PIN_HIGH;
        OCR2B   += soft_PWM[1];
        state = 1;
      }else if(state > 0 && state < divi){
        OCR2B   += soft_PWM[1];
        state ++;
      }else if(state == divi){
        PWM_1_PIN_LOW
        OCR2B   += (255-soft_PWM[1]);
        state = (divi+1); 
      }else if(state > divi){
        OCR2B   += (255-soft_PWM[1]);
        state ++;
        if(state == (divi*2)){
           state = 0;
        }
      } 
    }
  #endif
  #if NUMBER_MOTOR > 2
    ISR(TIMER1_COMPA_vect) {
      static uint8_t divi = 0;
      static uint8_t state = 0;
      if(state == 0){
        divi = soft_PWM_Frequency[2]*2;
        PWM_2_PIN_HIGH;
        OCR1A   += soft_PWM[2];
        state = 1;
      }else if(state > 0 && state < divi){
        OCR1A   += soft_PWM[2];
        state ++;
      }else if(state == divi){
        PWM_2_PIN_LOW;
        OCR1A   += (255-soft_PWM[2]);
        state = (divi+1); 
      }else if(state > divi){
        OCR1A   += (255-soft_PWM[2]);
        state ++;
        if(state == (divi*2)){
           state = 0;
        }
      } 
    }
  #endif
  #if NUMBER_MOTOR > 3 
    ISR(TIMER1_COMPB_vect) {
      static uint8_t divi = 0;
      static uint8_t state = 0;
      if(state == 0){
        divi = soft_PWM_Frequency[3]*2;
        PWM_3_PIN_HIGH;
        OCR1B   += soft_PWM[3];
        state = 1;
      }else if(state > 0 && state < divi){
        OCR1B   += soft_PWM[3];
        state ++;
      }else if(state == divi){
        PWM_3_PIN_LOW;
        OCR1B   += (255-soft_PWM[3]);
        state = (divi+1); 
      }else if(state > divi){
        OCR1B   += (255-soft_PWM[3]);
        state ++;
        if(state == (divi*2)){
           state = 0;
        }
      } 
    }
  #endif
#endif

regards Felix

Alexinparis · Post by **Alexinparis** » Mon Jan 02, 2012 9:05 pm

Hi,

It's a nice exercise

One question: a PPM signal is based on the high state witch should last between 1000 and 2000 microseconds.
In this case, I don't think you can go higher than 500Hz, except if you don't use the highest position (2000).
With a limit of 1850 in the code, I think it's ok.

ronco · Post by **ronco** » Mon Jan 02, 2012 11:20 pm

Hi Alex,

i dont made much tests with it but as i said .. it seems to work with 550hz ..

i work on a nother method atm.. if i get it to work we can pulse 8 pins with timer 2 with a resolution ~500 and 4xx hz

we will see...

regards Felix

Th0rsten · Post by **Th0rsten** » Tue Jan 03, 2012 12:03 am

With 550 Hz the time of oscillation is 0,001818 sec.
You need 0,002000 sec. for max power on the ESC = 500 Hz is the maximum.
So it's not working.
Calculation is: 1/hz = time.

ronco · Post by **ronco** » Tue Jan 03, 2012 11:49 am

Th0rsten wrote:With 550 Hz the time of oscillation is 0,001818 sec.
You need 0,002000 sec. for max power on the ESC = 500 Hz is the maximum.
So it's not working.
Calculation is: 1/hz = time.

i see.. i noticed that max throttle felt lower with 550hz :S

.. if i get it to work we can pulse 8 pins with timer 2 with a resolution ~500 and 4xx hz

.. it dosent work :/

i tried this with

Code: Select all

  TCCR2A = 0;  // normal counting mode 
  TCCR2B = TCCR2B & 0b11111000 | 0x1; // set it to 32kHz
  TCNT2 = 0;     // clear the timer2 count 
  TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt

and

Code: Select all

ISR(TIMER2_OVF_vect) {
  TCNT2 = xxx;
}

but it seems to have conflicts with MWC code.

so i modified the code from first post to have 488hz and a usable resolution of 200..

i pulse it a little less the half time high before i use the pulse width values. it is 200 not 255 because if we have a value close to 0 it becomes inaccurate.

now we have [1000;2000] => [0;200] instead of [1000;2000] => [125;250] .. my indoor quad flyes well with it

code:

Code: Select all


#define SOFT_PWM_MOTORS // to activate

#define PWM_0_PIN       9
#define PWM_0_PIN_HIGH  PORTB |= 1<<1;
#define PWM_0_PIN_LOW   PORTB &= ~(1<<1);

#define PWM_1_PIN       10
#define PWM_1_PIN_HIGH  PORTB |= 1<<2;
#define PWM_1_PIN_LOW   PORTB &= ~(1<<2);

#define PWM_2_PIN       11
#define PWM_2_PIN_HIGH  PORTB |= 1<<3;
#define PWM_2_PIN_LOW   PORTB &= ~(1<<3);

#define PWM_3_PIN       3
#define PWM_3_PIN_HIGH  PORTD |= 1<<3;
#define PWM_3_PIN_LOW   PORTD &= ~(1<<3);


// pwm pulse values
int8_t soft_PWM[4] = {0,0,0,0};

void writeMotors() { //[1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #endif
  #if defined(PROMINI)
    for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
      #if defined(SOFT_PWM_MOTORS)
        soft_PWM[i] = (motor[i]-1000)/5;  //[1000;2000] => [0;200]
      #else
        analogWrite(PWM_PIN[i], motor[i]>>3);
      #endif
  #endif
}

// init
void init_Soft_PWM(){
  defined(SOFT_PWM_MOTORS)
    pinMode(PWM_0_PIN, OUTPUT);
    TCCR2A = 0;
    //Timer 2 to 4kHz
    TCCR2B = TCCR2B & 0b11111000 | 0x02;
    TIMSK2 |= (1<<OCIE2A);
    #if NUMBER_MOTOR > 1
      pinMode(PWM_1_PIN, OUTPUT);
      TIMSK2 |= (1<<OCIE2B);
    #endif
    
    #if NUMBER_MOTOR > 2
      pinMode(PWM_2_PIN, OUTPUT);
      TCCR1A = 0;
      //Timer 1 to 4kHz
      TCCR1B = TCCR1B & 0b11111000 | 0x02;
      TIMSK1 |= (1<<OCIE1A); 
      #if NUMBER_MOTOR > 3
        pinMode(PWM_3_PIN, OUTPUT);
        TIMSK1 |= (1<<OCIE1B);
      #endif
    #endif
  #endif  
}

defined(SOFT_PWM_MOTORS)
  ISR(TIMER2_COMPA_vect) {
    static uint8_t wait_counter = 0;
    static uint8_t state = 0;
    
    if(wait_counter < 6){
      if(wait_counter == 0){
        PWM_0_PIN_HIGH;
      }
      OCR2A += 255;
      wait_counter++;
    }else if(wait_counter == 6){
      OCR2A += 255;
      wait_counter++;    
    }
    
    if(state == 0 && wait_counter > 6){
      OCR2A += soft_PWM[0]+40;
      state = 1;
    }else if(state > 0 && state < 9){
      OCR2A += soft_PWM[0]+40;
      state ++;
    }else if(state == 9){
      PWM_0_PIN_LOW;
      OCR2A += 215-soft_PWM[0];
      state = (10); 
    }else if(state >= 9){
      OCR2A += 215-soft_PWM[0];
      state ++;
      if(state > 18){
         state = 0;
         wait_counter = 0;
      }
    } 
  }
  #if NUMBER_MOTOR > 1
    ISR(TIMER2_COMPB_vect) {
      static uint8_t wait_counter = 0;
      static uint8_t state = 0;
      
      if(wait_counter < 6){
        if(wait_counter == 0){
          PWM_1_PIN_HIGH;
        }
        OCR2B += 255;
        wait_counter++;
      }else if(wait_counter == 6){
        OCR2B += 255;
        wait_counter++;    
      }
      
      if(state == 0 && wait_counter > 6){
        OCR2B += soft_PWM[1]+40;
        state = 1;
      }else if(state > 0 && state < 9){
        OCR2B += soft_PWM[1]+40;
        state ++;
      }else if(state == 9){
        PWM_1_PIN_LOW;
        OCR2B += 215-soft_PWM[1];
        state = (10); 
      }else if(state >= 9){
        OCR2B += 215-soft_PWM[1];
        state ++;
        if(state > 18){
           state = 0;
           wait_counter = 0;
        }
      } 
    }
  #endif
  #if NUMBER_MOTOR > 2
    ISR(TIMER1_COMPA_vect) {
      static uint8_t wait_counter = 0;
      static uint8_t state = 0;
      
      if(wait_counter < 6){
        if(wait_counter == 0){
          PWM_2_PIN_HIGH;
        }
        OCR1A += 255;
        wait_counter++;
      }else if(wait_counter == 6){
        OCR1A += 255;
        wait_counter++;    
      }
      
      if(state == 0 && wait_counter > 6){
        OCR1A += soft_PWM[2]+40;
        state = 1;
      }else if(state > 0 && state < 9){
        OCR1A += soft_PWM[2]+40;
        state ++;
      }else if(state == 9){
        PWM_2_PIN_LOW;
        OCR1A += 215-soft_PWM[2];
        state = (10); 
      }else if(state >= 9){
        OCR1A += 215-soft_PWM[2];
        state ++;
        if(state > 18){
           state = 0;
           wait_counter = 0;
        }
      } 
    }
  #endif
  #if NUMBER_MOTOR > 3 
    ISR(TIMER1_COMPB_vect) {
      static uint8_t wait_counter = 0;
      static uint8_t state = 0;
      
      if(wait_counter < 6){
        if(wait_counter == 0){
          PWM_3_PIN_HIGH;
        }
        OCR1B += 255;
        wait_counter++;
      }else if(wait_counter == 6){
        OCR1B += 255;
        wait_counter++;    
      }
      
      if(state == 0 && wait_counter > 6){
        OCR1B += soft_PWM[3]+40;
        state = 1;
      }else if(state > 0 && state < 9){
        OCR1B += soft_PWM[3]+40;
        state ++;
      }else if(state == 9){
        PWM_3_PIN_LOW;
        OCR1B += 215-soft_PWM[3];
        state = (10); 
      }else if(state >= 9){
        OCR1B += 215-soft_PWM[3];
        state ++;
        if(state > 18){
           state = 0;
           wait_counter = 0;
        }
      } 
    }
  #endif
#endif

regards Felix

ziss_dm · Post by **ziss_dm** » Tue Jan 03, 2012 11:39 pm

Hi Felix,

Just couple of notes

1) It is possible to re-flash escs to accept range [2;252] and still use HW PWM:
viewtopic.php?f=13&t=516
2) It is looks like, it is possible to extend resolution of the PWM, using sigma-delta modulation:
viewtopic.php?f=7&t=797

regards,
ziss_dm

ronco · Post by **ronco** » Wed Jan 04, 2012 7:50 pm

hi, ziss

do you know the resolution of normal ESCs ?

i did some tests now .. it seems to work well

... but it slowes the cycletime for arround 200

i also made some fine tuning to the range .. now it starts with ~1010 with a value of 0..

Code: Select all

#define SOFT_PWM_MOTORS // to activate

#define PWM_0_PIN       9
#define PWM_0_PIN_HIGH  PORTB |= 1<<1;
#define PWM_0_PIN_LOW   PORTB &= ~(1<<1);

#define PWM_1_PIN       10
#define PWM_1_PIN_HIGH  PORTB |= 1<<2;
#define PWM_1_PIN_LOW   PORTB &= ~(1<<2);

#define PWM_2_PIN       11
#define PWM_2_PIN_HIGH  PORTB |= 1<<3;
#define PWM_2_PIN_LOW   PORTB &= ~(1<<3);

#define PWM_3_PIN       3
#define PWM_3_PIN_HIGH  PORTD |= 1<<3;
#define PWM_3_PIN_LOW   PORTD &= ~(1<<3);


// pwm pulse values
int16_t soft_PWM[4] = {0,0,0,0};

void writeMotors() { //[1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #endif
  #if defined(PROMINI)
    for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
      #if defined(SOFT_PWM_MOTORS)
        soft_PWM[i] = (motor[i]-1000)/5;  //[1000;2000] => [0;200]
      #else
        analogWrite(PWM_PIN[i], motor[i]>>3);
      #endif
  #endif
}

// init
void init_Soft_PWM(){
  #if defined(SOFT_PWM_MOTORS)
    pinMode(PWM_0_PIN, OUTPUT);
    TCCR2A = 0;
    //Timer 2 to 4kHz
    TCCR2B = TCCR2B & 0b11111000 | 0x02;
    TIMSK2 |= (1<<OCIE2A);
    #if NUMBER_MOTOR > 1
      pinMode(PWM_1_PIN, OUTPUT);
      TIMSK2 |= (1<<OCIE2B);
    #endif
    
    #if NUMBER_MOTOR > 2
      pinMode(PWM_2_PIN, OUTPUT);
      TCCR1A = 0;
      //Timer 1 to 4kHz
      TCCR1B = TCCR1B & 0b11111000 | 0x02;
      TIMSK1 |= (1<<OCIE1A); 
      #if NUMBER_MOTOR > 3
        pinMode(PWM_3_PIN, OUTPUT);
        TIMSK1 |= (1<<OCIE1B);
      #endif
    #endif
  #endif  
}

#if defined(SOFT_PWM_MOTORS)
  ISR(TIMER2_COMPA_vect) {
    static uint8_t wait_counter = 0;
    static uint8_t state = 0;
    
    if(wait_counter < 6){
      if(wait_counter == 0){
        PWM_0_PIN_HIGH;
      }
      OCR2A += 255;
      wait_counter++;
    }else if(wait_counter == 6){
      OCR2A += 127;
      wait_counter++;    
    }
    
    if(state == 0 && wait_counter > 6){
      OCR2A += soft_PWM[0]+40;
      state = 1;
    }else if(state > 0 && state < 9){
      OCR2A += soft_PWM[0]+40;
      state ++;
    }else if(state == 9){
      PWM_0_PIN_LOW;
      OCR2A += 215-soft_PWM[0];
      state = (10); 
    }else if(state >= 9){
      OCR2A += 215-soft_PWM[0];
      state ++;
      if(state > 18){
         state = 0;
         wait_counter = 0;
      }
    } 
  }
  #if NUMBER_MOTOR > 1
    ISR(TIMER2_COMPB_vect) {
      static uint8_t wait_counter = 0;
      static uint8_t state = 0;
      
      if(wait_counter < 6){
        if(wait_counter == 0){
          PWM_1_PIN_HIGH;
        }
        OCR2B += 255;
        wait_counter++;
      }else if(wait_counter == 6){
        OCR2B += 127;
        wait_counter++;    
      }
      
      if(state == 0 && wait_counter > 6){
        OCR2B += soft_PWM[1]+40;
        state = 1;
      }else if(state > 0 && state < 9){
        OCR2B += soft_PWM[1]+40;
        state ++;
      }else if(state == 9){
        PWM_1_PIN_LOW;
        OCR2B += 215-soft_PWM[1];
        state = (10); 
      }else if(state >= 9){
        OCR2B += 215-soft_PWM[1];
        state ++;
        if(state > 18){
           state = 0;
           wait_counter = 0;
        }
      } 
    }
  #endif
  #if NUMBER_MOTOR > 2
    ISR(TIMER1_COMPA_vect) {
      static uint8_t wait_counter = 0;
      static uint8_t state = 0;
      
      if(wait_counter < 6){
        if(wait_counter == 0){
          PWM_2_PIN_HIGH;
        }
        OCR1A += 255;
        wait_counter++;
      }else if(wait_counter == 6){
        OCR1A += 127;
        wait_counter++;    
      }
      
      if(state == 0 && wait_counter > 6){
        OCR1A += soft_PWM[2]+40;
        state = 1;
      }else if(state > 0 && state < 9){
        OCR1A += soft_PWM[2]+40;
        state ++;
      }else if(state == 9){
        PWM_2_PIN_LOW;
        OCR1A += 215-soft_PWM[2];
        state = (10); 
      }else if(state >= 9){
        OCR1A += 215-soft_PWM[2];
        state ++;
        if(state > 18){
           state = 0;
           wait_counter = 0;
        }
      } 
    }
  #endif
  #if NUMBER_MOTOR > 3 
    ISR(TIMER1_COMPB_vect) {
      static uint8_t wait_counter = 0;
      static uint8_t state = 0;
      
      if(wait_counter < 6){
        if(wait_counter == 0){
          PWM_3_PIN_HIGH;
        }
        OCR1B += 255;
        wait_counter++;
      }else if(wait_counter == 6){
        OCR1B += 127;
        wait_counter++;    
      }
      
      if(state == 0 && wait_counter > 6){
        OCR1B += soft_PWM[3]+40;
        state = 1;
      }else if(state > 0 && state < 9){
        OCR1B += soft_PWM[3]+40;
        state ++;
      }else if(state == 9){
        PWM_3_PIN_LOW;
        OCR1B += 215-soft_PWM[3];
        state = (10); 
      }else if(state >= 9){
        OCR1B += 215-soft_PWM[3];
        state ++;
        if(state > 18){
           state = 0;
           wait_counter = 0;
        }
      } 
    }
  #endif
#endif

regards Felix

ronco · Post by **ronco** » Fri Jan 06, 2012 10:50 pm

again some news

i found a way to drive 4 ESCs with one timer ... so they are now on default 488Hz and uses a resolution of 245 [1000;2000] => [0;245]

this means we may now drive a octocopter with a pro mini!

so

timer 1 a = motor 1 & 2 // pins 9 & 10
timer 1 b = motor 3 & 4 // pins 11& 3
timer 2 a = motor 5 & 6 // pins 5 & 6 (or A0 & A1)
timer 2 b = motor 7 & 8 // pins A2 & 12 [edit](maybe 4 & 7 if used with sum recivers)[/edit]

to have accurate signals min output is now 1042 that seems to be ok for the most ESCs but minthrottle must be lowed for these 42

i have tested this weith one tri and two quads .. till now everything works fine.

Code: Select all


#define SOFT_PWM_MOTORS // to activate

#define PWM_1_PIN       9
#define PWM_1_PIN_HIGH  PORTB |= 1<<1;
#define PWM_1_PIN_LOW   PORTB &= ~(1<<1);

#define PWM_2_PIN       10
#define PWM_2_PIN_HIGH  PORTB |= 1<<2;
#define PWM_2_PIN_LOW   PORTB &= ~(1<<2);

#define PWM_3_PIN       11
#define PWM_3_PIN_HIGH  PORTB |= 1<<3;
#define PWM_3_PIN_LOW   PORTB &= ~(1<<3);

#define PWM_4_PIN       3
#define PWM_4_PIN_HIGH  PORTD |= 1<<3;
#define PWM_4_PIN_LOW   PORTD &= ~(1<<3);

#if defined(A0_A1_PIN_HEX)
  #define PWM_5_PIN       A0
  #define PWM_5_PIN_HIGH  PORTC |= 1<<0;
  #define PWM_5_PIN_LOW   PORTC &= ~(1<<0);
  
  #define PWM_6_PIN       A1
  #define PWM_6_PIN_HIGH  PORTC |= 1<<1;
  #define PWM_6_PIN_LOW   PORTC &= ~(1<<1);
#else
  #define PWM_5_PIN       5
  #define PWM_5_PIN_HIGH  PORTD |= 1<<5;
  #define PWM_5_PIN_LOW   PORTD &= ~(1<<5);
  
  #define PWM_6_PIN       6
  #define PWM_6_PIN_HIGH  PORTD |= 1<<6;
  #define PWM_6_PIN_LOW   PORTD &= ~(1<<6);
#endif

#define PWM_7_PIN       A2
#define PWM_7_PIN_HIGH  PORTC |= 1<<2;
#define PWM_7_PIN_LOW   PORTC &= ~(1<<2);

#define PWM_8_PIN       12
#define PWM_8_PIN_HIGH  PORTB |= 1<<4;
#define PWM_8_PIN_LOW   PORTB &= ~(1<<4);


// pwm pulse values
int16_t soft_PWM[8] = {0,0,0,0,0,0,0,0};

void writeMotors() { // [1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #else
  
    #if defined(SOFT_PWM_MOTORS)
      for(uint8_t i=0;i<NUMBER_MOTOR;i++)
        soft_PWM[i] = (motor[i]-1000)/4.08; //[1000;2000] => [0;245]
    #else 
      for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
        analogWrite(PWM_PIN[i], motor[i]>>3);
      #if (NUMBER_MOTOR == 5)
        atomicPWM_PIN5_highState = motor[5]/8;
        atomicPWM_PIN5_lowState = 255-atomicPWM_PIN5_highState;
      #endif
      #if (NUMBER_MOTOR == 6)
        atomicPWM_PIN6_highState = motor[4]/8;
        atomicPWM_PIN6_lowState = 255-atomicPWM_PIN6_highState;
      #endif
    #endif
  #endif
}

// init
void init_Soft_PWM(){
  #if defined(SOFT_PWM_MOTORS)
    TCCR1A = 0;
    TIMSK1 |= (1<<OCIE1A);
    #if NUMBER_MOTOR > 2
      TIMSK1 |= (1<<OCIE1B);
    #endif
    #if NUMBER_MOTOR > 4
      TCCR2A = 0;
      TIMSK2 |= (1<<OCIE2A);    
    #endif
    #if NUMBER_MOTOR > 6
      TIMSK2 |= (1<<OCIE2B);    
    #endif
  #endif  
}

#if defined(SOFT_PWM_MOTORS)
  ISR(TIMER1_COMPA_vect) {
    static uint8_t state = 0;
    if(state == 0){
      PWM_1_PIN_HIGH;
      OCR1A += soft_PWM[0]+5;
      state = 1;
    }else if(state == 1){
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_LOW;
      #endif
      OCR1A += 255-(soft_PWM[1]+5);
      state = 2;
    }else if(state == 2){
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_HIGH;
      #endif
      OCR1A += soft_PWM[1]+5;
      state = 3;  
    }else if(state == 3){
      PWM_1_PIN_LOW;
      OCR1A += 255-(soft_PWM[0]+5);
      state = 0;   
    }
  }
  #if NUMBER_MOTOR > 2
    ISR(TIMER1_COMPB_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_3_PIN_HIGH;
        OCR1B += soft_PWM[2]+5;
        state = 1;
      }else if(state == 1){
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_LOW;
        #endif
        OCR1B += 255-(soft_PWM[3]+5);
        state = 2;
      }else if(state == 2){
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_HIGH;
        #endif
        OCR1B += soft_PWM[3]+5;
        state = 3;  
      }else if(state == 3){
        PWM_3_PIN_LOW;
        OCR1B += 255-(soft_PWM[2]+5);
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 4
    ISR(TIMER2_COMPA_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_5_PIN_HIGH;
        OCR2A += soft_PWM[4]+5;
        state = 1;
      }else if(state == 1){
        PWM_6_PIN_LOW;
        OCR2A += 255-(soft_PWM[5]+5);
        state = 2;
      }else if(state == 2){
        PWM_6_PIN_HIGH;
        OCR2A += soft_PWM[5]+5;
        state = 3;  
      }else if(state == 3){
        PWM_5_PIN_LOW;
        OCR2A += 255-(soft_PWM[4]+5);
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 6
    ISR(TIMER2_COMPB_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_7_PIN_HIGH;
        OCR2B += soft_PWM[6]+5;
        state = 1;
      }else if(state == 1){
        PWM_8_PIN_LOW;
        OCR2B += 255-(soft_PWM[7]+5);
        state = 2;
      }else if(state == 2){
        PWM_8_PIN_HIGH;
        OCR2B += soft_PWM[7]+5;
        state = 3;  
      }else if(state == 3){
        PWM_7_PIN_LOW;
        OCR2B += 255-(soft_PWM[6]+5);
        state = 0;   
      }
    } 
  #endif
#endif

regards Felix

ronco · Post by **ronco** » Sun Jan 08, 2012 2:18 pm

huh ..

I thought the interest would be larger

here are the benefits of this method... :

- (as i said) octocopter may be possible with pro mini
- hexa and octo can use gimbal with pro mini (with sum reciver)
- we have a higher signal resolution (245 steps instead of 125)
- we can use every pin we want

things that stays the same:

- cycletime is the same as it is with analogWrite (checket with tri and quad)
- output frequency is 488 Hz

disadvantages:

- the code is a little larger
- min PPM signal is 1042 istead of 995-1000

regards Felix

ronco · Post by **ronco** » Sun Jan 08, 2012 3:39 pm

and here are some oszilloscope recodings

made with two channels both from timer 1 a
- green line is output PWM 1 and red is PWM 2 .. red is just to have a clue and stays at value 0 (of 245)
- lower line is high state time

PWM 1 at 0

PWM 1 at 123

PWM 1 at 245

regards Felix

Hamburger · Post by **Hamburger** » Mon Jan 09, 2012 12:45 am

Felix,

I think this is really great news (and it got buried under the flow of new threads). Though others are more qualified to grasp the value of your code.
For me flying a hexa with gimbal control would be interesting in the future.
Besides, I am interested in driving more than the two standard servos which MWii provides (my target is either for a pitch copter or for a mwii controlled plane). Does your code also allow to reduce ouput frequency to something average servos can deal with?

Alexinparis · Post by **Alexinparis** » Mon Jan 09, 2012 10:51 am

ronco wrote:again some news

i found a way to drive 4 ESCs with one timer ... so they are now on default 488Hz and uses a resolution of 245 [1000;2000] => [0;245]

this means we may now drive a octocopter with a pro mini!

so

timer 1 a = motor 1 & 2 // pins 9 & 10
timer 1 b = motor 3 & 4 // pins 11& 3
timer 2 a = motor 5 & 6 // pins 5 & 6 (or A0 & A1)
timer 2 b = motor 7 & 8 // pins A2 & 12 [edit](maybe 4 & 7 if used with sum recivers)[/edit]

to have accurate signals min output is now 1042 that seems to be ok for the most ESCs but minthrottle must be lowed for these 42

i have tested this weith one tri and two quads .. till now everything works fine.

Code: Select all


#define SOFT_PWM_MOTORS // to activate

#define PWM_1_PIN       9
#define PWM_1_PIN_HIGH  PORTB |= 1<<1;
#define PWM_1_PIN_LOW   PORTB &= ~(1<<1);

#define PWM_2_PIN       10
#define PWM_2_PIN_HIGH  PORTB |= 1<<2;
#define PWM_2_PIN_LOW   PORTB &= ~(1<<2);

#define PWM_3_PIN       11
#define PWM_3_PIN_HIGH  PORTB |= 1<<3;
#define PWM_3_PIN_LOW   PORTB &= ~(1<<3);

#define PWM_4_PIN       3
#define PWM_4_PIN_HIGH  PORTD |= 1<<3;
#define PWM_4_PIN_LOW   PORTD &= ~(1<<3);

#if defined(A0_A1_PIN_HEX)
  #define PWM_5_PIN       A0
  #define PWM_5_PIN_HIGH  PORTC |= 1<<0;
  #define PWM_5_PIN_LOW   PORTC &= ~(1<<0);
  
  #define PWM_6_PIN       A1
  #define PWM_6_PIN_HIGH  PORTC |= 1<<1;
  #define PWM_6_PIN_LOW   PORTC &= ~(1<<1);
#else
  #define PWM_5_PIN       5
  #define PWM_5_PIN_HIGH  PORTD |= 1<<5;
  #define PWM_5_PIN_LOW   PORTD &= ~(1<<5);
  
  #define PWM_6_PIN       6
  #define PWM_6_PIN_HIGH  PORTD |= 1<<6;
  #define PWM_6_PIN_LOW   PORTD &= ~(1<<6);
#endif

#define PWM_7_PIN       A2
#define PWM_7_PIN_HIGH  PORTC |= 1<<2;
#define PWM_7_PIN_LOW   PORTC &= ~(1<<2);

#define PWM_8_PIN       12
#define PWM_8_PIN_HIGH  PORTB |= 1<<4;
#define PWM_8_PIN_LOW   PORTB &= ~(1<<4);


// pwm pulse values
int16_t soft_PWM[8] = {0,0,0,0,0,0,0,0};

void writeMotors() { // [1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #else
  
    #if defined(SOFT_PWM_MOTORS)
      for(uint8_t i=0;i<NUMBER_MOTOR;i++)
        soft_PWM[i] = (motor[i]-1000)/4.08; //[1000;2000] => [0;245]
    #else 
      for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
        analogWrite(PWM_PIN[i], motor[i]>>3);
      #if (NUMBER_MOTOR == 5)
        atomicPWM_PIN5_highState = motor[5]/8;
        atomicPWM_PIN5_lowState = 255-atomicPWM_PIN5_highState;
      #endif
      #if (NUMBER_MOTOR == 6)
        atomicPWM_PIN6_highState = motor[4]/8;
        atomicPWM_PIN6_lowState = 255-atomicPWM_PIN6_highState;
      #endif
    #endif
  #endif
}

// init
void init_Soft_PWM(){
  #if defined(SOFT_PWM_MOTORS)
    TCCR1A = 0;
    TIMSK1 |= (1<<OCIE1A);
    #if NUMBER_MOTOR > 2
      TIMSK1 |= (1<<OCIE1B);
    #endif
    #if NUMBER_MOTOR > 4
      TCCR2A = 0;
      TIMSK2 |= (1<<OCIE2A);    
    #endif
    #if NUMBER_MOTOR > 6
      TIMSK2 |= (1<<OCIE2B);    
    #endif
  #endif  
}

#if defined(SOFT_PWM_MOTORS)
  ISR(TIMER1_COMPA_vect) {
    static uint8_t state = 0;
    if(state == 0){
      PWM_1_PIN_HIGH;
      OCR1A += soft_PWM[0]+5;
      state = 1;
    }else if(state == 1){
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_LOW;
      #endif
      OCR1A += 255-(soft_PWM[1]+5);
      state = 2;
    }else if(state == 2){
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_HIGH;
      #endif
      OCR1A += soft_PWM[1]+5;
      state = 3;  
    }else if(state == 3){
      PWM_1_PIN_LOW;
      OCR1A += 255-(soft_PWM[0]+5);
      state = 0;   
    }
  }
  #if NUMBER_MOTOR > 2
    ISR(TIMER1_COMPB_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_3_PIN_HIGH;
        OCR1B += soft_PWM[2]+5;
        state = 1;
      }else if(state == 1){
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_LOW;
        #endif
        OCR1B += 255-(soft_PWM[3]+5);
        state = 2;
      }else if(state == 2){
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_HIGH;
        #endif
        OCR1B += soft_PWM[3]+5;
        state = 3;  
      }else if(state == 3){
        PWM_3_PIN_LOW;
        OCR1B += 255-(soft_PWM[2]+5);
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 4
    ISR(TIMER2_COMPA_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_5_PIN_HIGH;
        OCR2A += soft_PWM[4]+5;
        state = 1;
      }else if(state == 1){
        PWM_6_PIN_LOW;
        OCR2A += 255-(soft_PWM[5]+5);
        state = 2;
      }else if(state == 2){
        PWM_6_PIN_HIGH;
        OCR2A += soft_PWM[5]+5;
        state = 3;  
      }else if(state == 3){
        PWM_5_PIN_LOW;
        OCR2A += 255-(soft_PWM[4]+5);
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 6
    ISR(TIMER2_COMPB_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_7_PIN_HIGH;
        OCR2B += soft_PWM[6]+5;
        state = 1;
      }else if(state == 1){
        PWM_8_PIN_LOW;
        OCR2B += 255-(soft_PWM[7]+5);
        state = 2;
      }else if(state == 2){
        PWM_8_PIN_HIGH;
        OCR2B += soft_PWM[7]+5;
        state = 3;  
      }else if(state == 3){
        PWM_7_PIN_LOW;
        OCR2B += 255-(soft_PWM[6]+5);
        state = 0;   
      }
    } 
  #endif
#endif

regards Felix

Hi Felix,
It's still interresting to have your experiment feedback.

At the beginning of multiwii, I tried to handle everything via software PWM like you, but the stability pulse was worse than a true hardware PWM. So I decided to use it only for motor 5&6 on hexa config with promini.
But It was at a time I still used I2C wire lib (which is plenty of interrupts) and digitalWrite() (which takes a very long time in comparison with a direct port access)

Did you try to fly with your mod and make an objective comparison ?
Is the RC decoding still stable with this mod ?

Maybe another remark: I thing calculation like soft_PWM[7]+5; or 255-(soft_PWM[6]+5); should be made outside the ISR even is time consuming is very small.

ronco · Post by **ronco** » Mon Jan 09, 2012 6:45 pm

Alexinparis wrote:Did you try to fly with your mod and make an objective comparison ?

Hi Alex,

i did soome tests today (only indoor because of very bad weather).. it works fine for my 2 quads .. but there is a difference. i need lower P and I with the soft PWM code but its not more instable! i think its because of the higher signal resolution. maybe some courageous folks want to test it too ? ;P

Alexinparis wrote:Is the RC decoding still stable with this mod ?

i noticed no difference in the GUI

Alexinparis wrote:Maybe another remark: I thing calculation like soft_PWM[7]+5; or 255-(soft_PWM[6]+5); should be made outside the ISR even is time consuming is very small.

thats true -.- and done

Code: Select all

#define SOFT_PWM_MOTORS // to activate

#define PWM_1_PIN       9
#define PWM_1_PIN_HIGH  PORTB |= 1<<1;
#define PWM_1_PIN_LOW   PORTB &= ~(1<<1);

#define PWM_2_PIN       10
#define PWM_2_PIN_HIGH  PORTB |= 1<<2;
#define PWM_2_PIN_LOW   PORTB &= ~(1<<2);

#define PWM_3_PIN       11
#define PWM_3_PIN_HIGH  PORTB |= 1<<3;
#define PWM_3_PIN_LOW   PORTB &= ~(1<<3);

#define PWM_4_PIN       3
#define PWM_4_PIN_HIGH  PORTD |= 1<<3;
#define PWM_4_PIN_LOW   PORTD &= ~(1<<3);

#if defined(A0_A1_PIN_HEX)
  #define PWM_5_PIN       A0
  #define PWM_5_PIN_HIGH  PORTC |= 1<<0;
  #define PWM_5_PIN_LOW   PORTC &= ~(1<<0);
  
  #define PWM_6_PIN       A1
  #define PWM_6_PIN_HIGH  PORTC |= 1<<1;
  #define PWM_6_PIN_LOW   PORTC &= ~(1<<1);
#else
  #define PWM_5_PIN       5
  #define PWM_5_PIN_HIGH  PORTD |= 1<<5;
  #define PWM_5_PIN_LOW   PORTD &= ~(1<<5);
  
  #define PWM_6_PIN       6
  #define PWM_6_PIN_HIGH  PORTD |= 1<<6;
  #define PWM_6_PIN_LOW   PORTD &= ~(1<<6);
#endif

#define PWM_7_PIN       A2
#define PWM_7_PIN_HIGH  PORTC |= 1<<2;
#define PWM_7_PIN_LOW   PORTC &= ~(1<<2);

#define PWM_8_PIN       12
#define PWM_8_PIN_HIGH  PORTB |= 1<<4;
#define PWM_8_PIN_LOW   PORTB &= ~(1<<4);


// pwm pulse values
int16_t soft_PWM[8] = {5,5,5,5,5,5,5,5};

void writeMotors() { // [1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #else
  
    #if defined(SOFT_PWM_MOTORS)
      for(uint8_t i=0;i<NUMBER_MOTOR;i++)
        soft_PWM[i] = ((motor[i]-1000)/4.08)+5; //[1042;2000] => [5;250]
    #else 
      for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
        analogWrite(PWM_PIN[i], motor[i]>>3);
      #if (NUMBER_MOTOR == 5)
        atomicPWM_PIN5_highState = motor[5]/8;
        atomicPWM_PIN5_lowState = 255-atomicPWM_PIN5_highState;
      #endif
      #if (NUMBER_MOTOR == 6)
        atomicPWM_PIN6_highState = motor[4]/8;
        atomicPWM_PIN6_lowState = 255-atomicPWM_PIN6_highState;
      #endif
    #endif
  #endif
}

// init
void init_Soft_PWM(){
  #if defined(SOFT_PWM_MOTORS)
    pinMode(PWM_1_PIN,OUTPUT);
    #if NUMBER_MOTOR > 1
      pinMode(PWM_2_PIN,OUTPUT);
    #endif
    TCCR1A = 0;
    TIMSK1 |= (1<<OCIE1A);
    #if NUMBER_MOTOR > 2
      pinMode(PWM_3_PIN,OUTPUT);
      #if NUMBER_MOTOR > 3
        pinMode(PWM_4_PIN,OUTPUT);
      #endif
      TIMSK1 |= (1<<OCIE1B);
    #endif
    #if NUMBER_MOTOR > 4
      pinMode(PWM_5_PIN,OUTPUT);
      pinMode(PWM_6_PIN,OUTPUT);
      TCCR2A = 0;
      TIMSK2 |= (1<<OCIE2A);    
    #endif
    #if NUMBER_MOTOR > 6
      pinMode(PWM_7_PIN,OUTPUT);
      pinMode(PWM_8_PIN,OUTPUT);
      TIMSK2 |= (1<<OCIE2B);    
    #endif
  #endif  
}

#if defined(SOFT_PWM_MOTORS)
  ISR(TIMER1_COMPA_vect) {
    static uint8_t state = 0;
    if(state == 0){
      PWM_1_PIN_HIGH;
      OCR1A += soft_PWM[0];
      state = 1;
    }else if(state == 1){
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_LOW;
      #endif
      OCR1A += 255-soft_PWM[1];
      state = 2;
    }else if(state == 2){
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_HIGH;
      #endif
      OCR1A += soft_PWM[1];
      state = 3;  
    }else if(state == 3){
      PWM_1_PIN_LOW;
      OCR1A += 255-soft_PWM[0];
      state = 0;   
    }
  }
  #if NUMBER_MOTOR > 2
    ISR(TIMER1_COMPB_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_3_PIN_HIGH;
        OCR1B += soft_PWM[2];
        state = 1;
      }else if(state == 1){
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_LOW;
        #endif
        OCR1B += 255-soft_PWM[3];
        state = 2;
      }else if(state == 2){
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_HIGH;
        #endif
        OCR1B += soft_PWM[3];
        state = 3;  
      }else if(state == 3){
        PWM_3_PIN_LOW;
        OCR1B += 255-soft_PWM[2];
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 4
    ISR(TIMER2_COMPA_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_5_PIN_HIGH;
        OCR2A += soft_PWM[4];
        state = 1;
      }else if(state == 1){
        PWM_6_PIN_LOW;
        OCR2A += 255-soft_PWM[5];
        state = 2;
      }else if(state == 2){
        PWM_6_PIN_HIGH;
        OCR2A += soft_PWM[5];
        state = 3;  
      }else if(state == 3){
        PWM_5_PIN_LOW;
        OCR2A += 255-soft_PWM[4];
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 6
    ISR(TIMER2_COMPB_vect) {
      static uint8_t state = 0;
      if(state == 0){
        PWM_7_PIN_HIGH;
        OCR2B += soft_PWM[6];
        state = 1;
      }else if(state == 1){
        PWM_8_PIN_LOW;
        OCR2B += 255-soft_PWM[7];
        state = 2;
      }else if(state == 2){
        PWM_8_PIN_HIGH;
        OCR2B += soft_PWM[7];
        state = 3;  
      }else if(state == 3){
        PWM_7_PIN_LOW;
        OCR2B += 255-soft_PWM[6];
        state = 0;   
      }
    } 
  #endif
#endif

Hamburger wrote:Besides, I am interested in driving more than the two standard servos which MWii provides (my target is either for a pitch copter or for a mwii controlled plane). Does your code also allow to reduce ouput frequency to something average servos can deal with?

Hi,

thats not needed we can drive 8 servos with the method alex uses.
that may look like this

(example code)

Code: Select all

int16_t atomicServo[8] = {125,125,125,125,125,125,125,125};


#define NUMBER_SERVO  8



#define SERVO_1_PIN       9
#define SERVO_1_PIN_HIGH  PORTB |= 1<<1;
#define SERVO_1_PIN_LOW   PORTB &= ~(1<<1);

#define SERVO_2_PIN       10
#define SERVO_2_PIN_HIGH  PORTB |= 1<<2;
#define SERVO_2_PIN_LOW   PORTB &= ~(1<<2);

#define SERVO_3_PIN       11
#define SERVO_3_PIN_HIGH  PORTB |= 1<<3;
#define SERVO_3_PIN_LOW   PORTB &= ~(1<<3);

#define SERVO_4_PIN       3
#define SERVO_4_PIN_HIGH  PORTD |= 1<<3;
#define SERVO_4_PIN_LOW   PORTD &= ~(1<<3);

#define SERVO_5_PIN       A0
#define SERVO_5_PIN_HIGH  PORTC |= 1<<0;
#define SERVO_5_PIN_LOW   PORTC &= ~(1<<0);

#define SERVO_6_PIN       A1
#define SERVO_6_PIN_HIGH  PORTC |= 1<<1;
#define SERVO_6_PIN_LOW   PORTC &= ~(1<<1);

#define SERVO_7_PIN       A2
#define SERVO_7_PIN_HIGH  PORTC |= 1<<2;
#define SERVO_7_PIN_LOW   PORTC &= ~(1<<2);

#define SERVO_8_PIN       12
#define SERVO_8_PIN_HIGH  PORTB |= 1<<4;
#define SERVO_8_PIN_LOW   PORTB &= ~(1<<4);

...
...
init and write servos
...
...


ISR(TIMER0_COMPA_vect) {
  static uint8_t state = 0;
  static uint8_t count;
  if (state == 0) {
    //http://billgrundmann.wordpress.com/2009/03/03/to-use-or-not-use-writedigital/ 
      // we know that NUMBER_SERVO is more then 0
      SERVO_1_PIN_HIGH;
    
    OCR0A+= 250; // 1000 us
    state++ ;
  } else if (state == 1) {
    OCR0A+= atomicServo[0]; // 1000 + [0-1020] us
    state++;
  } else if (state == 2) {
    
      SERVO_1_PIN_LOW;
    
    #if (NUMBER_SERVO > 1)  
      SERVO_2_PIN_HIGH;;
    #endif
    OCR0A+= 250; // 1000 us
    state++;
  } else if (state == 3) {
    OCR0A+= atomicServo[1]; // 1000 + [0-1020] us
    state++;
  } else if (state == 4) {
    #if (NUMBER_SERVO > 1)  
      SERVO_2_PIN_LOW;
    #endif
    #if (NUMBER_SERVO > 2)  
      SERVO_3_PIN_HIGH;
    #endif
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 5) {
    OCR0A+= atomicServo[2]; // 1000 + [0-1020] us
    state++;
  } else if (state == 6) {
    #if (NUMBER_SERVO > 2)    
      SERVO_3_PIN_LOW;
    #endif
    #if (NUMBER_SERVO > 3)    
      SERVO_4_PIN_HIGH;
    #endif
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 7) {
    OCR0A+= atomicServo[3]; // 1000 + [0-1020] us
    state++;
  } else if (state == 8) {
    #if (NUMBER_SERVO > 3)    
      SERVO_4_PIN_LOW;
    #endif  
    #if (NUMBER_SERVO > 4)    
      SERVO_5_PIN_HIGH;;
    #endif 
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 9) {
    OCR0A+= atomicServo[4]; // 1000 + [0-1020] us
    state++;
  } else if (state == 10) {
    #if (NUMBER_SERVO > 4)    
      SERVO_5_PIN_LOW;
    #endif 
    #if (NUMBER_SERVO > 5)    
      SERVO_6_PIN_HIGH;;
    #endif 
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 11) {
    OCR0A+= atomicServo[5]; // 1000 + [0-1020] us
    state++;
  } else if (state == 12) {
    #if (NUMBER_SERVO > 5)    
      SERVO_6_PIN_LOW;
    #endif 
    #if (NUMBER_SERVO > 6)    
      SERVO_7_PIN_HIGH;
    #endif 
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 13) {
    OCR0A+= atomicServo[6]; // 1000 + [0-1020] us
    state++;
  } else if (state == 14) {
    #if (NUMBER_SERVO > 6)    
      SERVO_7_PIN_LOW;
    #endif 
    #if (NUMBER_SERVO > 7)    
      SERVO_8_PIN_HIGH;
    #endif     
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 15) {
    OCR0A+= atomicServo[7]; // 1000 + [0-1020] us
    state++;
  } else if (state == 16) {
    #if (NUMBER_SERVO > 7)    
      SERVO_8_PIN_LOW;
    #endif 
    count = 2;
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 17) {
    if (count > 0) count--;
    else state = 0;
    OCR0A+= 250;
  }
}

regards Felix

ziss_dm · Post by **ziss_dm** » Tue Jan 10, 2012 1:25 am

Hi Felix,

i need lower P and I with the soft PWM code but its not more instable! i think its because of the higher signal resolution. maybe some courageous folks want to test it too ? ;P

I think, this is because the effective resulution actually decreased due the jitter. ;( If you want, I could check it with a scope on weekend, but I would guess, that this interrupt handlers would have jitter around 8us. You can slightly improve this by turning on HW output compare on the last cycle, but in my expirements it was still not acceptable.

I think, best solution for PROMINI is:
2 servos -> timer1 with prescaler /8 (2000 steps resolution)
4 motors -> timer0, timer2 phase correct pwm prescaler /64 (as is)

Code: Select all

  TCCR1A = _BV(WGM11); /* Fast PWM, ICR1 is top */
  TCCR1B = _BV(WGM13) | _BV(WGM12) /* Fast PWM, ICR1 is top */
   | _BV(CS11) /* div 8 clock prescaler */

   ICR1 - refresh rate in 0.5us (44000 for std. servo, for digitals can be lower)
   OCR1A - value 1 in 0.5us
   OCR2A - value 2 in 0.5us

For more than 4 motors it is better to use something like this:
http://www.rcgroups.com/forums/showthread.php?t=1399162

BTW: What we trying to archive?

regards,
ziss_dm

ronco · Post by **ronco** » Tue Jan 10, 2012 7:51 am

hmm

it really seems to work in my tests ..

its hard to explain with my bad english -.- but it was like this:

with soft PWM i lowed P for 0,3 and I for 0,005 to have no oscilations and it felt stable
then i used the same settings with analoWrite .. then it was drifting .. like P and I was to low..

ziss_dm wrote: You can slightly improve this by turning on HW output compare on the last cycle, but in my expirements it was still not acceptable.

what is this HW?

ziss_dm wrote:If you want, I could check it with a scope on weekend

yes please .. i have only a bad souncard software scope

and i think i have thomething that makes it eaven more accurate..

it was like (example):

pwm_ 0 = 150;

(state = 0)
pin_high;
OCRXX += pwm_0;
(next state)
pin_LOW;
OCRXX += 255-pwm_0;

but if the pwm_0 value changes between the states the result may be bad .. so i changed it to this

(state = 0)
M1val = pwm_0;
pin_high;
OCRXX += M1val ;
(next state)
pin_LOW;
OCRXX += 255-M1val ;

this makes sure that high and low state comes from the same value

new code

Code: Select all

#define SOFT_PWM_MOTORS // to activate

#define PWM_1_PIN       9
#define PWM_1_PIN_HIGH  PORTB |= 1<<1;
#define PWM_1_PIN_LOW   PORTB &= ~(1<<1);

#define PWM_2_PIN       10
#define PWM_2_PIN_HIGH  PORTB |= 1<<2;
#define PWM_2_PIN_LOW   PORTB &= ~(1<<2);

#define PWM_3_PIN       11
#define PWM_3_PIN_HIGH  PORTB |= 1<<3;
#define PWM_3_PIN_LOW   PORTB &= ~(1<<3);

#define PWM_4_PIN       3
#define PWM_4_PIN_HIGH  PORTD |= 1<<3;
#define PWM_4_PIN_LOW   PORTD &= ~(1<<3);

#if defined(A0_A1_PIN_HEX)
  #define PWM_5_PIN       A0
  #define PWM_5_PIN_HIGH  PORTC |= 1<<0;
  #define PWM_5_PIN_LOW   PORTC &= ~(1<<0);
  
  #define PWM_6_PIN       A1
  #define PWM_6_PIN_HIGH  PORTC |= 1<<1;
  #define PWM_6_PIN_LOW   PORTC &= ~(1<<1);
#else
  #define PWM_5_PIN       5
  #define PWM_5_PIN_HIGH  PORTD |= 1<<5;
  #define PWM_5_PIN_LOW   PORTD &= ~(1<<5);
  
  #define PWM_6_PIN       6
  #define PWM_6_PIN_HIGH  PORTD |= 1<<6;
  #define PWM_6_PIN_LOW   PORTD &= ~(1<<6);
#endif

#define PWM_7_PIN       A2
#define PWM_7_PIN_HIGH  PORTC |= 1<<2;
#define PWM_7_PIN_LOW   PORTC &= ~(1<<2);

#define PWM_8_PIN       12
#define PWM_8_PIN_HIGH  PORTB |= 1<<4;
#define PWM_8_PIN_LOW   PORTB &= ~(1<<4);


// pwm pulse values
int16_t soft_PWM[8] = {5,5,5,5,5,5,5,5};

void writeMotors() { // [1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #else
  
    #if defined(SOFT_PWM_MOTORS)
      for(uint8_t i=0;i<NUMBER_MOTOR;i++)
        soft_PWM[i] = ((motor[i]-1000)/4.08)+5; //[1042;2000] => [5;250]
    #else 
      for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
        analogWrite(PWM_PIN[i], motor[i]>>3);
      #if (NUMBER_MOTOR == 5)
        atomicPWM_PIN5_highState = motor[5]/8;
        atomicPWM_PIN5_lowState = 255-atomicPWM_PIN5_highState;
      #endif
      #if (NUMBER_MOTOR == 6)
        atomicPWM_PIN6_highState = motor[4]/8;
        atomicPWM_PIN6_lowState = 255-atomicPWM_PIN6_highState;
      #endif
    #endif
  #endif
}

// init
void init_Soft_PWM(){
  #if defined(SOFT_PWM_MOTORS)
    pinMode(PWM_1_PIN,OUTPUT);
    #if NUMBER_MOTOR > 1
      pinMode(PWM_2_PIN,OUTPUT);
    #endif
    TCCR1A = 0;
    TIMSK1 |= (1<<OCIE1A);
    #if NUMBER_MOTOR > 2
      pinMode(PWM_3_PIN,OUTPUT);
      #if NUMBER_MOTOR > 3
        pinMode(PWM_4_PIN,OUTPUT);
      #endif
      TIMSK1 |= (1<<OCIE1B);
    #endif
    #if NUMBER_MOTOR > 4
      pinMode(PWM_5_PIN,OUTPUT);
      pinMode(PWM_6_PIN,OUTPUT);
      TCCR2A = 0;
      TIMSK2 |= (1<<OCIE2A);    
    #endif
    #if NUMBER_MOTOR > 6
      pinMode(PWM_7_PIN,OUTPUT);
      pinMode(PWM_8_PIN,OUTPUT);
      TIMSK2 |= (1<<OCIE2B);    
    #endif
  #endif  
}

#if defined(SOFT_PWM_MOTORS)
  ISR(TIMER1_COMPA_vect) {
    static uint8_t state = 0;
    static uint8_t M1Val = 5;
    static uint8_t M2Val = 5;
    if(state == 0){
      M1Val = soft_PWM[0];
      PWM_1_PIN_HIGH;
      OCR1A += M1Val;
      state = 1;
    }else if(state == 1){
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_LOW;
      #endif
      OCR1A += 255-M2Val;
      state = 2;
    }else if(state == 2){
      M2Val = soft_PWM[1];
      #if NUMBER_MOTOR > 1
        PWM_2_PIN_HIGH;
      #endif
      OCR1A += M2Val;
      state = 3;  
    }else if(state == 3){
      PWM_1_PIN_LOW;
      OCR1A += 255-M1Val;
      state = 0;   
    }
  }
  #if NUMBER_MOTOR > 2
    ISR(TIMER1_COMPB_vect) {
      static uint8_t state = 0;
      static uint8_t M1Val = 5;
      static uint8_t M2Val = 5;
      if(state == 0){
        M1Val = soft_PWM[2];
        PWM_3_PIN_HIGH;
        OCR1B += M1Val;
        state = 1;
      }else if(state == 1){
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_LOW;
        #endif
        OCR1B += 255-M2Val;
        state = 2;
      }else if(state == 2){
        M2Val = soft_PWM[3];
        #if NUMBER_MOTOR > 3
          PWM_4_PIN_HIGH;
        #endif
        OCR1B += M2Val;
        state = 3;  
      }else if(state == 3){
        PWM_3_PIN_LOW;
        OCR1B += 255-M1Val;
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 4
    ISR(TIMER2_COMPA_vect) {
      static uint8_t state = 0;
      static uint8_t M1Val = 5;
      static uint8_t M2Val = 5;
      if(state == 0){
        M1Val = soft_PWM[4];
        PWM_5_PIN_HIGH;
        OCR2A += M1Val;
        state = 1;
      }else if(state == 1){
        PWM_6_PIN_LOW;
        OCR2A += 255-M2Val;
        state = 2;
      }else if(state == 2){
        M2Val = soft_PWM[5];
        PWM_6_PIN_HIGH;
        OCR2A += M2Val;
        state = 3;  
      }else if(state == 3){
        PWM_5_PIN_LOW;
        OCR2A += 255-M1Val;
        state = 0;   
      }
    } 
  #endif
  #if NUMBER_MOTOR > 6
    ISR(TIMER2_COMPB_vect) {
      static uint8_t state = 0;
      static uint8_t M1Val = 5;
      static uint8_t M2Val = 5;
      if(state == 0){
        M1Val = soft_PWM[6];
        PWM_7_PIN_HIGH;
        OCR2B += M1Val;
        state = 1;
      }else if(state == 1){
        PWM_8_PIN_LOW;
        OCR2B += 255-M2Val;
        state = 2;
      }else if(state == 2){
        M2Val = soft_PWM[7];
        PWM_8_PIN_HIGH;
        OCR2B += M2Val;
        state = 3;  
      }else if(state == 3){
        PWM_7_PIN_LOW;
        OCR2B += 255-M1Val;
        state = 0;   
      }
    } 
  #endif
#endif

regards Felix

ziss_dm · Post by **ziss_dm** » Tue Jan 10, 2012 10:19 am

Hi Felix,

what is this HW?

Currently you disconnected output compare module from the pin and using only interrupt. It is possible to connect OCP module to turn on/off pin by hardware and in interrupt just reload OCRxx..

regards,
ziss_dm

aBUGSworstnightmare · Tue Jan 10, 2012 12:34 pm

Hi,

I want to use the A2 PWM output (CAMTRIG servo output) of a ProMini for some 'external hardware' but I need a much higher PWM frequency than 50Hz. Something in the range of 450 - 500 Hz would be fine if the resolution is around 200 steps.

What is the best solution for doing this? Thank you for your help,
rgds
aBUGSworstnightmare

ronco · Post by **ronco** » Tue Jan 10, 2012 5:32 pm

Hi ziss, can you give me some example code? i would like to experiment with this

@aBUGSworstnightmare 488Hz are possible .. but you need a free timer for doing it... 2 possibilities:

1. you use a tri copter with normal code (then timer 2 b is free?)
2. you use my soft pwm code (that is pretty experimental ATM.)

regards Felix

aBUGSworstnightmare · Post by **aBUGSworstnightmare** » Tue Jan 10, 2012 7:28 pm

Hi ronco,

well, the soft pwm code will be an option for the future. Seems like I will have to live with 50Hz at the moment.

Is it possible to have a PWM duty cycle range of around 500us - 2250us with the current code (for CAMTRIG only)?

Rgds
aBUGSworstnightmare

ziss_dm · Post by **ziss_dm** » Wed Jan 11, 2012 9:17 am

Hi Felix,

This is an example for Timer2:

Code: Select all

ISR(TIMER2_OVF_vect) {
  if (TCCR2A & (_BV(COM2A1) | _BV(COM2B1))) {
    // Hold current value 
    PORTB = PINB;
    PORTD = PIND;
    // OC2x disconnected 
    TCCR2A &= ~(_BV(COM2A1) | _BV(COM2B1));
    // Reload counter with correction
    TCNT2 += (256 - (250 - 2));
  } else {
    // OC2x connected 
    TCCR2A |= (_BV(COM2A1) | _BV(COM2B1));
  }  
}  

void setup() {
  TCCR2A = _BV(COM2A1) |
           _BV(COM2B1) |   
           _BV(WGM20)  | _BV(WGM21);
  // Range  1-251 -> 1000-2000         
  OCR2A = 126; OCR2B = 126;  
  pinMode(3, OUTPUT); pinMode(11, OUTPUT);
  TIMSK2 |= _BV(TOIE2);    
}

void loop() {
}

regards,
ziss_dm

ronco · Post by **ronco** » Wed Jan 11, 2012 10:43 am

Thank you ziss!

with the example you give me we have a resolution 250 at 495Hz .. pulsed by hardware .. so why dont we use it?

ill make some tests today!

regards Felix

ziss_dm · Post by **ziss_dm** » Wed Jan 11, 2012 11:16 am

Hi Felix,

ronco wrote:with the example you give me we have a resolution 250 at 495Hz .. pulsed by hardware .. so why dont we use it?

There are couple of reasons:
1) It still have small jitter
2) It uses CPU time
3) It is real pain to implement it for Timer0, as it used by core for "sys-tick"

By the end of the day, it was easier (for me) to modify ESC firmware and leave Arduino "as-is".

regards,
ziss_dm

ronco · Post by **ronco** » Wed Jan 11, 2012 8:32 pm

hmm...

so what do you think if we use a second pro mini via i2c thats just pwm the motors .. may that help?

regards Felix

ronco · Post by **ronco** » Thu Jan 12, 2012 4:10 pm

i just try it ATM

but i have some problems with the i2c connection

because i have no clue of it i just copied alex method from his LED Ring code

in output

Code: Select all

void writeMotors() { // [1000;2000] => [125;250]
  #if defined(ext_soft_PPM)
    i2c_rep_start(ext_soft_PPM_ADDRESS);
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
          i2c_write(200);// just to see it sends
     // i2c_write(((motor[i]-1000)/4.08)+5);
    i2c_stop();
  #else
  ....

and for the second pro mini

Code: Select all

#include <Wire.h>

void refreshVals(int16_t n) {
  uint8_t p=0;  
  while(Wire.available()) {
    soft_PWM[p++]=Wire.read(); // recive was replaced by read in arduino 1.0
    if (p>7) p=7;
  }
}

void setup(){
  Wire.begin(0x6D);    // join i2c bus with address #2
  Wire.onReceive(refreshVals);   // register event
  ...
}

it sends an recives data but i have heavy i2c errors so the pin 13 LED on MWC promini stays on, and i2c error count couts fast up

i have disabled the internal pullups on both arduinos .. i have 2k2 external pullups and one WMP on i2c

i dont know whats wrong ..

regards Felix

ronco · Post by **ronco** » Thu Jan 12, 2012 6:23 pm

done -.-

in my MWC sketch was a BMA020 defined ...

ronco · Post by **ronco** » Sat Jan 14, 2012 1:51 am

... i did a lot of tests now .. and you are right

i cant get a result thats better then analogWrite...

but with my "two ESCs per timer channel" we can use hexa on pro mini with servos and octo without servos.

i modified the output of MWC dev 20111220 .. i cant test it because i have no hexa or octo

[edit]
i forgot

with this motor 1-4 is analogWrite (as it was).. 5 & 6 OCR0B so we can use OCR0A for servos and for octo 7 & 8 with OCR0A

[/edit]

Code: Select all


#if defined(BI) || defined(TRI) || defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING) || defined(CAMTRIG)
  #define SERVO
#endif

#if defined(GIMBAL)
  #define NUMBER_MOTOR 0
#elif defined(FLYING_WING)
  #define NUMBER_MOTOR 1
#elif defined(BI)
  #define NUMBER_MOTOR 2
#elif defined(TRI)
  #define NUMBER_MOTOR 3
#elif defined(QUADP) || defined(QUADX) || defined(Y4)
  #define NUMBER_MOTOR 4
#elif defined(Y6) || defined(HEX6) || defined(HEX6X)
  #define NUMBER_MOTOR 6
#elif defined(OCTOX8) || defined(OCTOFLATP) || defined(OCTOFLATX)
  #define NUMBER_MOTOR 8
#endif

uint8_t PWM_PIN[8] = {MOTOR_ORDER};
volatile uint8_t atomicServo[4] = {125,125,125,125};

//for HEX Y6 and HEX6/HEX6X flat and for promini
volatile uint8_t atomicPWM_PIN5_lowState;
volatile uint8_t atomicPWM_PIN5_highState;
volatile uint8_t atomicPWM_PIN6_lowState;
volatile uint8_t atomicPWM_PIN6_highState;

volatile uint8_t atomicPWM_PINA2_lowState;
volatile uint8_t atomicPWM_PINA2_highState;
volatile uint8_t atomicPWM_PINA3_lowState;
volatile uint8_t atomicPWM_PINA3_highState;


void writeServos() {
  #if defined(SERVO)
    atomicServo[0] = (servo[0]-1000)/4;
    atomicServo[1] = (servo[1]-1000)/4;
    atomicServo[2] = (servo[2]-1000)/4;
    atomicServo[3] = (servo[3]-1000)/4;
  #endif
}

void writeMotors() { // [1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #else
    for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
    #if (NUMBER_MOTOR == 6)
      atomicPWM_PIN5_highState = ((motor[5]-1000)/4.08)+5;
      atomicPWM_PIN5_lowState = 255-atomicPWM_PIN5_highState;
      atomicPWM_PIN6_highState = ((motor[4]-1000)/4.08)+5;
      atomicPWM_PIN6_lowState = 255-atomicPWM_PIN6_highState;
    #endif
    #if (NUMBER_MOTOR == 8)
      atomicPWM_PINA2_highState = ((motor[6]-1000)/4.08)+5;
      atomicPWM_PINA2_lowState = 255-atomicPWM_PIN5_highState;
      atomicPWM_PINA3_highState = ((motor[7]-1000)/4.08)+5;
      atomicPWM_PINA3_lowState = 255-atomicPWM_PIN6_highState;
    #endif
  #endif
}

void writeAllMotors(int16_t mc) {   // Sends commands to all motors
  for (uint8_t i =0;i<NUMBER_MOTOR;i++)
    motor[i]=mc;
  writeMotors();
}

#if defined(LOG_VALUES) || (POWERMETER == 1)
void logMotorsPower() {
  uint32_t amp;
  /* true cubic function; when divided by vbat_max=126 (12.6V) for 3 cell battery this gives maximum value of ~ 1000 */
  const uint32_t amperes[64] =   {0,4,13,31,60,104,165,246,350,481,640,831,1056,1319,1622,1969,2361,2803,3297,3845,4451,5118,5848,6645,
                                7510,8448,9461,10551,11723,12978,14319,15750,17273,18892,20608,22425,24346,26374,28512,30762,33127,35611,
                                38215,40944,43799,46785,49903,53156,56548,60081,63759,67583,71558,75685,79968,84410,89013,93781,98716,103821,
                                109099,114553,120186,126000 };
  if (vbat) { // by all means - must avoid division by zero 
    for (uint8_t i =0;i<NUMBER_MOTOR;i++) {
      amp = amperes[(motor[i] - 1000)>>4] / vbat; // range mapped from [1000:2000] => [0:1000]; then break that up into 64 ranges; lookup amp
      #ifdef LOG_VALUES
         pMeter[i]+= amp; // sum up over time the mapped ESC input 
      #endif
      #if (POWERMETER == 1)
         pMeter[PMOTOR_SUM]+= amp; // total sum over all motors
      #endif
    }
  }
}
#endif

void initOutput() {
  for(uint8_t i=0;i<NUMBER_MOTOR;i++)
    pinMode(PWM_PIN[i],OUTPUT);
  writeAllMotors(1000);
  delay(300);
  #if defined(SERVO)
    initializeServo();
  #elif (NUMBER_MOTOR == 6) && defined(PROMINI)
    initializeSoftPWM();
    #if defined(A0_A1_PIN_HEX)
      pinMode(5,INPUT);pinMode(6,INPUT);     // we reactivate the INPUT affectation for these two PINs
      pinMode(A0,OUTPUT);pinMode(A1,OUTPUT);
    #endif
  #elif (NUMBER_MOTOR == 8) && defined(PROMINI)
    initializeSoftPWM();
    #if defined(A0_A1_PIN_HEX)
      pinMode(5,INPUT);pinMode(6,INPUT);     // we reactivate the INPUT affectation for these two PINs
      pinMode(A0,OUTPUT);pinMode(A1,OUTPUT);
      pinMode(A2,OUTPUT);pinMode(A3,OUTPUT);
    #endif
  #endif
}

#if defined(SERVO)
void initializeServo() {
  #if defined(TRI)
    DIGITAL_SERVO_TRI_PINMODE;
  #endif
  #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
    DIGITAL_TILT_ROLL_PINMODE;
    DIGITAL_TILT_PITCH_PINMODE;
  #endif
  #if defined(CAMTRIG)
    DIGITAL_CAM_PINMODE;
  #endif
  #if defined(BI)
    DIGITAL_SERVO_TRI_PINMODE;
    DIGITAL_BI_LEFT_PINMODE;
  #endif
  TCCR0A = 0; // normal counting mode
  TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
}

// ****servo yaw with a 50Hz refresh rate****
// prescaler is set by default to 64 on Timer0
// Duemilanove : 16MHz / 64 => 4 us
// 256 steps = 1 counter cycle = 1024 us
// algorithm strategy:
// pulse high servo 0 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 0
// pulse high servo 1 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 1
// pulse high servo 2 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 2
// pulse high servo 3 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 3
// do nothing for 14 x 1000 us
ISR(TIMER0_COMPA_vect) {
  static uint8_t state = 0;
  static uint8_t count;
  if (state == 0) {
    //http://billgrundmann.wordpress.com/2009/03/03/to-use-or-not-use-writedigital/
    #if defined(TRI) || defined (BI)
      DIGITAL_SERVO_TRI_HIGH;
    #endif
    OCR0A+= 250; // 1000 us
    state++ ;
  } else if (state == 1) {
    OCR0A+= atomicServo[0]; // 1000 + [0-1020] us
    state++;
  } else if (state == 2) {
    #if defined(TRI) || defined (BI)
      DIGITAL_SERVO_TRI_LOW;
    #endif
    #if defined(BI)
      DIGITAL_BI_LEFT_HIGH;
    #endif
    #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
      DIGITAL_TILT_PITCH_HIGH;
    #endif
    OCR0A+= 250; // 1000 us
    state++;
  } else if (state == 3) {
    OCR0A+= atomicServo[1]; // 1000 + [0-1020] us
    state++;
  } else if (state == 4) {
    #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
      DIGITAL_TILT_PITCH_LOW;
      DIGITAL_TILT_ROLL_HIGH;
    #endif
    #if defined(BI)
      DIGITAL_BI_LEFT_LOW;
    #endif
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 5) {
    OCR0A+= atomicServo[2]; // 1000 + [0-1020] us
    state++;
  } else if (state == 6) {
    #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
      DIGITAL_TILT_ROLL_LOW;
    #endif
    #if defined(CAMTRIG)
      DIGITAL_CAM_HIGH;
    #endif
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 7) {
    OCR0A+= atomicServo[3]; // 1000 + [0-1020] us
    state++;
  } else if (state == 8) {
    #if defined(CAMTRIG)
      DIGITAL_CAM_LOW;
    #endif
    count = 10; // 12 x 1000 us
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 9) {
    if (count > 0) count--;
    else state = 0;
    OCR0A+= 250;
  }
}
#endif

#if (NUMBER_MOTOR == 6 || NUMBER_MOTOR == 8) && defined(PROMINI)
void initializeSoftPWM() {
  TCCR0A = 0; // normal counting mode
  #if (NUMBER_MOTOR > 6)
    TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
  #endif
  TIMSK0 |= (1<<OCIE0B);
}
#if (NUMBER_MOTOR > 6)
  // pin a2 and 12 for octo
  ISR(TIMER0_COMPA_vect) {
    static uint8_t state = 0;
    if(state == 0){
      PORTC |= 1<<2; // PIN A2 high
      OCR0A += atomicPWM_PINA2_highState;
      state = 1;
    }else if(state == 1){
      PORTB &= ~(1<<4); //digital PIN 12 low
      OCR0A += atomicPWM_PINA3_lowState;
      state = 2;
    }else if(state == 2){
      PORTB |= 1<<4; //digital PIN 12 high
      OCR0A += atomicPWM_PINA3_highState;
      state = 3;  
    }else if(state == 3){
      PORTC &= ~(1<<2); // PIN A2 low
      OCR0A += atomicPWM_PINA2_lowState;
      state = 0;   
    }
  }
#endif




ISR(TIMER0_COMPB_vect) { //the same with digital PIN 5 & 6 or A0 & A1 and OCR0B counter
  static uint8_t state = 0;
  if(state == 0){
    #if not defined(A0_A1_PIN_HEX)
      PORTD |= 1<<5; //digital PIN 5 high
    #else
      PORTC |= 1<<0;//PIN A0
    #endif
    OCR0B += atomicPWM_PIN5_highState;
    state = 1;
  }else if(state == 1){
    #if not defined(A0_A1_PIN_HEX)
      PORTD &= ~(1<<6);
    #else
      PORTC &= ~(1<<1);
    #endif
    OCR0B += atomicPWM_PIN6_lowState;
    state = 2;
  }else if(state == 2){
    #if not defined(A0_A1_PIN_HEX)
      PORTD |= 1<<6;
    #else
      PORTC |= 1<<1;//PIN A1
    #endif
    OCR0B += atomicPWM_PIN6_highState;
    state = 3;  
  }else if(state == 3){
    #if not defined(A0_A1_PIN_HEX)
      PORTD &= ~(1<<5); //digital PIN 5 low
    #else
      PORTC &= ~(1<<0);
    #endif 
    OCR0B += atomicPWM_PIN5_lowState;
    state = 0;   
  }
}
#endif


void mixTable() {
  int16_t maxMotor;
  uint8_t i,axis;
  static uint8_t camCycle = 0;
  static uint8_t camState = 0;
  static uint32_t camTime = 0;
  
  #define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z

  #if NUMBER_MOTOR > 3
    //prevent "yaw jump" during yaw correction
    axisPID[YAW] = constrain(axisPID[YAW],-100-abs(rcCommand[YAW]),+100+abs(rcCommand[YAW]));
  #endif
  #ifdef BI
    motor[0] = PIDMIX(+1, 0, 0); //LEFT
    motor[1] = PIDMIX(-1, 0, 0); //RIGHT        
    servo[0]  = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
    servo[1]  = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
  #endif
  #ifdef TRI
    motor[0] = PIDMIX( 0,+4/3, 0); //REAR
    motor[1] = PIDMIX(-1,-2/3, 0); //RIGHT
    motor[2] = PIDMIX(+1,-2/3, 0); //LEFT
    servo[0] = constrain(TRI_YAW_MIDDLE + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
  #endif
  #ifdef QUADP
    motor[0] = PIDMIX( 0,+1,-1); //REAR
    motor[1] = PIDMIX(-1, 0,+1); //RIGHT
    motor[2] = PIDMIX(+1, 0,+1); //LEFT
    motor[3] = PIDMIX( 0,-1,-1); //FRONT
  #endif
  #ifdef QUADX
    motor[0] = PIDMIX(-1,+1,-1); //REAR_R
    motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
    motor[2] = PIDMIX(+1,+1,+1); //REAR_L
    motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
  #endif
  #ifdef Y4
    motor[0] = PIDMIX(+0,+1,-1);   //REAR_1 CW
    motor[1] = PIDMIX(-1,-1, 0); //FRONT_R CCW
    motor[2] = PIDMIX(+0,+1,+1);   //REAR_2 CCW
    motor[3] = PIDMIX(+1,-1, 0); //FRONT_L CW
  #endif
  #ifdef Y6
    motor[0] = PIDMIX(+0,+4/3,+1); //REAR
    motor[1] = PIDMIX(-1,-2/3,-1); //RIGHT
    motor[2] = PIDMIX(+1,-2/3,-1); //LEFT
    motor[3] = PIDMIX(+0,+4/3,-1); //UNDER_REAR
    motor[4] = PIDMIX(-1,-2/3,+1); //UNDER_RIGHT
    motor[5] = PIDMIX(+1,-2/3,+1); //UNDER_LEFT    
  #endif
  #ifdef HEX6
    motor[0] = PIDMIX(-1/2,+1/2,+1); //REAR_R
    motor[1] = PIDMIX(-1/2,-1/2,-1); //FRONT_R
    motor[2] = PIDMIX(+1/2,+1/2,+1); //REAR_L
    motor[3] = PIDMIX(+1/2,-1/2,-1); //FRONT_L
    motor[4] = PIDMIX(+0  ,-1  ,+1); //FRONT
    motor[5] = PIDMIX(+0  ,+1  ,-1); //REAR
  #endif
  #ifdef HEX6X
    motor[0] = PIDMIX(-1/2,+1/2,+1); //REAR_R
    motor[1] = PIDMIX(-1/2,-1/2,+1); //FRONT_R
    motor[2] = PIDMIX(+1/2,+1/2,-1); //REAR_L
    motor[3] = PIDMIX(+1/2,-1/2,-1); //FRONT_L
    motor[4] = PIDMIX(-1  ,+0  ,-1); //RIGHT
    motor[5] = PIDMIX(+1  ,+0  ,+1); //LEFT
  #endif
  #ifdef OCTOX8
    motor[0] = PIDMIX(-1,+1,-1); //REAR_R
    motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
    motor[2] = PIDMIX(+1,+1,+1); //REAR_L
    motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
    motor[4] = PIDMIX(-1,+1,+1); //UNDER_REAR_R
    motor[5] = PIDMIX(-1,-1,-1); //UNDER_FRONT_R
    motor[6] = PIDMIX(+1,+1,-1); //UNDER_REAR_L
    motor[7] = PIDMIX(+1,-1,+1); //UNDER_FRONT_L
  #endif
  #ifdef OCTOFLATP
    motor[0] = PIDMIX(+7/10,-7/10,+1); //FRONT_L
    motor[1] = PIDMIX(-7/10,-7/10,+1); //FRONT_R
    motor[2] = PIDMIX(-7/10,+7/10,+1); //REAR_R
    motor[3] = PIDMIX(+7/10,+7/10,+1); //REAR_L
    motor[4] = PIDMIX(+0   ,-1   ,-1); //FRONT
    motor[5] = PIDMIX(-1   ,+0   ,-1); //RIGHT
    motor[6] = PIDMIX(+0   ,+1   ,-1); //REAR
    motor[7] = PIDMIX(+1   ,+0   ,-1); //LEFT 
  #endif
  #ifdef OCTOFLATX
    motor[0] = PIDMIX(+1  ,-1/2,+1); //MIDFRONT_L
    motor[1] = PIDMIX(-1/2,-1  ,+1); //FRONT_R
    motor[2] = PIDMIX(-1  ,+1/2,+1); //MIDREAR_R
    motor[3] = PIDMIX(+1/2,+1  ,+1); //REAR_L
    motor[4] = PIDMIX(+1/2,-1  ,-1); //FRONT_L
    motor[5] = PIDMIX(-1  ,-1/2,-1); //MIDFRONT_R
    motor[6] = PIDMIX(-1/2,+1  ,-1); //REAR_R
    motor[7] = PIDMIX(+1  ,+1/2,-1); //MIDREAR_L 
  #endif

  #ifdef SERVO_TILT
    if ((rcOptions1 & activate1[BOXCAMSTAB]) || (rcOptions2 & activate2[BOXCAMSTAB])) {
      servo[1] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcData[AUX3]-1500 , TILT_PITCH_MIN, TILT_PITCH_MAX);
      servo[2] = constrain(TILT_ROLL_MIDDLE  + TILT_ROLL_PROP  * angle[ROLL]  /16 + rcData[AUX4]-1500, TILT_ROLL_MIN, TILT_ROLL_MAX);
    } else {
      servo[1] = constrain(TILT_PITCH_MIDDLE  + rcData[AUX3]-1500 , TILT_PITCH_MIN, TILT_PITCH_MAX);
      servo[2] = constrain(TILT_ROLL_MIDDLE   + rcData[AUX4]-1500,  TILT_ROLL_MIN, TILT_ROLL_MAX);
    }
  #endif
  #ifdef GIMBAL
    servo[1] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    servo[2] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP   * angle[ROLL]  /16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
  #endif
  #ifdef FLYING_WING
   motor[0] = rcCommand[THROTTLE];
    //if (passthroughMode) {// use raw stick values to drive output 
    // follow aux1 as being three way switch **NOTE: better to implement via check boxes in GUI 
    if (rcData[AUX1]<1300) { // passthrough
       servo[1]  = constrain(WING_LEFT_MID  + PITCH_DIRECTION_L * (rcData[PITCH]-MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL]-MIDRC), WING_LEFT_MIN,  WING_LEFT_MAX); //LEFT
       servo[2]  = constrain(WING_RIGHT_MID + PITCH_DIRECTION_R * (rcData[PITCH]-MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL]-MIDRC), WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
    } else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
       servo[1]  = constrain(WING_LEFT_MID  + PITCH_DIRECTION_L * axisPID[PITCH]        + ROLL_DIRECTION_L * axisPID[ROLL], WING_LEFT_MIN,  WING_LEFT_MAX); //LEFT
       servo[2]  = constrain(WING_RIGHT_MID + PITCH_DIRECTION_R * axisPID[PITCH]        + ROLL_DIRECTION_R * axisPID[ROLL], WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
    }
  #endif
  #if defined(CAMTRIG)
    if (camCycle==1) {
      if (camState == 0) {
        servo[3] = CAM_SERVO_HIGH;
        camState = 1;
        camTime = millis();
      } else if (camState == 1) {
       if ( (millis() - camTime) > CAM_TIME_HIGH ) {
         servo[3] = CAM_SERVO_LOW;
         camState = 2;
         camTime = millis();
       }
      } else { //camState ==2
       if ( (millis() - camTime) > CAM_TIME_LOW ) {
         camState = 0;
         camCycle = 0;
       }
      }
    }
    if ((rcOptions1 & activate1[BOXCAMTRIG]) || (rcOptions1 & activate2[BOXCAMTRIG])) camCycle=1;
  #endif

  maxMotor=motor[0];
  for(i=1;i< NUMBER_MOTOR;i++)
    if (motor[i]>maxMotor) maxMotor=motor[i];
  for (i = 0; i < NUMBER_MOTOR; i++) {
    if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
      motor[i] -= maxMotor - MAXTHROTTLE;
    motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE);    
    if ((rcData[THROTTLE]) < MINCHECK)
      #ifndef MOTOR_STOP
        motor[i] = MINTHROTTLE;
      #else
        motor[i] = MINCOMMAND;
      #endif
    if (armed == 0)
      motor[i] = MINCOMMAND;
  }

  #if defined(LOG_VALUES) || (POWERMETER == 1)
    logMotorsPower();
  #endif
}

regards Felix

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 3:24 pm

That's probably what i need..

I have build my HEX and gimbal for it... to find that MultiWii can't drive both at the time..
Somebody link me here.

I am not 100% good in english.. lol

Can i try a code for you .. ?
Gimball + HEX config ? (need 2 axis stabilisation on cam)

ronco · Post by **ronco** » Sun Jan 15, 2012 3:56 pm

Hi,
you can!

if you use it with sum reciver (else pin A0 and A1 is taken by motor 5 & 6)

download the dev 20111220 and replace the code from output with the posted one...
but please be carefull

[edit]! hex + gibal dosent work ATM .. ! ill post a working code soon :/ [/edit]

regards

Felix

ronco · Post by **ronco** » Sun Jan 15, 2012 4:30 pm

got it

now this should work ..

Code: Select all


#if defined(BI) || defined(TRI) || defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING) || defined(CAMTRIG)
  #define SERVO
#endif
#if defined(GIMBAL)
  #define NUMBER_MOTOR 0
#elif defined(FLYING_WING)
  #define NUMBER_MOTOR 1
#elif defined(BI)
  #define NUMBER_MOTOR 2
#elif defined(TRI)
  #define NUMBER_MOTOR 3
#elif defined(QUADP) || defined(QUADX) || defined(Y4)
  #define NUMBER_MOTOR 4
#elif defined(Y6) || defined(HEX6) || defined(HEX6X)
  #define NUMBER_MOTOR 6
#elif defined(OCTOX8) || defined(OCTOFLATP) || defined(OCTOFLATX)
  #define NUMBER_MOTOR 8
#endif

uint8_t PWM_PIN[8] = {MOTOR_ORDER};
volatile uint8_t atomicServo[4] = {125,125,125,125};

//for HEX Y6 and HEX6/HEX6X flat and for promini
volatile uint8_t atomicPWM_PIN5_lowState;
volatile uint8_t atomicPWM_PIN5_highState;
volatile uint8_t atomicPWM_PIN6_lowState;
volatile uint8_t atomicPWM_PIN6_highState;

volatile uint8_t atomicPWM_PINA2_lowState;
volatile uint8_t atomicPWM_PINA2_highState;
volatile uint8_t atomicPWM_PIN12_lowState;
volatile uint8_t atomicPWM_PIN12_highState;


void writeServos() {
  #if defined(SERVO)
    atomicServo[0] = (servo[0]-1000)/4;
    atomicServo[1] = (servo[1]-1000)/4;
    atomicServo[2] = (servo[2]-1000)/4;
    atomicServo[3] = (servo[3]-1000)/4;
  #endif
}

void writeMotors() { // [1000;2000] => [125;250]
  #if defined(MEGA)
    for(uint8_t i=0;i<NUMBER_MOTOR;i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
  #else
    for(uint8_t i=0;i<min(NUMBER_MOTOR,4);i++)
      analogWrite(PWM_PIN[i], motor[i]>>3);
    #if (NUMBER_MOTOR == 6)
      atomicPWM_PIN5_highState = ((motor[5]-1000)/4.08)+5;
      atomicPWM_PIN5_lowState = 255-atomicPWM_PIN5_highState;
      atomicPWM_PIN6_highState = ((motor[4]-1000)/4.08)+5;
      atomicPWM_PIN6_lowState = 255-atomicPWM_PIN6_highState;
    #endif
    #if (NUMBER_MOTOR == 8)
      atomicPWM_PINA2_highState = ((motor[6]-1000)/4.08)+5;
      atomicPWM_PINA2_lowState = 255-atomicPWM_PINA2_highState;
      atomicPWM_PIN12_highState = ((motor[7]-1000)/4.08)+5;
      atomicPWM_PIN12_lowState = 255-atomicPWM_PIN12_highState;
    #endif
  #endif
}

void writeAllMotors(int16_t mc) {   // Sends commands to all motors
  for (uint8_t i =0;i<NUMBER_MOTOR;i++)
    motor[i]=mc;
  writeMotors();
}

#if defined(LOG_VALUES) || (POWERMETER == 1)
void logMotorsPower() {
  uint32_t amp;
  /* true cubic function; when divided by vbat_max=126 (12.6V) for 3 cell battery this gives maximum value of ~ 1000 */
  const uint32_t amperes[64] =   {0,4,13,31,60,104,165,246,350,481,640,831,1056,1319,1622,1969,2361,2803,3297,3845,4451,5118,5848,6645,
                                7510,8448,9461,10551,11723,12978,14319,15750,17273,18892,20608,22425,24346,26374,28512,30762,33127,35611,
                                38215,40944,43799,46785,49903,53156,56548,60081,63759,67583,71558,75685,79968,84410,89013,93781,98716,103821,
                                109099,114553,120186,126000 };
  if (vbat) { // by all means - must avoid division by zero 
    for (uint8_t i =0;i<NUMBER_MOTOR;i++) {
      amp = amperes[(motor[i] - 1000)>>4] / vbat; // range mapped from [1000:2000] => [0:1000]; then break that up into 64 ranges; lookup amp
      #ifdef LOG_VALUES
         pMeter[i]+= amp; // sum up over time the mapped ESC input 
      #endif
      #if (POWERMETER == 1)
         pMeter[PMOTOR_SUM]+= amp; // total sum over all motors
      #endif
    }
  }
}
#endif

void initOutput() {
  for(uint8_t i=0;i<NUMBER_MOTOR;i++)
    pinMode(PWM_PIN[i],OUTPUT);
  writeAllMotors(1000);
  delay(300);
  #if defined(SERVO)
    initializeServo();
  #elif (NUMBER_MOTOR == 6) && defined(PROMINI)
    initializeSoftPWM();
    #if defined(A0_A1_PIN_HEX)
      pinMode(5,INPUT);pinMode(6,INPUT);     // we reactivate the INPUT affectation for these two PINs
      pinMode(A0,OUTPUT);pinMode(A1,OUTPUT);
    #endif
  #elif (NUMBER_MOTOR == 8) && defined(PROMINI)
    initializeSoftPWM();
    #if defined(A0_A1_PIN_HEX)
      pinMode(5,INPUT);pinMode(6,INPUT);     // we reactivate the INPUT affectation for these two PINs
      pinMode(A0,OUTPUT);pinMode(A1,OUTPUT);
      pinMode(A2,OUTPUT);pinMode(12,OUTPUT);
    #endif
  #endif
}

#if defined(SERVO)
void initializeServo() {
  #if defined(TRI)
    DIGITAL_SERVO_TRI_PINMODE;
  #endif
  #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
    DIGITAL_TILT_ROLL_PINMODE;
    DIGITAL_TILT_PITCH_PINMODE;
  #endif
  #if defined(CAMTRIG)
    DIGITAL_CAM_PINMODE;
  #endif
  #if defined(BI)
    DIGITAL_SERVO_TRI_PINMODE;
    DIGITAL_BI_LEFT_PINMODE;
  #endif
  TCCR0A = 0; // normal counting mode
  TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
  
  // timer 0B for hex with cam
  #if (NUMBER_MOTOR == 6) && defined(PROMINI)
    TIMSK0 |= (1<<OCIE0B); 
  #endif
}

// ****servo yaw with a 50Hz refresh rate****
// prescaler is set by default to 64 on Timer0
// Duemilanove : 16MHz / 64 => 4 us
// 256 steps = 1 counter cycle = 1024 us
// algorithm strategy:
// pulse high servo 0 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 0
// pulse high servo 1 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 1
// pulse high servo 2 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 2
// pulse high servo 3 -> do nothing for 1000 us -> do nothing for [0 to 1000] us -> pulse down servo 3
// do nothing for 14 x 1000 us
ISR(TIMER0_COMPA_vect) {
  static uint8_t state = 0;
  static uint8_t count;
  if (state == 0) {
    //http://billgrundmann.wordpress.com/2009/03/03/to-use-or-not-use-writedigital/
    #if defined(TRI) || defined (BI)
      DIGITAL_SERVO_TRI_HIGH;
    #endif
    OCR0A+= 250; // 1000 us
    state++ ;
  } else if (state == 1) {
    OCR0A+= atomicServo[0]; // 1000 + [0-1020] us
    state++;
  } else if (state == 2) {
    #if defined(TRI) || defined (BI)
      DIGITAL_SERVO_TRI_LOW;
    #endif
    #if defined(BI)
      DIGITAL_BI_LEFT_HIGH;
    #endif
    #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
      DIGITAL_TILT_PITCH_HIGH;
    #endif
    OCR0A+= 250; // 1000 us
    state++;
  } else if (state == 3) {
    OCR0A+= atomicServo[1]; // 1000 + [0-1020] us
    state++;
  } else if (state == 4) {
    #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
      DIGITAL_TILT_PITCH_LOW;
      DIGITAL_TILT_ROLL_HIGH;
    #endif
    #if defined(BI)
      DIGITAL_BI_LEFT_LOW;
    #endif
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 5) {
    OCR0A+= atomicServo[2]; // 1000 + [0-1020] us
    state++;
  } else if (state == 6) {
    #if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
      DIGITAL_TILT_ROLL_LOW;
    #endif
    #if defined(CAMTRIG)
      DIGITAL_CAM_HIGH;
    #endif
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 7) {
    OCR0A+= atomicServo[3]; // 1000 + [0-1020] us
    state++;
  } else if (state == 8) {
    #if defined(CAMTRIG)
      DIGITAL_CAM_LOW;
    #endif
    count = 10; // 12 x 1000 us
    state++;
    OCR0A+= 250; // 1000 us
  } else if (state == 9) {
    if (count > 0) count--;
    else state = 0;
    OCR0A+= 250;
  }
}
#endif

#if (NUMBER_MOTOR == 6 || NUMBER_MOTOR == 8) && defined(PROMINI)
void initializeSoftPWM() {
  // if there are servos its alredy done
  #if (NUMBER_MOTOR == 6) && not defined(SERVO)
    TCCR0A = 0; // normal counting mode
    TIMSK0 |= (1<<OCIE0B); // Enable CTC interrupt  
  #endif
  
  #if (NUMBER_MOTOR > 6)
    TCCR0A = 0; // normal counting mode
    TIMSK0 |= (1<<OCIE0A); // Enable CTC interrupt
    TIMSK0 |= (1<<OCIE0B);
  #endif
}
#if (NUMBER_MOTOR > 6)
  ISR(TIMER0_COMPA_vect) {
    static uint8_t state = 0;
    if(state == 0){
      PORTC |= 1<<2; //digital PIN A2 high
      OCR0A += atomicPWM_PINA2_highState;
      state = 1;
    }else if(state == 1){
      PORTB &= ~(1<<4); //digital PIN 12 low
      OCR0A += atomicPWM_PIN12_lowState;
      state = 2;
    }else if(state == 2){
      PORTB |= 1<<4; //digital PIN 12 high
      OCR0A += atomicPWM_PIN12_highState;
      state = 3;  
    }else if(state == 3){
      PORTC &= ~(1<<2); //digital PIN A2 low
      OCR0A += atomicPWM_PINA2_lowState;
      state = 0;   
    }
  }
#endif




ISR(TIMER0_COMPB_vect) { //the same with digital PIN 5 & 6 or A0 & A1 and OCR0B counter
  static uint8_t state = 0;
  if(state == 0){
    #if not defined(A0_A1_PIN_HEX)
      PORTD |= 1<<5; //digital PIN 5 high
    #else
      PORTC |= 1<<0;//PIN A0
    #endif
    OCR0B += atomicPWM_PIN5_highState;
    state = 1;
  }else if(state == 1){
    #if not defined(A0_A1_PIN_HEX)
      PORTD &= ~(1<<6);
    #else
      PORTC &= ~(1<<1);
    #endif
    OCR0B += atomicPWM_PIN6_lowState;
    state = 2;
  }else if(state == 2){
    #if not defined(A0_A1_PIN_HEX)
      PORTD |= 1<<6;
    #else
      PORTC |= 1<<1;//PIN A1
    #endif
    OCR0B += atomicPWM_PIN6_highState;
    state = 3;  
  }else if(state == 3){
    #if not defined(A0_A1_PIN_HEX)
      PORTD &= ~(1<<5); //digital PIN 5 low
    #else
      PORTC &= ~(1<<0);
    #endif 
    OCR0B += atomicPWM_PIN5_lowState;
    state = 0;   
  }
}
#endif


void mixTable() {
  int16_t maxMotor;
  uint8_t i,axis;
  static uint8_t camCycle = 0;
  static uint8_t camState = 0;
  static uint32_t camTime = 0;
  
  #define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z

  #if NUMBER_MOTOR > 3
    //prevent "yaw jump" during yaw correction
    axisPID[YAW] = constrain(axisPID[YAW],-100-abs(rcCommand[YAW]),+100+abs(rcCommand[YAW]));
  #endif
  #ifdef BI
    motor[0] = PIDMIX(+1, 0, 0); //LEFT
    motor[1] = PIDMIX(-1, 0, 0); //RIGHT        
    servo[0]  = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
    servo[1]  = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
  #endif
  #ifdef TRI
    motor[0] = PIDMIX( 0,+4/3, 0); //REAR
    motor[1] = PIDMIX(-1,-2/3, 0); //RIGHT
    motor[2] = PIDMIX(+1,-2/3, 0); //LEFT
    servo[0] = constrain(TRI_YAW_MIDDLE + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
  #endif
  #ifdef QUADP
    motor[0] = PIDMIX( 0,+1,-1); //REAR
    motor[1] = PIDMIX(-1, 0,+1); //RIGHT
    motor[2] = PIDMIX(+1, 0,+1); //LEFT
    motor[3] = PIDMIX( 0,-1,-1); //FRONT
  #endif
  #ifdef QUADX
    motor[0] = PIDMIX(-1,+1,-1); //REAR_R
    motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
    motor[2] = PIDMIX(+1,+1,+1); //REAR_L
    motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
  #endif
  #ifdef Y4
    motor[0] = PIDMIX(+0,+1,-1);   //REAR_1 CW
    motor[1] = PIDMIX(-1,-1, 0); //FRONT_R CCW
    motor[2] = PIDMIX(+0,+1,+1);   //REAR_2 CCW
    motor[3] = PIDMIX(+1,-1, 0); //FRONT_L CW
  #endif
  #ifdef Y6
    motor[0] = PIDMIX(+0,+4/3,+1); //REAR
    motor[1] = PIDMIX(-1,-2/3,-1); //RIGHT
    motor[2] = PIDMIX(+1,-2/3,-1); //LEFT
    motor[3] = PIDMIX(+0,+4/3,-1); //UNDER_REAR
    motor[4] = PIDMIX(-1,-2/3,+1); //UNDER_RIGHT
    motor[5] = PIDMIX(+1,-2/3,+1); //UNDER_LEFT    
  #endif
  #ifdef HEX6
    motor[0] = PIDMIX(-1/2,+1/2,+1); //REAR_R
    motor[1] = PIDMIX(-1/2,-1/2,-1); //FRONT_R
    motor[2] = PIDMIX(+1/2,+1/2,+1); //REAR_L
    motor[3] = PIDMIX(+1/2,-1/2,-1); //FRONT_L
    motor[4] = PIDMIX(+0  ,-1  ,+1); //FRONT
    motor[5] = PIDMIX(+0  ,+1  ,-1); //REAR
  #endif
  #ifdef HEX6X
    motor[0] = PIDMIX(-1/2,+1/2,+1); //REAR_R
    motor[1] = PIDMIX(-1/2,-1/2,+1); //FRONT_R
    motor[2] = PIDMIX(+1/2,+1/2,-1); //REAR_L
    motor[3] = PIDMIX(+1/2,-1/2,-1); //FRONT_L
    motor[4] = PIDMIX(-1  ,+0  ,-1); //RIGHT
    motor[5] = PIDMIX(+1  ,+0  ,+1); //LEFT
  #endif
  #ifdef OCTOX8
    motor[0] = PIDMIX(-1,+1,-1); //REAR_R
    motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
    motor[2] = PIDMIX(+1,+1,+1); //REAR_L
    motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
    motor[4] = PIDMIX(-1,+1,+1); //UNDER_REAR_R
    motor[5] = PIDMIX(-1,-1,-1); //UNDER_FRONT_R
    motor[6] = PIDMIX(+1,+1,-1); //UNDER_REAR_L
    motor[7] = PIDMIX(+1,-1,+1); //UNDER_FRONT_L
  #endif
  #ifdef OCTOFLATP
    motor[0] = PIDMIX(+7/10,-7/10,+1); //FRONT_L
    motor[1] = PIDMIX(-7/10,-7/10,+1); //FRONT_R
    motor[2] = PIDMIX(-7/10,+7/10,+1); //REAR_R
    motor[3] = PIDMIX(+7/10,+7/10,+1); //REAR_L
    motor[4] = PIDMIX(+0   ,-1   ,-1); //FRONT
    motor[5] = PIDMIX(-1   ,+0   ,-1); //RIGHT
    motor[6] = PIDMIX(+0   ,+1   ,-1); //REAR
    motor[7] = PIDMIX(+1   ,+0   ,-1); //LEFT 
  #endif
  #ifdef OCTOFLATX
    motor[0] = PIDMIX(+1  ,-1/2,+1); //MIDFRONT_L
    motor[1] = PIDMIX(-1/2,-1  ,+1); //FRONT_R
    motor[2] = PIDMIX(-1  ,+1/2,+1); //MIDREAR_R
    motor[3] = PIDMIX(+1/2,+1  ,+1); //REAR_L
    motor[4] = PIDMIX(+1/2,-1  ,-1); //FRONT_L
    motor[5] = PIDMIX(-1  ,-1/2,-1); //MIDFRONT_R
    motor[6] = PIDMIX(-1/2,+1  ,-1); //REAR_R
    motor[7] = PIDMIX(+1  ,+1/2,-1); //MIDREAR_L 
  #endif

  #ifdef SERVO_TILT
    if ((rcOptions1 & activate1[BOXCAMSTAB]) || (rcOptions2 & activate2[BOXCAMSTAB])) {
      servo[1] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcData[AUX3]-1500 , TILT_PITCH_MIN, TILT_PITCH_MAX);
      servo[2] = constrain(TILT_ROLL_MIDDLE  + TILT_ROLL_PROP  * angle[ROLL]  /16 + rcData[AUX4]-1500, TILT_ROLL_MIN, TILT_ROLL_MAX);
    } else {
      servo[1] = constrain(TILT_PITCH_MIDDLE  + rcData[AUX3]-1500 , TILT_PITCH_MIN, TILT_PITCH_MAX);
      servo[2] = constrain(TILT_ROLL_MIDDLE   + rcData[AUX4]-1500,  TILT_ROLL_MIN, TILT_ROLL_MAX);
    }
  #endif
  #ifdef GIMBAL
    servo[1] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    servo[2] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP   * angle[ROLL]  /16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
  #endif
  #ifdef FLYING_WING
   motor[0] = rcCommand[THROTTLE];
    //if (passthroughMode) {// use raw stick values to drive output 
    // follow aux1 as being three way switch **NOTE: better to implement via check boxes in GUI 
    if (rcData[AUX1]<1300) { // passthrough
       servo[1]  = constrain(WING_LEFT_MID  + PITCH_DIRECTION_L * (rcData[PITCH]-MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL]-MIDRC), WING_LEFT_MIN,  WING_LEFT_MAX); //LEFT
       servo[2]  = constrain(WING_RIGHT_MID + PITCH_DIRECTION_R * (rcData[PITCH]-MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL]-MIDRC), WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
    } else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
       servo[1]  = constrain(WING_LEFT_MID  + PITCH_DIRECTION_L * axisPID[PITCH]        + ROLL_DIRECTION_L * axisPID[ROLL], WING_LEFT_MIN,  WING_LEFT_MAX); //LEFT
       servo[2]  = constrain(WING_RIGHT_MID + PITCH_DIRECTION_R * axisPID[PITCH]        + ROLL_DIRECTION_R * axisPID[ROLL], WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
    }
  #endif
  #if defined(CAMTRIG)
    if (camCycle==1) {
      if (camState == 0) {
        servo[3] = CAM_SERVO_HIGH;
        camState = 1;
        camTime = millis();
      } else if (camState == 1) {
       if ( (millis() - camTime) > CAM_TIME_HIGH ) {
         servo[3] = CAM_SERVO_LOW;
         camState = 2;
         camTime = millis();
       }
      } else { //camState ==2
       if ( (millis() - camTime) > CAM_TIME_LOW ) {
         camState = 0;
         camCycle = 0;
       }
      }
    }
    if ((rcOptions1 & activate1[BOXCAMTRIG]) || (rcOptions1 & activate2[BOXCAMTRIG])) camCycle=1;
  #endif

  maxMotor=motor[0];
  for(i=1;i< NUMBER_MOTOR;i++)
    if (motor[i]>maxMotor) maxMotor=motor[i];
  for (i = 0; i < NUMBER_MOTOR; i++) {
    if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
      motor[i] -= maxMotor - MAXTHROTTLE;
    motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE);    
    if ((rcData[THROTTLE]) < MINCHECK)
      #ifndef MOTOR_STOP
        motor[i] = MINTHROTTLE;
      #else
        motor[i] = MINCOMMAND;
      #endif
    if (armed == 0)
      motor[i] = MINCOMMAND;
  }

  #if defined(LOG_VALUES) || (POWERMETER == 1)
    logMotorsPower();
  #endif
}

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 6:51 pm

Ok just to be sure:

#define HEX6X
#define A0_A1_PIN_HEX
#define SERVO_TILT

So Motor 5 and 6 now A0 A1 (i was already like that)

Pin 5 and Pin 6 for Stab ?

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 7:04 pm

I guess not cause the two motor (5 6) start turning from power up.
Since thy don't have signal probably

ronco · Post by **ronco** » Sun Jan 15, 2012 7:15 pm

dialfonzo wrote:Ok just to be sure:

#define HEX6X
#define A0_A1_PIN_HEX
#define SERVO_TILT

So Motor 5 and 6 now A0 A1 (i was already like that)

Pin 5 and Pin 6 for Stab ?

hi,

and no

uncomment "#define A0_A1_PIN_HEX" and use pin 5 & 6 for the motors and A0 and A1 for gimbal

regards Felix

ronco · Post by **ronco** » Sun Jan 15, 2012 7:24 pm

mhh sorry i got also problems with my english -.-

use
#define HEX6X
#define SERVO_TILT

dont use A0_A1_PIN_HEX

then pin 5 & 6 for motors and A0 & A1for SERVO_TILT

regards Felix

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 7:25 pm

Ok

Camera Stabilisation now work.
Adjusting the value to have it level.

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 7:35 pm

And that's the result with CamStab activated on A0 and A1

http://www.youtube.com/watch?v=b07TozR6 ... 2l1JWjjvCU

ronco · Post by **ronco** » Sun Jan 15, 2012 7:40 pm

that looks good

but does it still fly ?

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 7:42 pm

Tested Motors but don't look to have any signal on 5 and 6
ESC are beeping like no signal and don't spin when the copter is armed.

I attach my config files

ronco · Post by **ronco** » Sun Jan 15, 2012 7:59 pm

dialfonzo wrote:Tested Motors but don't look to have any signal on 5 and 6
ESC are beeping like no signal and don't spin when the copter is armed.

I attach my config files

ok.. you are using the wrong (old and not working) output.ino code... use this one viewtopic.php?f=8&t=1069&start=20#p7753

or the attached file

regards Felix

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 8:03 pm

ronco wrote:
dialfonzo wrote:Tested Motors but don't look to have any signal on 5 and 6
ESC are beeping like no signal and don't spin when the copter is armed.

I attach my config files

ok.. you are using the wrong (old and not working) output.ino code... use this one viewtopic.php?f=8&t=1069&start=20#p7753

or the attached file

regards Felix

Ok thanks...!
Now all motor spin when armed.. need to do a fly test i guess...!

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 9:09 pm

A big thanks to ronco and other who may have work for that mod...!

Awesome it fly just as it should.
Camera Stabilisation A0 and A1
HEX configuration with motor on pin5 and pin6

Uploading a video and will add it.
Sorry but my servo have some play so the cam move a bit..

UPLOADED....!!!
http://www.youtube.com/watch?v=MJghxAHC ... e=youtu.be

KeesvR · Post by **KeesvR** » Sun Jan 15, 2012 9:26 pm

I'm a happy person :mrgreen:

who made you happy.

dialfonzo · Post by **dialfonzo** » Sun Jan 15, 2012 9:32 pm

KeesvR wrote:I'm a happy person who made you happy.

HéHé...

I'm really happy now..!
350mb of video take time to upload..

Curiously that DEV version feel more stable than the 1.9

KeesvR · Post by **KeesvR** » Sun Jan 15, 2012 9:56 pm

Yes for me the dev1220 is much better because of the 8g change for my BMA020.

ronco · Post by **ronco** » Sun Jan 15, 2012 10:34 pm

thanks for testing it

regards Felix