/**  TREAD877.C  **/


/************************************************************************/
/**                                                                    **/
/**  PICF877 program to run the treads on my RS "Bedlam" based robot.  **/
/**  PWM (pulse-width-modulation) is used for motor speed control.     **/
/**  Two down-pointing InfraRed Proximity Sensors are at the front.    **/
/**                                                                    **/
/**  As used by my entry in the "Journey Robot Contest" at the         **/
/**  RI/SME Student Robotic Engineering Challenge.                     **/
//*  April 8 2001 at Robert Morris College (near Pittsburgh).          **/
/**                                                                    **/
/**  Copyright 2001 Brian Reed.  All Rights Reserved.                  **/
/**  Feel free to ask for permission to use.  www.reedonline.com       **/
/**                                                                    **/
/************************************************************************/


#include "stdio.h"   // HTML for this page makes me use the double quotes
#include "pic.h"     // instead of the <> signs.  Replace them.


/** Miscellaneous defines **/
#define DIR_FORWARD       1
#define DIR_REVERSE       0
#define IR_THRESHOLD    250


/************************************************************************/
/** Make our own references to some 'F877 registers for readability.   **/
/** RED_DIREC is really just RB1, BLU_DIREC is RB2.                    **/
/** RED_SPEED is CCPR1L, BLU_SPEED is CCPR2L.                          **/
/************************************************************************/
static volatile bit             RED_DIREC  @ (unsigned)&PORTB*8+1;
static volatile bit             BLU_DIREC  @ (unsigned)&PORTB*8+2;
static volatile unsigned char   RED_SPEED  @ 0x15;
static volatile unsigned char   BLU_SPEED  @ 0x1B;


/************************************************************************/
/** Delay support definitions - All compiler optimizations must be ON! **/
/** The CLK_FREQ definition must match the target device.              **/
/************************************************************************/
#ifndef CLK_FREQ
#define CLK_FREQ   4MHZ      /* Crystal frequency in MHz */
#endif

#define MHZ *1000L            /* Number of kHz in a MHz */
#define KHZ *1                /* Number of kHz in a kHz */

#if CLK_FREQ >= 12MHZ
  #define DelayUs(x)  { \
              unsigned char _uctmp; \
              _uctmp = (x)*((CLK_FREQ)/(12MHZ)); \
              while (--_uctmp != 0) \
                  continue; }
#else
  #define DelayUs(x)  { \
              unsigned char _uctmp; \
              _uctmp = (x)/((12MHZ)/(CLK_FREQ))|1; \
              while (--_uctmp != 0) \
				  continue; }
#endif


/************************************************************************/
/** Initialize the PWM sections of the PIC16F877.                      **/
/************************************************************************/
void PWM_init(void)
{
  // > Q: Does a TMR2 prescale of 1:4 mean I multiply by 0.25 or 4.0?
  // > Answer: "multiply by .25 or divide by 4"
  //   The 4 in the equation is because Timer2 runs at Tosc/4
  //   [[  PWM period= (PR2+1) * 4 * Tosc * (TMR2 prescale)  ]]

  // 1:4 duty cycle for both PWMs
  CCP1CON = 0x0C;
  CCP2CON = 0x0C;  

  // Set period register
  PR2 = 0xFF;      

  // Pre-scale setting & Timer2=ON
  T2CON = 0x06;

  // Set two tri-state pin's as output for the dir bits on each tread
  TRISB1 = 0;                // DIR1 bit for RED tread
  TRISB2 = 0;                // DIR2 bit for BLUE tread
  //Good init code but not needed when doing TRISC=0 in main()
  //TRISC2 = 0;              // PWM1 output pin (pin 17 on 40 pin)
  //TRISC1 = 0;              // PWM2 output pin (pin 16 on 40 pin)

  // set track speeds to 0 and dir bits to FORWARD
  RED_DIREC = DIR_FORWARD;   
  BLU_DIREC = DIR_FORWARD;
  RED_SPEED = 0;
  BLU_SPEED = 0;
}


/************************************************************************/
/** Routines to init the A/D sections & get A/D input values.          **/
/************************************************************************/
void AD_init(void)
{
  OPTION = 0x87;             // Set TMR0 prescaler, and 1:256
  ADCON1 = 0x02;             // Left justify result, 3 analog channels
}

unsigned char AD_RA0(void)
{
  ADCON0 = 0x41;             // Fosc/8, A/D enabled
  DelayUs(20);               // Allow time for A/D capacitor to charge
  ADGO = 1;                  // Set the bit to start the conversion
  while (ADGO)               // Wait for the conversion to complete,
    continue;                //   then continue
  ADCON0 = 0;                // Disable A/D
  return ADRESH;             // Return the AD value (just the top 8 bits)
}

unsigned char AD_RA1(void)
{
  ADCON0 = 0x49;             // Fosc/8, A/D enabled
  DelayUs(20);               // Allow time for A/D capacitor to charge
  ADGO = 1;                  // Set the bit to start the conversion
  while (ADGO)               // Wait for the conversion to complete,
    continue;                //   then continue
  ADCON0 = 0;                // Disable A/D
  return ADRESH;             // Return the AD value (just the top 8 bits)
}

unsigned char AD_RA2(void)
{
  ADCON0 = 0x51;             // Fosc/8, A/D enabled
  DelayUs(20);               // Allow time for A/D capacitor to charge
  ADGO = 1;                  // Set the bit to start the conversion
  while (ADGO)               // Wait for the conversion to complete,
    continue;                //   then continue
  ADCON0 = 0;                // Disable A/D
  return ADRESH;             // Return the AD value (just the top 8 bits)
}


/************************************************************************/
/**  DelayMs() function                                                **/
/************************************************************************/
void DelayMs(unsigned char ms)
{
#if CLK_FREQ <= 2MHZ
  do
    DelayUs(996);
  while (--ms);
#endif
#if CLK_FREQ > 2MHZ
  unsigned char uc;
  do {
    uc = 4;
    do {
      DelayUs(250);
    }
    while (--uc);
  } while (--ms);
#endif
}


/************************************************************************/
/**  Application main() routine.  ***TOP LEVEL OF THE APP***           **/
/************************************************************************/
void main(void)
{
unsigned char uctmp;   // for any quick use of an unsigned char value

  // Main inits
  TRISC = 0;    // Set PORTC Tri-State bits to outputs (LEDs and PWMs)
  PORTC = 0;    // Clear all bits in PORTC to 0
  PWM_init();   // Init Pulse-Width-Modulation sub-system
  AD_init();    // Init Analog-to-Digital converter sub-system

  // Display RA0 pot value while waiting until RB0 is pressed.
  // This allows the user to set fwd/rev speed before running the course.
  while (RB0 != 0) {     // Loop until an RB0 button press
    uctmp = AD_RA0();    // Get the pot value
    PORTC = uctmp;       // Display the pot value on the LEDs
  }

  // Main loop:
  // Go forward until an IRPD sensor detects the black line,
  // then backup a bit and pivot left or right and resume forward.
  while (1) {

    // Just for fun, flash the two outside LEDs.
    for (uctmp=0; uctmp<7; uctmp++) {
      PORTC = 0x01;
      DelayMs(30);
      PORTC = 0x80;
      DelayMs(30);
    }

    PORTC = 0;     // Clear the LEDs on port C

    // BOTH TREADS FORWARD
    RED_DIREC = DIR_FORWARD;
    BLU_DIREC = DIR_FORWARD;
    // Get the speed from the potentiometer on RA0
    uctmp = AD_RA0();
    RED_SPEED = uctmp;
    BLU_SPEED = uctmp;

    // This internal loop is always checking the IRPD sensors
    while (1) {

      if (AD_RA1() > IR_THRESHOLD) {
        PORTC = 0x80;  // LED output indication

        // Stop now, then backup&stop, pivot&stop, & return to outside loop
        RED_SPEED = 0;
        BLU_SPEED = 0;
        DelayMs(250);
        DelayMs(250);

        // Backup & stop
        //// GOOD TESTING: BETWEEN 600 AND 1000 MS OF BACKUP
        RED_DIREC = DIR_REVERSE;
        BLU_DIREC = DIR_REVERSE;
        uctmp = AD_RA0();  // Get the speed from the potentiometer on RA0
        RED_SPEED = uctmp;
        BLU_SPEED = uctmp;
        DelayMs(250);
        DelayMs(250);
        DelayMs(250);
        DelayMs(250);
        RED_SPEED = 0;
        BLU_SPEED = 0;
        DelayMs(250);
        DelayMs(250);

        // Pivot & stop
        //// GOOD TESTING: SPEED = 0x80 DelayMs=250
        RED_DIREC = DIR_FORWARD;
        BLU_DIREC = DIR_REVERSE;
        RED_SPEED = 0x80;
        BLU_SPEED = 0x80;
        DelayMs(250);
        RED_SPEED = 0;
        BLU_SPEED = 0;
        // The LED flash will delay a moment before resuming forward motion

        break;       // break out of internal while(1) loop
      }

      else if (AD_RA2() > IR_THRESHOLD) {
        PORTC = 0x01;  // LED output indication

        // Stop now, then backup&stop, pivot&stop, & return to outside loop
        RED_SPEED = 0;
        BLU_SPEED = 0;
        DelayMs(250);
        DelayMs(250);

        // Backup & stop
        RED_DIREC = DIR_REVERSE;
        BLU_DIREC = DIR_REVERSE;
        uctmp = AD_RA0();  // Get the speed from the potentiometer on RA0
        RED_SPEED = uctmp;
        BLU_SPEED = uctmp;
        DelayMs(250);
        DelayMs(250);
        DelayMs(250);
        DelayMs(250);
        RED_SPEED = 0;
        BLU_SPEED = 0;
        DelayMs(250);
        DelayMs(250);

        // Pivot & stop
        RED_DIREC = DIR_REVERSE;
        BLU_DIREC = DIR_FORWARD;
        RED_SPEED = 0x80;
        BLU_SPEED = 0x80;
        DelayMs(250);
        RED_SPEED = 0;
        BLU_SPEED = 0;
        // The LED flash will delay a moment before resuming forward motion

        break;       // break out of internal while(1) loop
      }

    }

  }

}