Juan Esteban Paz
Published © CC BY

Stepper Motor Control

A full featured stepper control involving speed and line interpolation for 2-axis motion.

IntermediateFull instructions provided2 hours2,018
Stepper Motor Control

Things used in this project

Story

Read more

Schematics

PSoc Workspace

Code

Code

C/C++
Program Code
/* ========================================
Juan Esteban Paz 
*/

#include <math.h>
#include "project.h"
int a=0,b=0,nspeed=500,speedx=0,speedy=0,steps=0;//,stepsX=0,stepsY=0;
int start=0,dirx,diry;
double dx=0,dy=0,xlast=0,ylast=0;
double mVoltsA,mot,joystickzero,adcCountsA=0;
 double x=0,y=0;

#define enableX() EnX_Write(1u);
#define disableX() EnX_Write(0u);
#define enableY() EnY_Write(1u);
#define disableY() EnY_Write(0u);

void Stop(){
    PWMX_Stop();    PWMY_Stop();      
    disableX()      disableY();   
    a=0;//b=0;
}
CY_ISR(CounterISR){//counter iSR
    Stop(); 
    Counter_STATUS;
    Counter_ReadStatusRegister();  
    CountINT_ClearPending(); 
}
void command(){  
    Stop();dirx=1;diry=1;a=1;b=1;//DIR Y inverted
    //Verify motor direction and update steps to the absolute value
    if(dx<0){dirx=0;dx=-dx;}// dirx=dx > 0?1:-1;
    xdir_Write(dirx);       
    if(dy<0){diry=0;dy=-dy;}//diry=dy > 0?1:-1;
    ydir_Write(diry);    
    //verify which motor should run slower and set number of steps 
   if(dx>=dy){
        speedy=nspeed*dx/dy;
        if(dy==0)speedy=nspeed; //slower
        speedx=nspeed;
        steps=dx;
        Sel_Write(0);
    }else{//dy>>
        speedx=nspeed*dy/dx;//slower
        if(dx==0)speedx=nspeed;
        speedy=nspeed;
        steps=dy;
        Sel_Write(1);
    }    
    //Update configuration for step count and motor speed
    Counter_WritePeriod(steps+3);
    Counter_WriteCompare(steps);
    
    PWMX_Start();PWMY_Start(); 
    PWMX_WritePeriod(speedx);
    PWMX_WriteCompare(speedx/3);
    PWMX_WriteCounter(0);
    
    PWMY_WritePeriod(speedy);
    PWMY_WriteCompare(speedy/3);
    PWMY_WriteCounter(0);
    Counter_WriteCounter(0); 
    if(dx!=0){enableX();}//LEDA_Write(0);
    if(dy!=0){enableY();}  //LEDB_Write(0);  
}

CY_ISR(ISRTOGGLE){//input signal trigger
    if((a==1)||(b==1)){
        Stop();
        start=0;
    }else{
        start=1;
    }
   
}
int main(void)
{   
    CyGlobalIntEnable;
    Counter_Start();   
    ISR_StartEx(ISRTOGGLE);
    ISR_ClearPending();
    start=0; 
    CountINT_StartEx(CounterISR);
    //int trajx[21]={7,6,5,3,1,0,-3,-4,-6,-7,-7,-7,-6,-4,-2,-0,1,3,5,6,7};
    //int trajy[21]={-1,-3,-5,-6,-7,-7,-6,-5,-4,-2,0,2,4,5,6,7,7,6,5,3,1};
    //int trajx[21]={3,3,3,2,1,-1,-3,-4,-5,-4,-2,1,4,6,6,5,3,0,-4,-7,-8};
   // int trajy[21]={4,1,-1,-3,-4,-4,-3,-1,1,3,5,6,5,2,-1,-4,-6,-7,-6,-4,0,};
    
    // coordinate sets for line interpolation.
    int trajx[49]={3,3,3,2,1,-1,-3,-4,-5,-4,-2,1,4,6,6,5,3,0,-4,-7,-8,-7,-5,-1,4,7,9,9,6,2,-3,-7,-10,-11,-9,-4,2,7,11,12,11,6,0,-7,-12,-14,-13,-9,-2};
    int trajy[49]={4,1,-1,-3,-4,-4,-3,-1,1,3,5,6,5,2,-1,-4,-6,-7,-6,-4,0,4,7,9,8,5,1,-3,-7,-10,-10,-7,-3,2,7,11,12,10,5,-1,-7,-11,-13,-12,-7,-1,6,12,15};
    int t=0; 
    
    for(;;){
       
      if(start==1){
        
        for(t=0;t<=48;t=t+1){
            dx=3*trajx[t];
            dy=3*trajy[t];
            command();
            while(a==1){CyDelay(5);}            
        }        
        Stop();
        dx=0;dy=0;
        start=0;
        }
       
    }
}

/* [] END OF FILE */

Credits

Juan Esteban Paz

Juan Esteban Paz

12 projects • 10 followers
Mechanical and Electrical Engineer enjoy working with PSOC
Contact

Comments