Diff: main.cpp

Revision:

0:8e4a6920a484

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 08 19:51:11 2013 +0000
@@ -0,0 +1,189 @@
+ /**    
+ * @file    main.cpp
+ * @brief   Bipolar Steppr Motor Control with dual H Bridge
+ * @details Software plays stepper motor lookup tables for dual h bridge control.
+ *                  
+ *
+ * @Author  Jeroen Lodder
+ * @Date    September 2013
+ * @version 1
+ *
+ */
+
+#include "mbed.h"
+#include "smclut.h"
+
+    
+    /* MOSFET io 
+     *  FET         L1  L2  H1  H2  H3  H4  L3  L4
+     *  Motor       MA                          MB          
+     *  mbed        p21 p22 p25 p26 p27 p28 p23 p24
+     * ------------------------ V+
+     * |            |            |          |       
+     * \H1      \H2      \H3        \H4 
+     * |--MA--|          |--MB--|       
+     * \L1      \L2      \L3        \L4 
+     * |            |            |          |       
+     * ------------------------ V- 
+     */
+
+/* Fet driver definitions */
+    /* Turn off time fet driver in micro seconds (uS) */
+    #define TURN_ON_TIME_FET_DRIVER 750
+
+/* Control definitions */
+    /* Mutliplier of speed wait time setting */ 
+    #define SPEED_SCALING   0.5
+    
+/* Motor control definitions */
+    #define PWM_PERIOD          512   // Q15
+    #define PWM_AMPLITIUDE  1           // 0 to 1, equals torque
+    #define PHASE_SHIFT         1           // Shift in rads 
+
+#define PI 3.14159265359
+
+AnalogOut signal(p18);
+Serial pc(USBTX, USBRX);
+
+/* IO declarations */
+    /* LED coil indicators */
+    DigitalOut LED_H1(LED1);
+    DigitalOut LED_H2(LED2);
+    DigitalOut LED_H3(LED3);
+    DigitalOut LED_H4(LED4);
+    #define SetLed()    LED_H1 = FET_H1;  \
+                                        LED_H2 = FET_H2;  \
+                                        LED_H3 = FET_H3;  \
+                                        LED_H4 = FET_H4;    
+    
+    /* H Bridge output IO */
+    PwmOut FET_L1( p21 );     // MA
+    PwmOut FET_L2( p22 );     // MA
+    
+    PwmOut FET_L3( p23 );     // MB
+    PwmOut FET_L4( p24 );     // MB
+    
+    DigitalOut FET_H1( p25 ); // MA
+    DigitalOut FET_H2( p26 ); // MA
+    
+    DigitalOut FET_H3( p27 ); // MB
+    DigitalOut FET_H4( p28 ); // MB
+
+    /* Motor Control Ticker */
+    Ticker smc;
+    volatile int smc_walker =   0;
+    volatile int smc_dir        = 1;
+    volatile int smc_steps  = -1;
+    volatile int smc_free       = 1;
+
+
+void smc_routine(){
+    #define i   smc_walker
+
+        // Phase 1 A    
+        // If sin +, H1->L2
+        // If sin -, H2->L1
+        FET_H1 = LUT_H1[i];
+        FET_H2 = LUT_H2[i];
+        FET_L1.pulsewidth_us( LUT_L1[i] );
+        FET_L2.pulsewidth_us( LUT_L2[i] );
+        
+        // Phase 1 A    
+        // If sin +, H1->L2
+        // If sin -, H2->L1
+        FET_H3 = LUT_H3[i]; 
+        FET_H4 = LUT_H4[i]; 
+        FET_L3.pulsewidth_us( LUT_L3[i] );
+        FET_L4.pulsewidth_us( LUT_L4[i] );
+        
+//  uint8_t bridge =    ( FET_H1.read() << 7 )|
+//                                      ( isPositive(FET_L1.read()) << 6 )|
+//                                      ( FET_H2.read() << 5 )|
+//                                      ( isPositive(FET_L2.read()) << 4 )|
+//                                      ( FET_H3.read() << 3 )|
+//                                      ( isPositive(FET_L3.read()) << 2 )|
+//                                      ( FET_H4.read() << 1 )|
+//                                      ( isPositive(FET_L4.read()) << 0 );
+        SetLed();
+        #undef i
+        
+        /* Walk */
+        smc_walker += smc_dir;
+        if(smc_walker > 31)
+            smc_walker = 0;
+        if(smc_walker < 0)
+            smc_walker = 31;
+    /* Coutdown */
+        if(smc_steps != -1){
+            if(smc_steps == 0){
+                if(smc_free){
+                    // motor free
+                    FET_L1 = 1;
+                    FET_L2 = 1;
+                    FET_L3 = 1;
+                    FET_L4 = 1;
+                    FET_H1 = 0;
+                    FET_H2 = 0;
+                    FET_H3 = 0;
+                    FET_H4 = 0;
+                }else{
+                    // motor locked
+                }
+                SetLed();
+                smc.detach();
+            }
+            smc_steps--;
+        }
+}
+
+void smc_dostep(int steps, int dir, float time_ms, int free){
+    // steps   = number of microsteps (8 microsteps per full step)
+    // dir     = -1 or 1
+    // time_us = completion time in s
+    smc_steps = steps;
+    smc_dir     = dir;
+    smc_free    = free;
+    smc.attach_us(&smc_routine, (time_ms*1000)/steps);          
+}
+
+
+int main(void) {
+    pc.baud(115200);
+    pc.printf("hi\r\n");
+
+/* Control declarations */
+    /* Direction LUT are played */
+    //DigitalIn direction( p20 );   
+    /* Step pulse input if step mode */
+    //DigitalIn step( p18 );
+    /* Speed potmeter input */
+    //AnalogIn  speed( p19 ); 
+    
+    //AnalogIn  phase( p17 );
+    
+    /* PWM init */                      
+    FET_L1.period_us(PWM_PERIOD);
+    FET_L2.period_us(PWM_PERIOD);
+    FET_L3.period_us(PWM_PERIOD);
+    FET_L4.period_us(PWM_PERIOD);
+    FET_L1 = 1;
+    FET_L2 = 1;
+    FET_L3 = 1;
+    FET_L4 = 1;
+    FET_H1 = 0;
+    FET_H2 = 0;
+    FET_H3 = 0;
+    FET_H4 = 0;
+    SetLed();
+
+    //smc.attach_us(&smc_routine, 6000);
+    smc_dostep(1600, 1, 1000.0, 1);
+        
+
+    /* Stepper motor control loop */    
+    while(1){
+            while(!pc.readable());
+            while(pc.readable())    pc.getc();
+            smc_dostep(16, 1, 100.0, 0);
+    }
+}