Prev: embedded gcc linker issue--trying to build for m68000 cpu
Next: Number of Microprocessors in embedded devices
From: vibz86 on 4 Nov 2009 08:56
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
From: vinnie on 5 Nov 2009 09:58
>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
 | 
Pages: 1
Prev: embedded gcc linker issue--trying to build for m68000 cpu
Next: Number of Microprocessors in embedded devices