Skip to content
This repository
branch: master
Fetching contributors…

Cannot retrieve contributors at this time

file 170 lines (152 sloc) 7.401 kb
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170
/*
AeroQuad v3.0.1 - February 2012
www.AeroQuad.com
Copyright (c) 2012 Ted Carancho. All rights reserved.
An Open Source Arduino based multicopter.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/


#ifndef _AEROQUAD_MOTORS_PWM_TIMER_H_
#define _AEROQUAD_MOTORS_PWM_TIMER_H_

///***********************************************************/
///********************* PWMtimer Motors *********************/
///***********************************************************/
//// Special thanks to CupOfTea for authorting this class
//// http://aeroquad.com/showthread.php?1553-Timed-Motors_PWM
//// Uses system timers directly instead of analogWrite
///*Some basics about the 16 bit timer:
//- The timer counts clock ticks derived from the CPU clock. Using 16MHz CPU clock
// and a prescaler of 8 gives a timer clock of 2MHz, one tick every 0.5us. This
// is also called timer resolution.
//- The timer is used as cyclic upwards counter, the counter period is set in the
// ICRx register. IIRC period-1 has to be set in the ICRx register.
//- When the counter reaches 0, the outputs are set
//- When the counter reaches OCRxy, the corresponding output is cleared.
//In the code below, the period shall be 3.3ms (300hz), so the ICRx register is
// set to 6600 ticks of 0.5us/tick. It probably should be 6599, but who cares about
// this 0.5us for the period. This value is #define TOP
//The high time shall be 1000us, so the OCRxy register is set to 2000. In the code
// below this can be seen in the line "commandAllMotors(1000);" A change of
// the timer period does not change this setting, as the clock rate is still one
// tick every 0.5us. If the prescaler was changed, the OCRxy register value would
// be different.
//*/
///* Motor Mega Pin Port Uno Pin Port HEXA Mega Pin Port
// FRONT 2 PE4 3 PD3
// REAR 3 PE5 9 PB1
// RIGHT 5 PE3 10 PB2 7 PH4
// LEFT 6 PH3 11 PB3 8 PH5
//*/

#include "Arduino.h"

#include "Motors.h"

#if defined (USE_400HZ_ESC)
#define PWM_FREQUENCY 400 // in Hz
#else
#define PWM_FREQUENCY 300 // in Hz
#endif

#define PWM_PRESCALER 8
#define PWM_COUNTER_PERIOD (F_CPU/PWM_PRESCALER/PWM_FREQUENCY)

void initializeMotors(NB_Motors numbers) {
  numberOfMotors = numbers;

#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
    DDRE = DDRE | B00111000; // Set ports to output PE3-5, OC3A, OC3B, OC3C
    if (numberOfMotors == FOUR_Motors) {
      DDRH = DDRH | B00001000; // Set port to output PH3, OC4A
    }
    else if (numberOfMotors == SIX_Motors) { // for 6
      DDRH = DDRH | B00111000; // Set ports to output PH3-5, OC4A, OC4B, OC4C
    }
    else if (numberOfMotors == EIGHT_Motors) { // for 8 motor
      DDRH = DDRH | B00111000; // Set ports to output PH3-5, OC4A, OC4B, OC4C
      DDRB = DDRB | B01100000; // OC1A, OC1B
    }
#else
    DDRB = DDRB | B00001110; // Set ports to output PB1-3
    DDRD = DDRD | B00001000; // Set port to output PD3
#endif

  commandAllMotors(1000); // Initialise motors to 1000us (stopped)

#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
    // Init PWM Timer 3 // WGMn1 WGMn2 WGMn3 = Mode 14 Fast PWM, TOP = ICRn ,Update of OCRnx at BOTTOM
    TCCR3A = (1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1); // Clear OCnA/OCnB/OCnC on compare match, set OCnA/OCnB/OCnC at BOTTOM (non-inverting mode)
    TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31); // Prescaler set to 8, that gives us a resolution of 0.5us
    ICR3 = PWM_COUNTER_PERIOD; // Clock_speed / ( Prescaler * desired_PWM_Frequency) #defined above.
    if (numberOfMotors == FOUR_Motors) {
      // Init PWM Timer 4
      TCCR4A = (1<<WGM41)|(1<<COM4A1);
      TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
      ICR4 = PWM_COUNTER_PERIOD;
    }
    else if ((numberOfMotors == SIX_Motors) || (numberOfMotors == EIGHT_Motors)) { // for 8 motors
      // Init PWM Timer 4
      TCCR4A = (1<<WGM41)|(1<<COM4A1)|(1<<COM4B1)|(1<<COM4C1);
      TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
      ICR4 = PWM_COUNTER_PERIOD;
    }
if (numberOfMotors == EIGHT_Motors){ // for 8 motors
// Init PWM Timer 1
TCCR1A = (1<<WGM11)|(1<<COM1A1)|(1<<COM1B1);
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
      ICR1 = PWM_COUNTER_PERIOD;
    }
#else
    // Init PWM Timer 1 16 bit
    TCCR1A = (1<<WGM11)|(1<<COM1A1)|(1<<COM1B1);
    TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
    ICR1 = PWM_COUNTER_PERIOD;
    // Init PWM Timer 2 8bit // WGMn1 WGMn2 = Mode ? Fast PWM, TOP = 0xFF ,Update of OCRnx at BOTTOM
    TCCR2A = (1<<WGM20)|(1<<WGM21)|(1<<COM2A1)|(1<<COM2B1); // Clear OCnA/OCnB on compare match, set OCnA/OCnB at BOTTOM (non-inverting mode)
    TCCR2B = (1<<CS22)|(1<<CS21); // Prescaler set to 256, that gives us a resolution of 16us
    // TOP is fixed at 255 // Output_PWM_Frequency = 244hz = 16000000/(256*(1+255)) = Clock_Speed / (Prescaler * (1 + TOP))
#endif
}

void writeMotors() {
#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
    OCR3B = motorCommand[MOTOR1] * 2 ;
    OCR3C = motorCommand[MOTOR2] * 2 ;
    OCR3A = motorCommand[MOTOR3] * 2 ;
    OCR4A = motorCommand[MOTOR4] * 2 ;
    if (numberOfMotors == SIX_Motors || numberOfMotors == EIGHT_Motors) {
      OCR4B = motorCommand[MOTOR5] * 2 ;
      OCR4C = motorCommand[MOTOR6] * 2 ;
    }
if (numberOfMotors == EIGHT_Motors) {
OCR1A = motorCommand[MOTOR7] * 2 ;
      OCR1B = motorCommand[MOTOR8] * 2 ;
}
#else
    OCR2B = motorCommand[MOTOR1] / 16 ; // 1000-2000 to 128-256
    OCR1A = motorCommand[MOTOR2] * 2 ;
    OCR1B = motorCommand[MOTOR3] * 2 ;
    OCR2A = motorCommand[MOTOR4] / 16 ;
#endif
}

void commandAllMotors(int command) {
#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
    OCR3B = command * 2 ;
    OCR3C = command * 2 ;
    OCR3A = command * 2 ;
    OCR4A = command * 2 ;
    if (numberOfMotors == SIX_Motors || numberOfMotors == EIGHT_Motors) {
      OCR4B = command * 2 ;
      OCR4C = command * 2 ;
    }
    if (numberOfMotors == EIGHT_Motors) {
      OCR1A = command * 2 ;
      OCR1B = command * 2 ;
    }
#else
    OCR2B = command / 16 ;
    OCR1A = command * 2 ;
    OCR1B = command * 2 ;
    OCR2A = command / 16 ;
#endif
}

#endif
Something went wrong with that request. Please try again.