// ****  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
}