main.cpp

00001  /**    
00002  * @file    main.cpp
00003  * @brief   Bipolar Steppr Motor Control with dual H Bridge
00004  * @details Software plays stepper motor lookup tables for dual h bridge control.
00005  *                  
00006  *
00007  * @Author  Jeroen Lodder
00008  * @Date    September 2013
00009  * @version 1
00010  *
00011  */
00012 
00013 #include "mbed.h"
00014 #include "smclut.h"
00015 
00016     
00017     /* MOSFET io 
00018      *  FET         L1  L2  H1  H2  H3  H4  L3  L4
00019      *  Motor       MA                          MB          
00020      *  mbed        p21 p22 p25 p26 p27 p28 p23 p24
00021      * ------------------------ V+
00022      * |            |            |          |       
00023      * \H1      \H2      \H3        \H4 
00024      * |--MA--|          |--MB--|       
00025      * \L1      \L2      \L3        \L4 
00026      * |            |            |          |       
00027      * ------------------------ V- 
00028      */
00029 
00030 /* Fet driver definitions */
00031     /* Turn off time fet driver in micro seconds (uS) */
00032     #define TURN_ON_TIME_FET_DRIVER 750
00033 
00034 /* Control definitions */
00035     /* Mutliplier of speed wait time setting */ 
00036     #define SPEED_SCALING   0.5
00037     
00038 /* Motor control definitions */
00039     #define PWM_PERIOD          512   // Q15
00040     #define PWM_AMPLITIUDE  1           // 0 to 1, equals torque
00041     #define PHASE_SHIFT         1           // Shift in rads 
00042 
00043 #define PI 3.14159265359
00044 
00045 AnalogOut signal(p18);
00046 Serial pc(USBTX, USBRX);
00047 
00048 /* IO declarations */
00049     /* LED coil indicators */
00050     DigitalOut LED_H1(LED1);
00051     DigitalOut LED_H2(LED2);
00052     DigitalOut LED_H3(LED3);
00053     DigitalOut LED_H4(LED4);
00054     #define SetLed()    LED_H1 = FET_H1;  \
00055                                         LED_H2 = FET_H2;  \
00056                                         LED_H3 = FET_H3;  \
00057                                         LED_H4 = FET_H4;    
00058     
00059     /* H Bridge output IO */
00060     PwmOut FET_L1( p21 );     // MA
00061     PwmOut FET_L2( p22 );     // MA
00062     
00063     PwmOut FET_L3( p23 );     // MB
00064     PwmOut FET_L4( p24 );     // MB
00065     
00066     DigitalOut FET_H1( p25 ); // MA
00067     DigitalOut FET_H2( p26 ); // MA
00068     
00069     DigitalOut FET_H3( p27 ); // MB
00070     DigitalOut FET_H4( p28 ); // MB
00071 
00072     /* Motor Control Ticker */
00073     Ticker smc;
00074     volatile int smc_walker =   0;
00075     volatile int smc_dir        = 1;
00076     volatile int smc_steps  = -1;
00077     volatile int smc_free       = 1;
00078 
00079 
00080 void smc_routine(){
00081     #define i   smc_walker
00082 
00083         // Phase 1 A    
00084         // If sin +, H1->L2
00085         // If sin -, H2->L1
00086         FET_H1 = LUT_H1[i];
00087         FET_H2 = LUT_H2[i];
00088         FET_L1.pulsewidth_us( LUT_L1[i] );
00089         FET_L2.pulsewidth_us( LUT_L2[i] );
00090         
00091         // Phase 1 A    
00092         // If sin +, H1->L2
00093         // If sin -, H2->L1
00094         FET_H3 = LUT_H3[i]; 
00095         FET_H4 = LUT_H4[i]; 
00096         FET_L3.pulsewidth_us( LUT_L3[i] );
00097         FET_L4.pulsewidth_us( LUT_L4[i] );
00098         
00099 //  uint8_t bridge =    ( FET_H1.read() << 7 )|
00100 //                                      ( isPositive(FET_L1.read()) << 6 )|
00101 //                                      ( FET_H2.read() << 5 )|
00102 //                                      ( isPositive(FET_L2.read()) << 4 )|
00103 //                                      ( FET_H3.read() << 3 )|
00104 //                                      ( isPositive(FET_L3.read()) << 2 )|
00105 //                                      ( FET_H4.read() << 1 )|
00106 //                                      ( isPositive(FET_L4.read()) << 0 );
00107         SetLed();
00108         #undef i
00109         
00110         /* Walk */
00111         smc_walker += smc_dir;
00112         if(smc_walker > 31)
00113             smc_walker = 0;
00114         if(smc_walker < 0)
00115             smc_walker = 31;
00116     /* Coutdown */
00117         if(smc_steps != -1){
00118             if(smc_steps == 0){
00119                 if(smc_free){
00120                     // motor free
00121                     FET_L1 = 1;
00122                     FET_L2 = 1;
00123                     FET_L3 = 1;
00124                     FET_L4 = 1;
00125                     FET_H1 = 0;
00126                     FET_H2 = 0;
00127                     FET_H3 = 0;
00128                     FET_H4 = 0;
00129                 }else{
00130                     // motor locked
00131                 }
00132                 SetLed();
00133                 smc.detach();
00134             }
00135             smc_steps--;
00136         }
00137 }
00138 
00139 void smc_dostep(int steps, int dir, float time_ms, int free){
00140     // steps   = number of microsteps (8 microsteps per full step)
00141     // dir     = -1 or 1
00142     // time_us = completion time in s
00143     smc_steps = steps;
00144     smc_dir     = dir;
00145     smc_free    = free;
00146     smc.attach_us(&smc_routine, (time_ms*1000)/steps);          
00147 }
00148 
00149 
00150 int main(void) {
00151     pc.baud(115200);
00152     pc.printf("hi\r\n");
00153 
00154 /* Control declarations */
00155     /* Direction LUT are played */
00156     //DigitalIn direction( p20 );   
00157     /* Step pulse input if step mode */
00158     //DigitalIn step( p18 );
00159     /* Speed potmeter input */
00160     //AnalogIn  speed( p19 ); 
00161     
00162     //AnalogIn  phase( p17 );
00163     
00164     /* PWM init */                      
00165     FET_L1.period_us(PWM_PERIOD);
00166     FET_L2.period_us(PWM_PERIOD);
00167     FET_L3.period_us(PWM_PERIOD);
00168     FET_L4.period_us(PWM_PERIOD);
00169     FET_L1 = 1;
00170     FET_L2 = 1;
00171     FET_L3 = 1;
00172     FET_L4 = 1;
00173     FET_H1 = 0;
00174     FET_H2 = 0;
00175     FET_H3 = 0;
00176     FET_H4 = 0;
00177     SetLed();
00178 
00179     //smc.attach_us(&smc_routine, 6000);
00180     smc_dostep(1600, 1, 1000.0, 1);
00181         
00182 
00183     /* Stepper motor control loop */    
00184     while(1){
00185             while(!pc.readable());
00186             while(pc.readable())    pc.getc();
00187             smc_dostep(16, 1, 100.0, 0);
00188     }
00189 }