Baser Kandehir / brushlessController_TB6612FNG Featured

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?

UserRevisionLine numberNew 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 }