Brushless motor control library with TB6612FNG
Dependents: brushless_TB6612FNG gimbalController_brushless_IMU
Diff: brushlessController_TB6612FNG.cpp
- Revision:
- 0:fcf00057b1a3
- Child:
- 1:b52603b6a822
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/brushlessController_TB6612FNG.cpp Fri Jul 17 11:29:51 2015 +0000 @@ -0,0 +1,96 @@ +/* Brushless motor control library with TB6612FNG +* +* @author: Baser Kandehir +* @date: July 17, 2015 +* @license: Use this code however you'd like +* +* @brief: +* +* This a brushless motor control library. Generally brushless motors can be controlled +* using ESC s and motor drivers like L6234,L6235 etc. I previously control a brushless motor +* with 2x L293D. Motor was a little bit shaking and its movement was not precise. The reason for that +* was my way of driving the motor was a little bit wrong and max pwm freq. for L293D was restricted to +* 5 kHz. Now I am using 2x TB6612FNG which has fast response and supports max switching freq. up to 100 kHz. +* The movement is really precise and results are remarkable. What makes this movement so precise is the way +* we control pwm outputs. Before then I was just using digital outputs and it didnt give perfect results. +* Now I am using the pwm method which creates sin wave like shapes at the outputs with 120 deg phase shift. +* I learned the method from this website, check out the website for more information: +* +* http://www.berryjam.eu/2015/04/driving-bldc-gimbals-at-super-slow-speeds-with-arduino/ +* +*/ + +#include "brushlessController_TB6612FNG.h" + +PwmOut pwm[] = {(p21),(p22),(p24)}; // PWM outputs to drive brushless motor + +int step[3]; // Current steps of three pwm outputs +int sinArraySize; // Size of pwmSin array +int phaseShift; // Phase shift between outputs +bool firstTime = 1; + +/* Magical data array to drive the brushless motor */ +const int pwmSin[] = { // pwmSin data array consists of 360 samples +128, 132, 136, 140, 143, 147, 151, 155, 159, 162, 166, 170, 174, 178, 181, 185, 189, 192, 196, 200, +203, 207, 211, 214, 218, 221, 225, 228, 232, 235, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, +248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, +255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, +248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, +248, 248, 249, 250, 250, 251, 252, 252, 253, 253, 253, 254, 254, 254, 255, 255, 255, 255, 255, 255, +255, 255, 255, 255, 255, 255, 255, 254, 254, 254, 253, 253, 253, 252, 252, 251, 250, 250, 249, 248, +248, 247, 246, 245, 244, 243, 242, 241, 240, 239, 238, 235, 232, 228, 225, 221, 218, 214, 211, 207, +203, 200, 196, 192, 189, 185, 181, 178, 174, 170, 166, 162, 159, 155, 151, 147, 143, 140, 136, 132, +128, 124, 120, 116, 113, 109, 105, 101, 97, 94, 90, 86, 82, 78, 75, 71, 67, 64, 60, 56, +53, 49, 45, 42, 38, 35, 31, 28, 24, 21, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, +8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, +1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, +8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, +8, 8, 7, 6, 6, 5, 4, 4, 3, 3, 3, 2, 2, 2, 1, 1, 1, 1, 1, 1, +1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 6, 6, 7, 8, +8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 21, 24, 28, 31, 35, 38, 42, 45, 49, +53, 56, 60, 64, 67, 71, 75, 78, 82, 86, 90, 94, 97, 101, 105, 109, 113, 116, 120, 124 +}; + +/* Brushless motor control function +* +* @brief: This function can control a brushless motor PRECISELY with TB6612FNG. +* It initializes the 3 pwm outputs with 20 kHz frequency. Function takes data +* from the pwmSin array and gives the data to the 3 pwm outputs with 120 deg +* phase shift. By doing so it can control a brushless motor precisely with +* simple DC motor drivers. +* +* @variables: +* dir: Direction of the motor +* delay_time: Time delay between steps +*/ +void brushlessControl(bool dir, int delay_time) +{ + if(firstTime == 1) // do it only once (initialization process) + { + for(int i=0; i<3; i++) + pwm[i].period(1/20000.0); // 20 kHz pwm frequency (inaudible) + + sinArraySize = sizeof(pwmSin) / sizeof(int); // 360 for this case + phaseShift = sinArraySize / 3 ; // 120 for the above case + + step[0] = 0; + step[1] = step[0] + phaseShift; + step[2] = step[1] + phaseShift; + + firstTime = 0; + } + + for(int k=0; k<3; k++) + { + /* PWM duty cycles are determined by the step numbers of the outputs, + corresponding data is taken from pwmSin array */ + pwm[k] = pwmSin[step[k]]/255.0; // Pwm takes values between 0 and 1.0 (float) + + (dir == 1)? (step[k]++):(step[k]--); // Motor direction control + + /* If array index exceeds the array size, start over */ + if(step[k] == sinArraySize - 1) step[k] = 0; + if(step[k] < 0) step[k] = sinArraySize - 1; + } + wait_ms(delay_time); +} \ No newline at end of file