/* 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 29/12/15
 * Absolute Encoder added and IN Pos input 
 * 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
 * 
 */

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

//#device ADC=10
#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

long rise,fall,pulse_width; // Globals for int routine
int prev_tool_num = 0;
int tool_chg_pos = 0;


// Start of Functions  
#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 tool_found(long new_width){
    ///  printf("\r\n in Tool found function");
      int tool;
      if (new_width >= 900 && new_width <= 1050){
          tool = 1; }
          
      else if (new_width >= 1900 && new_width <= 2050){
          tool = 2; }
          
      else if (new_width >= 2900 && new_width <= 3050){
          tool = 3; }
         
      else if (new_width >= 3900 && new_width <= 4050){
          tool = 4; }
          
      else if (new_width >= 4900 && new_width <= 5050){
          tool = 5; }
          
      else if (new_width >= 5900 && new_width <= 6050){
          tool = 6; }
          
      else if (new_width >= 6900 && new_width <= 7050){
          tool = 7; }
          
      else if (new_width >= 7900 && new_width <= 8050){
          tool = 8;  }
      else if (new_width >= 8900 && new_width <= 9050){
          tool = 9;  }
      else
	  tool = 0;
      
      return tool;
}// end func
   
void move_steps(int next_move, int prev_tool_num, int tool_num)
{
   long step_pulses;
   long step; 
   
   step = 0;
   step_pulses = 0;
   disable_interrupts(INT_CCP2);   // Setup interrupt on falling edge
   disable_interrupts(GLOBAL);
    
   if((tool_num >= 1) && (tool_num <= 8)){
       // only a valid tool number allowed to move !!!  
       step_pulses = (next_move * MOTOR_STEPS);
       printf("\n\r %Lu No Step Pulses ", 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 next_move;
    //int prev_tool_num;
    int tool_num;
    int next_tool;
    int in_my_pos;
    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);
    output_high(EMG_STOP);
    output_high(STEP_PIN);
    output_high(DIR_PIN);
    output_high(TOOL_CHG_CMPT);
    
   // Write your code here
    printf("\r\n Step 1 Tool Changer Program Started");
   while (1){
             
   while(input(TOOL_CHG_START)){
	 printf("\n\r Step 2 ToolChange Pin = True");
	 new_width = pulse_width/5;  // reads Pulse Width
	 printf("\n\r Step 3 Measured Pulse Width = %Lu", new_width );
	 
	 tool_num  = tool_found(new_width); // Writes new tool number
     tool_chg_pos = input_b() >> 5 ; // reads tool changer position
     tool_chg_pos = tool_chg_pos + 1; // adds 1 to bcd input.
     prev_tool_num = tool_chg_pos;
     printf("\n\r First check Encoder Tool Pos = %I", tool_chg_pos);
	// this tests for invalid tool numbers and keeps retrying
	 switch(tool_num){
	       case 1:
	       case 2:  
	       case 3:
	       case 4:
	       case 5:
	       case 6:
	       case 7:
	       case 8:
		        printf("\r\n Step 5 Valid Tool Number 1-8 ");
	       break;
	      
	      default:
		          tool_num  = 0; // make sure we can exit
                  printf("\n\r Invalid tool Number");
               }// end switch
      
      // Testing       
      //tool_num = 1;   prev_tool_num = 0;
      
      next_tool = (tool_num - prev_tool_num);
      switch(next_tool){
           case 0:
           break;
           
	       case 1:
	       case 2:  
	       case 3:
	       case 4:
	       case 5:
	       case 6:
	       case 7:
	       case 8:
		         next_move = (tool_num - prev_tool_num);
	             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);
		         printf("\r\n Step 6 Valid Pos Number");
	       break;
                      
	       case -1:
	       case -2:  
	       case -3:
	       case -4:
	       case -5:
	       case -6:
	       case -7:
	       case -8:
		         next_move = abs((signed)tool_num - prev_tool_num);
	             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);
		         printf("\r\n Step 7 Valid Neg Number");
	        break;
          default:
              do{ // This should never happen just in case it does
			      printf("\r\n Tool Change Error !!!");
                  output_low(EMG_STOP); 
		         }while(input(TOOL_CHG_START));
        }
	      printf("\n\r  Step 8 Tool Number = %I", tool_num  );
	      printf("\n\r  Step 9 Prev ToolNumber = %I ", prev_tool_num);
	      printf("\n\r  Step 10 Next MOVE = %I ", next_move);
                   
	      move_steps(next_move, prev_tool_num, tool_num); // calculate number of steps to next position
          prev_tool_num = tool_num; // Update new tool 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
          tool_chg_pos = input_b() >> 5 ; // reads tool changer position
          tool_chg_pos = tool_chg_pos + 1; // adds 1 to bcd input.
          printf("\n\r Last check Encoder Tool Pos = %I", tool_chg_pos);
          if ((tool_chg_pos != tool_num) || input(TOOL_IN_POS)!= 1 )
          {
                  printf("\n\r EMergency Stop ");
                  output_low(EMG_STOP);
                  
          }
         
	      
	   } // end while
      
	   }// end while
  
 }   // end Main