/* Main.c file generated by New Project wizard
 *
 * Created:   Fri Oct 16 2015
 * Processor: PIC16F876
 * Compiler:  CCS for PIC
 * Programmer: W.M Croston
 * Version 1.3
 * Date 11-1-2016
 * New hardware PWM GEN on 5i25 card 
 * Base Freq is now 500Hz
 * New shortest route maths are used 
 * Program commented and tided up
 * Version 1.2
 * Date 29/12/15
 * Absolute Encoder added and IN Pos input 
 * Version 1.1
 * Date 21/10/15
 * Program Hash defines working
 * Program broken up into working functions
 * interrupts stopped during step pulses
 * Pos and neg tool changer movement implemented
 * Version 1.0
 * Measures Pulse Width from Pathpilot Basic program working 
 */

#include <16F876.h>
#include <stdlib.h>

#use delay(crystal=20000000)

#fuses HS, NOWDT, NOLVP, NOBROWNOUT, NOPROTECT, PUT
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7)

// Port A Pin Outputs
#define AIR_SOL PIN_A0
#define EMG_STOP PIN_A1
#define STEP_PIN PIN_A2
#define DIR_PIN PIN_A3
// note do not use A4 !!
#define TOOL_CHG_CMPT PIN_A5

// Port B Pin Inputs
#define TOOL_CHG_START PIN_B0
 
#define TOOL_IN_POS PIN_B4
// Pins B5-B7 used for BCD Encoder 

#define MOTOR_STEPS 875
// Globals used in program
long rise,fall,pulse_width; 

// Start of Functions Prototypes
#int_ccp2
void isr()
{
   rise = get_capture(1);     // CCP_1 is the time the pulse went high
   fall = get_capture(2);     // CCP_2 is the time the pulse went low

   pulse_width = fall - rise; // pulse_width/(clock/4) is the time 
}// end isr

int new_tool_number(long new_width){
    ///  printf("\r\n in Tool found function");
      int tool;
      if (new_width >= 190 && new_width <= 210){
          tool = 1; }
          
      else if (new_width >= 390 && new_width <= 410){
          tool = 2; }
          
      else if (new_width >= 590 && new_width <= 610){
          tool = 3; }
         
      else if (new_width >= 790 && new_width <= 810){
          tool = 4; }
          
      else if (new_width >= 990 && new_width <= 1010){
          tool = 5; }
          
      else if (new_width >= 1190 && new_width <= 1210){
          tool = 6; }
          
      else if (new_width >= 1390 && new_width <= 1410){
          tool = 7; }
          
      else if (new_width >= 1590 && new_width <= 1610){
          tool = 8;  }
      else
	  tool = 0;
      
      return tool;
}// end func
   
void move_stepper(int new_tool_num, int prev_tool_num)
{
   long step_pulses;
   long step; 
   int steps_tool_num;
   int C,D;
   
   step = 0;
   step_pulses = 0;
   steps_tool_num = 0;
   
   disable_interrupts(INT_CCP2);   // Setup interrupt on falling edge
   disable_interrupts(GLOBAL);
   
   C = ((prev_tool_num - new_tool_num) % 8);
   D = ((new_tool_num - prev_tool_num) % 8);
        
    if(D <= C ){
        printf("\r\n Going Forward");
        printf("\r\n Steps D = %I", D); 
        printf("\r\n New Tool Num = %I", new_tool_num);
        printf("\r\n Previous Tool Num = %I", prev_tool_num); 
        steps_tool_num = D;
        output_low(AIR_SOL); // Valid Tool so turn on sol and wait
	    output_low(DIR_PIN);  // Direction Pin 0 on stepper drive 
        delay_ms(1500);
    }
    else if(D > C){
        printf("\r\n Going Backwards");
        printf("\r\n Steps C = %I", C);
        printf("\r\n New Tool Num = %I", new_tool_num);
        printf("\r\n Previous Tool Num = %I", prev_tool_num); 
        steps_tool_num = C;
        output_low(AIR_SOL); // Valid Tool so turn on sol and wait
	    output_high(DIR_PIN); // Direction Pin 0 on stepper drive 
        delay_ms(1500);
    }
    
   if((steps_tool_num >= 1) && (steps_tool_num <= 8)){
       // only a valid tool number allowed to move !!!  
       step_pulses = (steps_tool_num * MOTOR_STEPS);
       printf("\n\r No of Step Pulses = %Lu ", step_pulses);
   for(step=0; step < step_pulses; step++)
       {    
           output_high(STEP_PIN);
           delay_us(600);
           output_low(STEP_PIN);
           delay_us(700);
        }//end for   
     }// end if
   
      enable_interrupts(INT_CCP2);   // Setup interrupt on falling edge
      enable_interrupts(GLOBAL);
}// end func




int main (void)
 { 
    int prev_tool_num = 0;
    int new_tool_num = 0;
    long new_width;
    setup_ccp1(CCP_CAPTURE_RE);    // Configure CCP1 to capture rise
    setup_ccp2(CCP_CAPTURE_FE);    // Configure CCP2 to capture fall
    setup_timer_1(T1_INTERNAL);    // Start timer 1
    enable_interrupts(INT_CCP2);   // Setup interrupt on falling edge
    enable_interrupts(GLOBAL);
    output_high(AIR_SOL);          // Set output pins to correct state
    output_high(EMG_STOP);
    output_low(STEP_PIN);
    output_high(DIR_PIN);
    output_high(TOOL_CHG_CMPT);
    
    // Program waits here until Tool Change Start goes High.
    printf("\r\n Tool Changer Program Started By WM Croston");
   while (1){
   // Note this pin needs some filtering to stop false Tool Changes.          
   while(input(TOOL_CHG_START)){
	     printf("\n\r Step 1 ToolChange Pin = True");
	     new_width = pulse_width/5;  // reads Pulse Width
	     printf("\n\r Step 2 Measured Pulse Width = %Lu", new_width );
	     new_tool_num  = new_tool_number(new_width); // Writes new tool number
         // reads tool changer position
         prev_tool_num = input_b() >> 5 ; 
         prev_tool_num = prev_tool_num + 1; // adds 1 to bcd input.
         printf("\n\r Step 3 First check Encoder Tool Pos = %I", prev_tool_num);
	     move_stepper(new_tool_num, prev_tool_num); // calculate number of steps to next position
         delay_ms(1000);
         output_high(AIR_SOL); // turn sol off
         output_high(TOOL_CHG_CMPT); // blip tool changed signal
         delay_ms(500);
         output_low(TOOL_CHG_CMPT);
         delay_ms(500);
         output_high(TOOL_CHG_CMPT);
	      // check if new tool pos and toolchanerger are the same
         prev_tool_num = input_b() >> 5 ; // reads tool changer position
         prev_tool_num = prev_tool_num + 1; // adds 1 to bcd input.
        
         if ((prev_tool_num != new_tool_num) || input(TOOL_IN_POS)!= 1 )
            {
                  printf("\n\r EMergency Stop ");
                  output_low(EMG_STOP);
            }// end if
         
	      
	   } // end while
      
	   }// end while
  
 }   // end Main