servo motor controller using dspic

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

formatting link

Reply to
vibz86
Loading thread data ...

to

explain

for

code

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

formatting link

Reply to
vinnie

ElectronDepot website is not affiliated with any of the manufacturers or service providers discussed here. All logos and trade names are the property of their respective owners.