Motor control interface

// ****  Kindly maximize the screen , (c) D.Ajay kumar , March , 2005  **** 
// 
// This Program is written in PIC C to interface a PIC16F877 to a LM629 , The program is expected to execute the following  

// operation ," Independent of the current resting position of the motor shaft , the shaft will complete thirty revolutions 
// in the reverse direction . Total time for the move is fifteen seconds. Total time for accn and deceleration is five secs" 
// **         ---- This system uses a 2500 cpr encoder ----            ** 
// Total counts for this position 2500 cpr x 4 counts/cycle = 10000 counts / rev. 
// 10000 x 30 rev = 300000 counts 
// 300000/5 = 60000 counts during acceleration and deceleration 
// 60000/2 = 30000 counts during acceleration 
// Sample perion of system = 2048 x (1/6x10e6) =  341 x 10e-6 seconds/sample.   ---- note : 6 MHz clock ---- 
// The number of samples during accn. (and deceleration) = 2.5 secs / (341x10e-6) = 7332 samples during accn/ decleration. 
// s=ut + 1/2 at^2 
// a=2s/t^2  = (2 x 30000 counts) / (7332 samples)^2 = 0.00111 counts / sample^2 
// Total counts travelled at max velocity is 4/5th of total counts travelled 
// (4x 300000)/5 = 240000 counts. 
// Number of samples at maximum velocity  10 seconds / (341 x 10e-6) = 29325  samples. 
// Using counts/sample velocity is found  :  240000/29325 = 8.184 counts / sample. 
// Scale acceleration values: 0.00111 x 65536 = 72.744 counts /sample^2 
// Scale velocity values : 8.184 x 65536 = 536346.624 counts/sample. 
// Round values to the nearest integer: 
// Acceleration = 73 = 00 00 00 49 hex counts/sample ^2 ----------------------------------------------(1) 
// Velocity = 536347 = 00 08 2F 1B hex counts/sample    ----------------------------------------------(2) 
// Position =-300000 = FF FB 6C 20 hex counts           ----------------------------------------------(3) 


#include "C:\pic\Examples\motorfin.h" 
#byte TRISB=0x86     // Initialize PORTB 
#byte TRISD=0x88     // Initialize PORT D 
#byte PORTB=0x06     // Set up PORT B 
#byte portd=0x08     // Set up PORT D 
#bit rst=0x08.7      // Define RESET pin to PORT B7 
#bit ps=0x08.6       // Define Port Select to PORT B6 
#bit wr=0x08.5       // Define Write to PORT B5 
#bit rd=0x08.4       // Define Read to PORT B4 
#bit RB0=0x06.0      // Define RB0 (busy-bit pin) 
void init_mod(void); 
void filter_mod(void); 
void busy_bit(void); 
void status_check(void); 
void out_c(short); 
void out_d(short); 
int sw; 
void main() 
{ 

   setup_adc_ports(NO_ANALOGS); 
   setup_adc(ADC_CLOCK_DIV_2); 
   setup_spi(FALSE); 
   setup_psp(PSP_DISABLED); 
   setup_counters(RTCC_INTERNAL,RTCC_DIV_2); 
   setup_timer_1(T1_DISABLED); 
   setup_timer_2(T2_DISABLED,0,1); 
   setup_ccp1(CCP_OFF); 
   setup_ccp2(CCP_OFF); 

while(1) 
   { 
   TRISD=0x00;      // Set PORT D as INPUT 
   init_mod();      // Call Initialize function 
   filter_mod();    // Call Filter function 
   out_c(0x1f);     // Call out_command module and load the trajectory parameters input buffers 
   busy_bit();      // Call busy bit check module 
   out_d(0x00);     // Call out data module and output HB of trajectory control word 
   out_d(0x2b);     // All three parameters (pos,vel,acc) will be loaded and all are relative 
   busy_bit();      // Call busy bit check module 
   out_d(0x00);     // Accn. data HIGH word HB, refer note ......(1) 
   out_d(0x00);     // Accn. data HIGH word LB 
   busy_bit();      // Call busy bit check module 
   out_d(0x00);     // Accn data LOW word HB 
   out_d(0x49);     // Accn data LOW word LB 
   busy_bit();      // Call busy bit check module 
   out_d(0x00);     // Vel. HIGH word HB, refer note ............(2) 
   out_d(0x08);     // Vel. HIGH word LB 
   busy_bit();      // Call busy bit check module 
   out_d(0x2f);     // Vel. LOW word HB 
   out_d(0x1b);     // vel. LOW word LB 
   busy_bit();      // Call busy bit check module 
   out_d(0xff);     // Pos. HIGH word HB, refer note ............(3) 
   out_d(0xfb);     // Pos. HIGH word LB 
   busy_bit();      // Call busy bit check module 
   out_d(0x6c);     // Pos. LOW word HB 
   out_d(0x20);     // Pos. LOW word LB 
   busy_bit();      // Call busy bit check module 
   out_c(0x01);     // STT issued to execute trajectory 

   } 
} 

                    // ** BUSY BIT CHECK MODULE ** // 
busy_bit() 
{ 
//                **   CS pin is permanently grounded (Low)   **                

TRISB=0xff;         // Set PORT B as OUTPUT 
portd = 0xFF;       // Output '11111111' on PORT D 
ps=0;               // Set PS=0 
rd=0;               // Set RD=0 
while(RB0);         // Wait till Busy_bit pin is set (RB0) 
ps=1;               // Set PS=1 
rd=1;               // Set RD=1 
portd = 0xFF;       // Output '00000000' on PORT D 
} 
                    
                    // ** INITIALIZATION MODULE ** // 

init_mod() 
{ 
portd = 0xFF; 

begin:              // Label 
rst=0b0;            // Set Reset Low 
delay_ms(1);        // Call Delay 1 ms. 
rst=0b1;            // Set Reset High 
delay_ms(3);        // Call Delay 3 ms. 
status_check();     // Call Status Check function 
                    // Check if Status Word is hex 'c4' or '84' 
if(!(sw==0xc4 || sw==0x84)) goto begin;        
    else           
       { 
       out_c(0x1d); // Tells which Interrupts to reset as indicated by zero's in the next data word 
       busy_bit();  // Call busy bit check module 
       out_d(0x00); // Don't Care data. 
       out_d(0x00); // All interrupts will be reset 
       busy_bit();  // Call busy bit check module 
       status_check(); // Call Status Check function 
                    // Check if status word is hex 'c0' or '80' 
if(!(sw==0xc0||sw==0x80))                
           goto begin; 
       busy_bit();  // Call busy bit check module 
       } 
portd = 0xFF;       // Output '11111111' on PORT D 
} 


                    // ** FILTER PROGRAMMING MODULE ** // 

void filter_mod(void) 
{ 
out_c(0x1e);        // LFIL command - initiates loading filter co-efficients. 
busy_bit();         // Call busy bit check module 
out_d(0x00);        // Filter control word , indicates which values Kp,Ki,Kd,Il will be loaded 
out_d(0x0e);        // selects that kp,ki,kd values will be loaded 
busy_bit();         // Call busy bit check module 
out_d(0x00);        // kp values 8 bit HB 
out_d(0x0a);        // LB 
busy_bit();         // Call busy bit check module 
out_d(0x00);        // ki values 8 bit HB 
out_d(0x00);        // LB 
busy_bit();         // Call busy bit check module 
out_d(0x00);        // kd values 8 bit HB 
out_d(0x00);        // LB 
busy_bit();         // Call busy bit check module 
out_c(0x04);        // Transfer new filter co-efficients to Working register 
busy_bit();         // Call busy bit check module 
} 

                    // ** DATA OUT FUNCTION ** // 

out_d(int dat) 
{ 
TRISB=0xff;         // Set PORT B as outputs 
portd = 0xFF;       // Output '11111111' on PORT D 
ps=1;               // Set PS high 
rd=1;               // Set RD high 
wr=0;               // Set WR low 
PORTB=dat;          // Output 'dat' on PORT B 
wr=1;               // Set WR high for sending next byte 
portd = 0xFF;       // Output '11111111' on PORT D 
} 

                    // ** COMMAND OUT FUNCTION **// 
out_c(int com) 
{ 
TRISB=0xff;         // Set PORT B as outputs 
portd = 0xFF;       // Output '11111111' on PORT D 
ps=0;               // Set PS low 
wr=0;               // Set WR low 
rd=1;               // Set RD high 
PORTB=com;          // Output 'com' on PORT B 
wr=1;               // Set WR high for sending next byte 
portd = 0xFF;       // Output '11111111' on PORT D 
} 

                    // ** STATUS CHECK FUNCTION ** // 
status_check() 
{ 
TRISB=0xff;         // Set PORT B as outputs 
portd = 0xFF;       // Set PORT D as outputs 
ps=0;               // Set PS low 
rd=0;               // Set RD low 
wr=1;               // Set WR high 
delay_ms(1);        // Call Delay for 1 ms 
sw=PORTB;           // Store PORTB value in 'sw' 
rd=1;               // Set RD high 
portd = 0xFF;       // Output '11111111' on PORT D 
}