EmbeddedRelated.com
Forums
   Forums    More Forums    comp.arch.embedded  

servo motor controller using dspic

Started by vibz86 ●November 4, 2009
Hi, I have to control 2 servo motors with dspic30f3011. I came across a
sample code which I have attached below. Im finding it really difficult to
understand it and customize it according to my needs. Can some one explain
to me whats the code doing (I can of course understand all the
initialization parts as I read the datasheet). It would be a great help for
me if someone can explain whats going on in the execution part of the code

------------------------------------------------------------

int counter,Dutycycle,direction,period,temp2,data;
float
cur_count,pre_count,distance_inc,error,distnace_val,inc_dis,distance_error,P_gain,error_pre,P_term,D_term,D_gain,PID_term,test,temp,I_term,I_gain,ref_dis,cur_count,PID_abs,address,error_acc;
int I_term_count,counter2;
float rad_s,val,temp,amp,distance_period,speed;
double time;
unsigned char open_loop = 0,drive_stop = 0;

int main (void)
{

TRISD = 0xFFF5;	// LED indicators RD1 & RD3  ..last four bits are 0101.so
RD1, RD3 = 0
TRISE = 0xFFFC; // pwm pins . 11111100.So RE0,1=0
ADPCFG  = 0xFFFE;// PortB digital.RB0 analog current sense. ..11111110
TRISB = 0xFFFF; //  all digital inputs RB0 analog current sense

TRISC = 0x4000; // TX1, RX1   ???  10000... RC15=1
TRISF = 0x56 ;// TX2,RX2,SPI and PWM control enable   ....1010110

SRbits.IPL = 3; /* enable CPU priority levels 4-7 */
CORCONbits.IPL3 = 0;

// .......... Timer 1 enable ...................................

TMR1 = 0; /* clear timer1 register */
PR1 = 0x4D; //TMR1_PERIOD; /* set period1 register */ ...1001101
T1CONbits.TCS = 0; /* set internal clock source */
T1CONbits.TCKPS = 3;

IPC0bits.T1IP = 5; /* set priority level */
IFS0bits.T1IF = 0; /* clear interrupt flag */
IEC0bits.T1IE = 0; /* disable interrupts */

T1CONbits.TON = 1; /* start the timer */

//..... PWM module enable.......................................

PTCONbits.PTOPS = 0;
PTCONbits.PTCKPS = 0;	// prescaler 
PTCONbits.PTMOD = 0;	// free running mode
PTCONbits.PTEN = 1;		// timebase enable
PTMRbits.PTMR = 0;		// pwm count value is zero

PTPER = 1000;			// period register. PWM time base counts up in free running
mode until it matches
                         //....PTPER.then PTMR is cleared
PWMCON1 = 0;
PWMCON1bits.PMOD1 = 0;
PWMCON1bits.PEN1H = 1;
PWMCON1bits.PEN1L = 1;

Dutycycle = 1000;				// DUTY CYCLE
direction = 0;

//.......... Encoder module enable ............................

  QEICON = 0x0700;				// Velociy mode disable,  ..11100000000.this is 4x
measurement mode. Position counter resets when matched with MAXCNT
  MAXCNT = 0xFFFF;
  DFLTCON = 0x00;
  IFS2bits.QEIIF = 0;			// disable any QEI interrupt
  IEC2bits.QEIIE = 0;
  IPC10bits.QEIIP = 4;			/* set priority level */
  POSCNT = 0;

//..........Serial port1  enable ............................

  U1BRG = 	10;		// BOARD RATE 115200

  IPC2bits.U1TXIP = 0x04;   //  Set UART TX interrupt priority
  IPC2bits.U1TXIP = 0x04;   //  Set UART RX interrupt priority

  U1MODE = 0x8800;          // ENABLE UART1 WITH 8  BIT, NO PARITY, 1
STOP,NO WAKE UP
  U1MODEbits.ALTIO = 1;		// ALTERNATE  IO
  
  U1STAbits.UTXEN  = 1;	    // TRANSMIT ENABLE
  
  IFS0bits.U1TXIF = 0;		// CLEAR FLAGS
  IFS0bits.U1RXIF = 0;
 
  IEC0bits.U1TXIE = 0;		// INTERUPT DISABLE
  IEC0bits.U1RXIE = 1;

  U1TXREG = 100;
  
 
//........... A to D Converter Enable ...............................






//............ Basic Initialisation Settings ......................

cur_count = POSCNT;
pre_count = cur_count;
distance_inc = 0;

P_gain = 5;
D_gain = 100;
I_gain = 0.1;
pre_count = 0;
cur_count = 0;
error = 0;
error_pre = 0;
counter2 = 0;
amp = 30;
period = 360;
distance_error = 0;


inc_dis = 0;
amp = 0;
//.................................................................

while(1)
{
   if (IFS0bits.T1IF == 1)

{
	IFS0bits.T1IF = 0;
	counter++;
	if (counter > 100)
	{
	LATDbits.LATD1 =! LATDbits.LATD1;
	counter  = 0;
	}
	
	inc_dis = distance_error/ 10;
	distance_error = distance_error - inc_dis;


// GET THE CURRENT ENCODER COUNT ....................
 
    pre_count = cur_count;
	
	cur_count = POSCNT;
	
//	distance_inc = 10;

	distance_inc = cur_count-pre_count ;

	if (fabs(distance_inc) > 0x3FFF)
{
	if (distance_inc < 0 )  // over  flow
	{
	 distance_inc = (0xffff - pre_count) + cur_count;
	}
	else // under  flow
	{
	 distance_inc = -((0xffff - cur_count) + pre_count);
	}
}
	if (distance_inc > 0)
	{
	 	PORTDbits.RD3 = 1;
	}
	else
		PORTDbits.RD3 = 0;
	



//-------------  FUZZY GAIN SHEDULER ------------------------------

// CALCULATE ACCURATE PID VALUES THROUGH FUZZY LOGIC






	
//-------------  PID ENGINE   -------------------------------------
	error_pre = error;


/*	counter2++;
	if (counter2 > 10)
	{
	counter2 = 0;
	if (error > 0)
		{
		U1TXREG = (int)(error+ 127);
		}
		else
		{
		U1TXREG = (int)(error+ 127);
		}
	}
*/

	error = error - distance_inc;  
	
	error= error + inc_dis ;    // this value is calculate from internal TP by
as, inc_dis = target_dis / (no of steps)

//	ref_dis = 0;

//	pos_val = target_dis - error;

//	WriteUSART (pos_val);

	P_term = error * P_gain;

	D_term = (error - error_pre)* D_gain ;

	PID_term = P_term + D_term;

	I_term_count = I_term_count + 1;

	if (I_term_count >= 1 )
	{
		I_term_count = 0;
		error_acc = error_acc + error;
		if (error_acc > 1024)
			error_acc =  1024;
		else if (error_acc < - 1024)
			error_acc = -1024;

		I_term = (error_acc) * I_gain;
	}

	PID_term = PID_term + I_term;
	
//................................................................................	
	
	
	if (open_loop == 1)
	{
		PID_term = distnace_val;
		cur_count = POSCNT;
		pre_count = cur_count;
		distance_inc = 0;
		error = 0;
		error_pre = 0;
		distance_error = 0;
		inc_dis = 0;
		
	}
		
	if (drive_stop == 1)
	{
		PID_term = 0;
		cur_count = POSCNT;
		pre_count = cur_count;
		distance_inc = 0;
		error = 0;
		error_pre = 0;
		distance_error = 0;
		inc_dis = 0;
		
	}

//.................................................................................	
	

	PID_abs = fabs(PID_term);	// limit the PID max 
	if (PID_abs > 1000)
	{
		PID_abs = 1000;
	}


	if (PID_term < 0)			// GET THE FINAL PWM VALUES
	{
		PID_term = 1000 - PID_abs;
	}
	else 
	{
	    PID_term = 1000 + PID_abs;
	}

	PDC1 = PID_term;				// SET THE PWM VALUE IN THE PWM MODULE
//...............................................................................


}

}
return 0;

}	   
					
---------------------------------------		
This message was sent using the comp.arch.embedded web interface on
http://www.EmbeddedRelated.com
Reply by vinnie ●November 5, 20092009-11-05
>Hi, I have to control 2 servo motors with dspic30f3011. I came across a
>sample code which I have attached below. Im finding it really difficult

to

>understand it and customize it according to my needs. Can some one

explain

>to me whats the code doing (I can of course understand all the
>initialization parts as I read the datasheet). It would be a great help

for

>me if someone can explain whats going on in the execution part of the

code

>

<-snip->

I appears to be code for one motor, not two. I suspect it won't be easy to
modify for two motors.

It might be better to start over with a different approach. There are a lot
of parts out there that do motor control for more than one motor. Kits are
available to get you rolling quickly.

Try to Google "brushless DC motor control kit"

--CG	   
					
---------------------------------------		
This message was sent using the comp.arch.embedded web interface on
http://www.EmbeddedRelated.com
You might also like... (promoted content)
Check out Memfault's New Sandbox!
Check out Memfault's New Sandbox!
Check out Memfault's New Sandbox!