mstep.c

步进伺服控制（采用了变细分控制）．．．． 很好消除了噪音和震动

/* C program for National Semi LMD18245/PIC microstepping driver board.

   IRQ version for Step input.

   ****16F627 version*****

   Embedded Acquisition Systems
   www.embeddedtronics.com
   copyright 2004-2005

   Compiled with PCW PIC Compiler Version 3.168

   April 7, 2005

   Note:
   If you see any errors, please contact me. jimf at embeddedtronics dot com

   Use this code at your own risk, no guarantee that it will work for you.

   This source code is for Non-commercial personal use only.
   If you want to use this code for any other purpose,
   please contact [e-mail] for a commercial license.

   If you modify/enhance this program, please send me a copy.  Thanks.

   Approximate motor performance speeds:
   Microstep10 routine runs about 25% slower due to the longer step calculation routine.
   Microstep10 about 1400RPM.
   Microstep8 about 2000rpm (53Khz step input rate).
   Microstep4 about 3500rpm (46Khz step input rate).
   Halfstep about 3000rpm (20Khz step input rate).
   Tested results with a Superior Electric M091-FD09 at 44volts motor supply. TurboCNC on P2-500 PC.
   Maximum rpm speeds will vary depending on motor type and computer speed.

   Changlog

   April 7, 2005 Added current reduction code, driver output current reduced after 3 minutes.
                 Changed microstep10 pulse routine for faster step rate.

                 Added new sine wave lookup tables, microstep8new and microstep10new.
                 The lookup table is offset by a few degrees. These need to be tested to see if they work better.


 PIC F627 Pin definition.
 pin17 RA0  Motor Driver A, DAC M1 Output
 pin18 RA1  Motor Driver A, DAC M2 Output
 pin1  RA2  Motor Driver A, DAC M3 Output
 pin2  RA3  Motor Driver A, DAC M4 Output
 pin3  RA4  unused

 pin6  RB0  STEP Input
 pin7  RB1  DIR Input
 pin8  RB2  Motor Driver A Direction Output
 pin9  RB3  Motor Driver B Direction Output
 pin10 RB4  Motor Driver B, DAC M1 Output
 pin11 RB5  Motor Driver B, DAC M2 Output
 pin12 RB6  Motor Driver B, DAC M3 Output
 pin13 RB7  Motor Driver B, DAC M4 Output


CS OUT current sense resister
(Vref x D/16) / (250e-6 x R) = A

R=20K   1amp
R=10K   2amp
R=8K    2.5amp
R=6.6K  3amp
1/4 watt
Maximum driver rating 3amps @ 55volts


16bit Timer1 overflow calculation.
For 20Mhz clock prescale is 5,000,000/T1_DIV_BY_X
T1_DIV_BY_8   1.6us  >>>  2^16 * 1.6us = .1049 seconds b4 overflow
T1_DIV_BY_1   200ns  >>>  2^16 * 200ns = .013 seconds b4 overlow
*/


#include <16F627.h>
#use delay(clock=20000000)
#fuses HS,NOWDT,MCLR,NOPROTECT,BROWNOUT,NOLVP
#byte   PORT_A =  5
#byte   PORT_B =  6
static char pos = 0;
static int16 timer_count=0;           // timer1 overflow counter
#define step_input    PIN_B0
#define dir_input     PIN_B1

//Choose step input configuration
//#define fullstep
//#define halfstep
//#define halfstep_torque
//#define microstep4
#define microstep8
//#define microstep8new
//#define microstep10
//#define microstep10new

//Comment out if step multiplier is NOT needed, routine yet not tested.
//#define step_multiply
//#define step_x  2      //Number of steps to multiply.

#ifdef fullstep
#define STEPS 4
//full step drive
//200 steps rev
byte PB[4] = {
0b11111000,
0b00000000,
0b11110100,
0b00001100
};

byte PA[4] = {
0b0000,
0b1111,
0b0000,
0b1111
};
#endif

#ifdef halfstep
//half step drive without torque compensation
//400 steps rev
#define STEPS 8
byte PB[8] = {
0b11111000,
0b11111000,
0b00000000,
0b11110000,
0b11110100,
0b11110100,
0b00001100,
0b11111100
};

byte PA[8] = {
0b0000,
0b1111,
0b1111,
0b1111,
0b0000,
0b1111,
0b1111,
0b1111
};
#endif

#ifdef halfstep_torque
//half step drive with torque compensation
//400 steps rev
#define STEPS 8
byte PB[8] = {
0b00001100,
0b10111100,
0b11111000,
0b10111000,
0b00000000,
0b10110000,
0b11110100,
0b10110100
};

byte PA[8] = {
0b1111,
0b1011,
0b0000,
0b1011,
0b1111,
0b1011,
0b0000,
0b1011
};
#endif

#ifdef microstep4
//quarter step drive
//800 steps rev
#define STEPS 16
byte PB[16] ={
0b00001100,
0b01101100,
0b10111100,
0b11101100,
0b11111000,
0b11101000,
0b10111000,
0b01101000,
0b00000000,
0b01100000,
0b10110000,
0b11100000,
0b11110100,
0b11100100,
0b10110100,
0b01100100
};

byte PA[16] = {
0b1111,
0b1110,
0b1011,
0b0110,
0b0000,
0b0110,
0b1011,
0b1110,
0b1111,
0b1110,
0b1011,
0b0110,
0b0000,
0b0110,
0b1011,
0b1110
};
#endif

#ifdef microstep8
//eight step drive
//1600 steps rev
#define STEPS 32
byte PB[32] ={
0b00001100,
0b00111100,
0b01101100,
0b10001100,
0b10111100,
0b11001100,
0b11101100,
0b11111100,
0b11111000,
0b11111000,
0b11101000,
0b11001000,
0b10111000,
0b10001000,
0b01101000,
0b00111000,
0b00000000,
0b00110000,
0b01100000,
0b10000000,
0b10110000,
0b11000000,
0b11100000,
0b11110000,
0b11110100,
0b11110100,
0b11100100,
0b11000100,
0b10110100,
0b10000100,
0b01100100,
0b00110100
};

byte PA[32] ={
0b1111,
0b1111,
0b1110,
0b1100,
0b1011,
0b1000,
0b0110,
0b0011,
0b0000,
0b0011,
0b0110,
0b1000,
0b1011,
0b1100,
0b1110,
0b1111,
0b1111,
0b1111,
0b1110,
0b1100,
0b1011,
0b1000,
0b0110,
0b0011,
0b0000,
0b0011,
0b0110,
0b1000,
0b1011,
0b1100,
0b1110,
0b1111
};
#endif

#ifdef microstep8new
//eight step drive offset by 5.625degrees
//1600 steps rev
#define STEPS 32
byte PB[32] = {
0b00011100,
0b01001100,
0b01111100,
0b10101100,
0b11001100,
0b11011100,
0b11101100,
0b11111100,
0b11111000,
0b11101000,
0b11011000,
0b11001000,
0b10101000,
0b01111000,
0b01001000,
0b00011000,
0b00010000,
0b01000000,
0b01110000,
0b10100000,
0b11000000,
0b11010000,
0b11100000,
0b11110000,
0b11110100,
0b11100100,
0b11010100,
0b11000100,
0b10100100,
0b01110100,
0b01000100,
0b00010100
};

byte PA[32] ={
0b1111,
0b1110,
0b1101,
0b1100,
0b1010,
0b0111,
0b0100,
0b0001,
0b0001,
0b0100,
0b0111,
0b1010,
0b1100,
0b1101,
0b1110,
0b1111,
0b1111,
0b1110,
0b1101,
0b1100,
0b1010,
0b0111,
0b0100,
0b0001,
0b0001,
0b0100,
0b0111,
0b1010,
0b1100,
0b1101,
0b1110,
0b1111
};
#endif

#ifdef microstep10
//ten step drive
//2000 steps rev
#define STEPS 40
byte PB[40] ={
0b00001100,
0b00101100,
0b01011100,
0b01111100,
0b10011100,
0b10111100,
0b11001100,
0b11011100,
0b11101100,
0b11111100,
0b11111000,
0b11111000,
0b11101000,
0b11011000,
0b11001000,
0b10111000,
0b10011000,
0b01111000,
0b01011000,
0b00101000,
0b00000000,
0b00100000,
0b01010000,
0b01110000,
0b10010000,
0b10110000,
0b11000000,
0b11010000,
0b11100000,
0b11110000,
0b11110100,
0b11110100,
0b11100100,
0b11010100,
0b11000100,
0b10110100,
0b10010100,
0b01110100,
0b01010100,
0b00100100
};

byte PA[40] ={
0b1111,
0b1111,
0b1110,
0b1101,
0b1100,
0b1011,
0b1001,
0b0111,
0b0101,
0b0010,
0b0000,
0b0010,
0b0101,
0b0111,
0b1001,
0b1011,
0b1100,
0b1101,
0b1110,
0b1111,
0b1111,
0b1111,
0b1110,
0b1101,
0b1100,
0b1011,
0b1001,
0b0111,
0b0101,
0b0010,
0b0000,
0b0010,
0b0101,
0b0111,
0b1001,
0b1011,
0b1100,
0b1101,
0b1110,
0b1111
};
#endif

#ifdef microstep10new
//ten step drive offset by 4.5 degrees
//2000 steps rev
#define STEPS 40
byte PB[40] ={
0b00011100,
0b01001100,
0b01101100,
0b10001100,
0b10101100,
0b10111100,
0b11011100,
0b11101100,
0b11111100,
0b11111100,
0b11111000,
0b11111000,
0b11101000,
0b11011000,
0b10111000,
0b10101000,
0b10001000,
0b01101000,
0b01001000,
0b00011000,
0b00010000,
0b01000000,
0b01100000,
0b10000000,
0b10100000,
0b10110000,
0b11010000,
0b11100000,
0b11110000,
0b11110000,
0b11110100,
0b11110100,
0b11100100,
0b11010100,
0b10110100,
0b10100100,
0b10000100,
0b01100100,
0b01000100,
0b00010100
};

byte PA[40] ={
0b1111,
0b1111,
0b1110,
0b1101,
0b1011,
0b1010,
0b1000,
0b0110,
0b0100,
0b0001,
0b0001,
0b0100,
0b0110,
0b1000,
0b1010,
0b1011,
0b1101,
0b1110,
0b1111,
0b1111,
0b1111,
0b1111,
0b1110,
0b1101,
0b1011,
0b1010,
0b1000,
0b0110,
0b0100,
0b0001,
0b0001,
0b0100,
0b0110,
0b1000,
0b1010,
0b1011,
0b1101,
0b1110,
0b1111,
0b1111
};
#endif

void step(char dir)
    {
#if  !defined (microstep10)  && !defined (microstep10new)
      pos = (steps + pos + dir) & (STEPS-1);  //works only for STEPS of 4,8,32
#else
      pos = (pos + STEPS + dir) % STEPS;    //for microstep10, this one works for any STEPS length value but takes longer to calculate
#endif
      PORT_A = PA[pos];  //output to drivers
      PORT_B = PB[pos];
    }

void stepper_off()	 //Turn off power to all windings.
	 {
	 PORT_A = 0;
	 PORT_B = 0;
	 }

void init_stepper()	 //Move a few to register correct step position.
	 {
	 int i;
	 for (i=0;i<8;i++){
	 	 step(-1);
		 delay_ms(100);
	 }
	 for (i=0;i<8;i++){
	 	 step(1);
		 delay_ms(100);
	 }
	}

#INT_EXT             //Pin B0 interrupt service routine for Step input Active low
void ext_isr() {
//read dir input and advance motor
signed int dir,i,temp;

   dir=input(dir_input);

#if  !defined (microstep10)  && !defined (microstep10new)
   if (dir==0){
	   dir=-1;}
#endif

//Step Multiply routine, not fully working. In testing stage.
#ifdef step_multiply
      for (i=0;i<step_x;i++){
         step(dir);
         delay_us(5);   //Note: May need to increase or decrease this delay depending on how slow your motor is.
      }
#else

#if  !defined (microstep10)  && !defined (microstep10new)
    pos = (steps + pos + dir) & (STEPS-1);  //works only for STEPS of 4,8,32
#else
    if(dir==0){            //faster microstep10 routine
        pos = (pos + 1);
        if (pos==40) {
        pos=0;
        }
    }
    else{
        if (pos==0) {
        pos=40;
        }
        pos = (pos - 1);
    }
#endif
      PORT_A = PA[pos];  //output to drivers
      PORT_B = PB[pos];
#endif

timer_count=0;         //reset timer1 counter
}

#int_timer1                       //This function is called everytime
timer_isr() {                     //the timer1 overflows (2^16).
    timer_count++;
    }

void main() {
   setup_timer_1( T1_INTERNAL | T1_DIV_BY_8 );  //init timer1
   enable_interrupts(INT_TIMER1);  //turn on timer1 interrupt
   setup_counters(RTCC_INTERNAL,RTCC_DIV_2);
   ext_int_edge(H_TO_L);           //init interrupt edge triggering for B0, Active Low
   enable_interrupts(INT_EXT);     //turn on external B0 interrupt
   enable_interrupts(GLOBAL);

   set_tris_a(0x10);  //set port A&B for correct data i/o directions
   set_tris_b(0x03);
   #use fast_io (A)
   #use fast_io (B)

stepper_off();     //No current to motor.
delay_ms(1000);    //1 second turn on delay.
init_stepper();

loop:
if (timer_count>1700){     //Shutdown motor drivers after 3 minutes when no step pulse activity
	   PORT_A = 0b0010;  //Set current level at 12% hold
      PORT_B = (PB[pos] & 0b00001111) | 0b00100000;
      }
goto loop;    //keep looping, waiting for interrupt request

}