Brushless motor control library with TB6612FNG
Dependents: brushless_TB6612FNG gimbalController_brushless_IMU
brushlessController_TB6612FNG.cpp@2:32d402d0ee1e, 2015-07-21 (annotated)
- Committer:
- BaserK
- Date:
- Tue Jul 21 08:01:16 2015 +0000
- Revision:
- 2:32d402d0ee1e
- Parent:
- 1:b52603b6a822
MIT license is added
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BaserK | 0:fcf00057b1a3 | 1 | /* Brushless motor control library with TB6612FNG |
BaserK | 0:fcf00057b1a3 | 2 | * |
BaserK | 0:fcf00057b1a3 | 3 | * @author: Baser Kandehir |
BaserK | 0:fcf00057b1a3 | 4 | * @date: July 17, 2015 |
BaserK | 2:32d402d0ee1e | 5 | * @license: MIT license |
BaserK | 2:32d402d0ee1e | 6 | * |
BaserK | 2:32d402d0ee1e | 7 | * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr |
BaserK | 2:32d402d0ee1e | 8 | * |
BaserK | 2:32d402d0ee1e | 9 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
BaserK | 2:32d402d0ee1e | 10 | * of this software and associated documentation files (the "Software"), to deal |
BaserK | 2:32d402d0ee1e | 11 | * in the Software without restriction, including without limitation the rights |
BaserK | 2:32d402d0ee1e | 12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
BaserK | 2:32d402d0ee1e | 13 | * copies of the Software, and to permit persons to whom the Software is |
BaserK | 2:32d402d0ee1e | 14 | * furnished to do so, subject to the following conditions: |
BaserK | 2:32d402d0ee1e | 15 | * |
BaserK | 2:32d402d0ee1e | 16 | * The above copyright notice and this permission notice shall be included in |
BaserK | 2:32d402d0ee1e | 17 | * all copies or substantial portions of the Software. |
BaserK | 2:32d402d0ee1e | 18 | * |
BaserK | 2:32d402d0ee1e | 19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
BaserK | 2:32d402d0ee1e | 20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
BaserK | 2:32d402d0ee1e | 21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
BaserK | 2:32d402d0ee1e | 22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
BaserK | 2:32d402d0ee1e | 23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
BaserK | 2:32d402d0ee1e | 24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
BaserK | 2:32d402d0ee1e | 25 | * THE SOFTWARE. |
BaserK | 2:32d402d0ee1e | 26 | * |
BaserK | 0:fcf00057b1a3 | 27 | * @brief: |
BaserK | 0:fcf00057b1a3 | 28 | * |
BaserK | 0:fcf00057b1a3 | 29 | * This a brushless motor control library. Generally brushless motors can be controlled |
BaserK | 0:fcf00057b1a3 | 30 | * using ESC s and motor drivers like L6234,L6235 etc. I previously control a brushless motor |
BaserK | 0:fcf00057b1a3 | 31 | * with 2x L293D. Motor was a little bit shaking and its movement was not precise. The reason for that |
BaserK | 0:fcf00057b1a3 | 32 | * was my way of driving the motor was a little bit wrong and max pwm freq. for L293D was restricted to |
BaserK | 0:fcf00057b1a3 | 33 | * 5 kHz. Now I am using 2x TB6612FNG which has fast response and supports max switching freq. up to 100 kHz. |
BaserK | 0:fcf00057b1a3 | 34 | * The movement is really precise and results are remarkable. What makes this movement so precise is the way |
BaserK | 0:fcf00057b1a3 | 35 | * we control pwm outputs. Before then I was just using digital outputs and it didnt give perfect results. |
BaserK | 0:fcf00057b1a3 | 36 | * Now I am using the pwm method which creates sin wave like shapes at the outputs with 120 deg phase shift. |
BaserK | 0:fcf00057b1a3 | 37 | * I learned the method from this website, check out the website for more information: |
BaserK | 0:fcf00057b1a3 | 38 | * |
BaserK | 0:fcf00057b1a3 | 39 | * http://www.berryjam.eu/2015/04/driving-bldc-gimbals-at-super-slow-speeds-with-arduino/ |
BaserK | 0:fcf00057b1a3 | 40 | * |
BaserK | 0:fcf00057b1a3 | 41 | */ |
BaserK | 0:fcf00057b1a3 | 42 | |
BaserK | 0:fcf00057b1a3 | 43 | #include "brushlessController_TB6612FNG.h" |
BaserK | 0:fcf00057b1a3 | 44 | |
BaserK | 0:fcf00057b1a3 | 45 | PwmOut pwm[] = {(p21),(p22),(p24)}; // PWM outputs to drive brushless motor |
BaserK | 0:fcf00057b1a3 | 46 | |
BaserK | 0:fcf00057b1a3 | 47 | int step[3]; // Current steps of three pwm outputs |
BaserK | 0:fcf00057b1a3 | 48 | int sinArraySize; // Size of pwmSin array |
BaserK | 0:fcf00057b1a3 | 49 | int phaseShift; // Phase shift between outputs |
BaserK | 0:fcf00057b1a3 | 50 | bool firstTime = 1; |
BaserK | 0:fcf00057b1a3 | 51 | |
BaserK | 0:fcf00057b1a3 | 52 | /* Magical data array to drive the brushless motor */ |
BaserK | 0:fcf00057b1a3 | 53 | const int pwmSin[] = { // pwmSin data array consists of 360 samples |
BaserK | 0:fcf00057b1a3 | 54 | 128, 132, 136, 140, 143, 147, 151, 155, 159, 162, 166, 170, 174, 178, 181, 185, 189, 192, 196, 200, |
BaserK | 0:fcf00057b1a3 | 55 | 203, 207, 211, 214, 218, 221, 225, 228, 232, 235, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, |
BaserK | 0:fcf00057b1a3 | 56 | 248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, |
BaserK | 0:fcf00057b1a3 | 57 | 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, |
BaserK | 0:fcf00057b1a3 | 58 | 248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, |
BaserK | 0:fcf00057b1a3 | 59 | 248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, |
BaserK | 0:fcf00057b1a3 | 60 | 255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, |
BaserK | 0:fcf00057b1a3 | 61 | 248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 235, 232, 228, 225, 221, 218, 214, 211, 207, |
BaserK | 0:fcf00057b1a3 | 62 | 203, 200, 196, 192, 189, 185, 181, 178, 174, 170, 166, 162, 159, 155, 151, 147, 143, 140, 136, 132, |
BaserK | 0:fcf00057b1a3 | 63 | 128, 124, 120, 116, 113, 109, 105, 101, 97, 94, 90, 86, 82, 78, 75, 71, 67, 64, 60, 56, |
BaserK | 0:fcf00057b1a3 | 64 | 53, 49, 45, 42, 38, 35, 31, 28, 24, 21, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, |
BaserK | 0:fcf00057b1a3 | 65 | 8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, |
BaserK | 0:fcf00057b1a3 | 66 | 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, |
BaserK | 0:fcf00057b1a3 | 67 | 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, |
BaserK | 0:fcf00057b1a3 | 68 | 8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, |
BaserK | 0:fcf00057b1a3 | 69 | 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, |
BaserK | 0:fcf00057b1a3 | 70 | 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 21, 24, 28, 31, 35, 38, 42, 45, 49, |
BaserK | 0:fcf00057b1a3 | 71 | 53, 56, 60, 64, 67, 71, 75, 78, 82, 86, 90, 94, 97, 101, 105, 109, 113, 116, 120, 124 |
BaserK | 0:fcf00057b1a3 | 72 | }; |
BaserK | 0:fcf00057b1a3 | 73 | |
BaserK | 0:fcf00057b1a3 | 74 | /* Brushless motor control function |
BaserK | 0:fcf00057b1a3 | 75 | * |
BaserK | 0:fcf00057b1a3 | 76 | * @brief: This function can control a brushless motor PRECISELY with TB6612FNG. |
BaserK | 0:fcf00057b1a3 | 77 | * It initializes the 3 pwm outputs with 20 kHz frequency. Function takes data |
BaserK | 0:fcf00057b1a3 | 78 | * from the pwmSin array and gives the data to the 3 pwm outputs with 120 deg |
BaserK | 0:fcf00057b1a3 | 79 | * phase shift. By doing so it can control a brushless motor precisely with |
BaserK | 0:fcf00057b1a3 | 80 | * simple DC motor drivers. |
BaserK | 0:fcf00057b1a3 | 81 | * |
BaserK | 0:fcf00057b1a3 | 82 | * @variables: |
BaserK | 0:fcf00057b1a3 | 83 | * dir: Direction of the motor |
BaserK | 1:b52603b6a822 | 84 | * delay_time (us): Time delay between steps |
BaserK | 0:fcf00057b1a3 | 85 | */ |
BaserK | 0:fcf00057b1a3 | 86 | void brushlessControl(bool dir, int delay_time) |
BaserK | 0:fcf00057b1a3 | 87 | { |
BaserK | 0:fcf00057b1a3 | 88 | if(firstTime == 1) // do it only once (initialization process) |
BaserK | 0:fcf00057b1a3 | 89 | { |
BaserK | 0:fcf00057b1a3 | 90 | for(int i=0; i<3; i++) |
BaserK | 0:fcf00057b1a3 | 91 | pwm[i].period(1/20000.0); // 20 kHz pwm frequency (inaudible) |
BaserK | 0:fcf00057b1a3 | 92 | |
BaserK | 0:fcf00057b1a3 | 93 | sinArraySize = sizeof(pwmSin) / sizeof(int); // 360 for this case |
BaserK | 0:fcf00057b1a3 | 94 | phaseShift = sinArraySize / 3 ; // 120 for the above case |
BaserK | 0:fcf00057b1a3 | 95 | |
BaserK | 0:fcf00057b1a3 | 96 | step[0] = 0; |
BaserK | 0:fcf00057b1a3 | 97 | step[1] = step[0] + phaseShift; |
BaserK | 0:fcf00057b1a3 | 98 | step[2] = step[1] + phaseShift; |
BaserK | 0:fcf00057b1a3 | 99 | |
BaserK | 0:fcf00057b1a3 | 100 | firstTime = 0; |
BaserK | 0:fcf00057b1a3 | 101 | } |
BaserK | 0:fcf00057b1a3 | 102 | |
BaserK | 0:fcf00057b1a3 | 103 | for(int k=0; k<3; k++) |
BaserK | 0:fcf00057b1a3 | 104 | { |
BaserK | 0:fcf00057b1a3 | 105 | /* PWM duty cycles are determined by the step numbers of the outputs, |
BaserK | 0:fcf00057b1a3 | 106 | corresponding data is taken from pwmSin array */ |
BaserK | 0:fcf00057b1a3 | 107 | pwm[k] = pwmSin[step[k]]/255.0; // Pwm takes values between 0 and 1.0 (float) |
BaserK | 0:fcf00057b1a3 | 108 | |
BaserK | 0:fcf00057b1a3 | 109 | (dir == 1)? (step[k]++):(step[k]--); // Motor direction control |
BaserK | 0:fcf00057b1a3 | 110 | |
BaserK | 0:fcf00057b1a3 | 111 | /* If array index exceeds the array size, start over */ |
BaserK | 0:fcf00057b1a3 | 112 | if(step[k] == sinArraySize - 1) step[k] = 0; |
BaserK | 0:fcf00057b1a3 | 113 | if(step[k] < 0) step[k] = sinArraySize - 1; |
BaserK | 0:fcf00057b1a3 | 114 | } |
BaserK | 1:b52603b6a822 | 115 | wait_us(delay_time); |
BaserK | 0:fcf00057b1a3 | 116 | } |