//-------------------------------------------------------------------------
// Hexapod project 12/21/2004
// Servo feedback module
//-------------------------------------------------------------------------
// Currently 6 channels (two legs)
// Targetted for a PIC16F88, using 6 of the 7 analog inputs.
//
// The original purpose of this was to measure the voltage across each shunt
// resistor, then figure out the amount of current that each servo was drawing
// to determine how hard it's working.
//
// But that's not quite how that ones I've experimented with work.
// Rather there is a steady amount of current drawn for varying durations
// ranging from 0 to cycle length of the servo refresh pwm signals.
//
// So instead of storing the actual voltage read in, it's compared to a
// threshold and counted.
//
// Every time I get a new pulse from the servo controller (indicating a new
// cycle) I save off my counts
//
// An RS-232 connection to the main servo controller provides on-demand
// data at 57600
// All this data is buffered and sent during the transmission buffer empty
// interrupt to make this communications effects on the sampling minimal.
//
// Can optionally use VREF later, would require remapping of channels
//
// Servo channels are on pins AN0-AN5
// Stick a current shunt resistor inline with the negative lead of the servo
// Something tiny (be sure it's of sufficient wattage)
// tie the servo side of each resistor to the corresponding channel
//
// Expecting a signal on BO/INT for synchonization with the servo
// driver
//
// Chance for expansion if you're servos stack up:
// I noticed with one servo I was testing with (HiTec HS-805BB) that
// the amount of current drawn turning one direction was different from
// the current drawn turning the other direction!!
// If you've got servos that behave this way reliably, by all means,
// store the max value instead of just comparing to the threshold and use
// it to deduce force direction.
//
// Of course the servos I've got 18 of and are already built into my bot
// don't exhibit this behavior.  Just my luck.
//
// Serial output interrupt buffering code ripped from:
// "http://www.pic-c.com/forum/old/messages/1053.html"
//-------------------------------------------------------------------------

#include <16F88.h>
#fuses NOWDT,NOPROTECT,NOLVP, NOFCMEN, NOIESO, NOBROWNOUT, EC_IO
#use delay(clock=20000000)
#use rs232(baud=115200, xmit=PIN_B5, rcv=PIN_B2, ERRORS)

#define DEFAULT_HIGH_VALUE    1 // Default sensitivity

                                 // Somewhat important!
                                 // Too low and you can get crosstalk between
                                 // channels.
                                 // Too high and you miss ramping up and down
                                 // data and the signals become a bit lower
                                 // resolution (jumpier)

//Serial protocol to master
#define REPORT_VALUES         0x41
#define SET_VOLTAGE           0x42

#define hi(x)  (*(&x+1))

#priority ext, tbe, rda

//-------------------------------------------------------------------------
//RS232 output buffering vars and forward declarations
#define XMIT_FIFO_SIZE        50
char xmit_fifo[XMIT_FIFO_SIZE]; // Fifo for RS232 xmit data
char *xmit_fifo_head; // Pointers for the xmit fifo
char *xmit_fifo_tail;
char xmit_fifo_byte_count; // Number of bytes in the xmit fifo

void init_rs232_xmit_fifo(void);
void rs232_xmit_isr(void);
void send_rs232_char(char c);

//-------------------------------------------------------------------------
//Relevant vars for servo sensing
BYTE ActiveVoltageValue = DEFAULT_HIGH_VALUE;

BYTE CycleCount=0;
BYTE ActivityCounters0;
BYTE ActivityCounters1;
BYTE ActivityCounters2;
BYTE ActivityCounters3;
BYTE ActivityCounters4;
BYTE ActivityCounters5;

BYTE SyncSignal = 0; //set on the positive side of the PWM pulse
                     //it doesn't matter that we might be starting
                     //the sampling in the middle of a cycle, because
                     //we'll end it at the same point in the cycle.
                     //we are not timing how long each servo's current
                     //it high before it goes low, we are counting all
                     //time it's high regardless of edges

void SendData()
{
   send_rs232_char(CycleCount);
   send_rs232_char(ActivityCounters0);
   send_rs232_char(ActivityCounters1);
   send_rs232_char(ActivityCounters2);
   send_rs232_char(ActivityCounters3);
   send_rs232_char(ActivityCounters4);
   send_rs232_char(ActivityCounters5);
}

void main() {
   BYTE CycleCountLive;
   BYTE ActivityCountersLive0;
   BYTE ActivityCountersLive1;
   BYTE ActivityCountersLive2;
   BYTE ActivityCountersLive3;
   BYTE ActivityCountersLive4;
   BYTE ActivityCountersLive5;


   setup_ccp1(CCP_OFF);

   setup_adc_ports(sAN0 | sAN1 | sAN2 | sAN3 | sAN4 | sAN5 | sAN6 | VSS_VREF);
   setup_adc(ADC_CLOCK_INTERNAL);

   enable_interrupts(GLOBAL);
   enable_interrupts(INT_EXT);
   enable_interrupts(INT_RDA);

   ext_int_edge(0, L_TO_H);

   output_high(PIN_B5); // Set the RS232 transmit pin as an output.
   output_float(PIN_B2); // Set the RS232 receive pin as an input.


   //set all our inputs to float
   output_float(PIN_A0);
   output_float(PIN_A1);
   output_float(PIN_A2);
   output_float(PIN_A3);
   output_float(PIN_A4);
   output_float(PIN_B6);  //AN5 = PIN_B6 because of the oscillator pins on port a
   output_float(PIN_B7);  //AN6 = PIN_B7 because of the oscillator pins on port a

   //output 0's to the rest of the pins (don't leave them floating)
   output_low(PIN_A6);
   output_low(PIN_A7);
   output_low(PIN_B1);
   output_low(PIN_B3);
   output_low(PIN_B4);

   init_rs232_xmit_fifo();

   while(1)
   {
      BYTE channel;

      CycleCountLive = 0;

      ActivityCountersLive0 = 0;
      ActivityCountersLive1 = 0;
      ActivityCountersLive2 = 0;
      ActivityCountersLive3 = 0;
      ActivityCountersLive4 = 0;
      ActivityCountersLive5 = 0;

      SyncSignal = 0;

      while (SyncSignal == 0)
      {
         set_adc_channel(0);
	     delay_us(20);
         if (read_adc() >= ActiveVoltageValue)
            ActivityCountersLive0++;

         set_adc_channel(1);
	     delay_us(20);
         if (read_adc() > ActiveVoltageValue)
            ActivityCountersLive1++;

         set_adc_channel(2);
	     delay_us(20);
         if (read_adc() > ActiveVoltageValue)
            ActivityCountersLive2++;

         //skipping channel 3, its VREF+
         set_adc_channel(4);
	     delay_us(20);
         if (read_adc() > ActiveVoltageValue)
            ActivityCountersLive3++;

         set_adc_channel(5);
	     delay_us(20);
         if (read_adc() > ActiveVoltageValue)
            ActivityCountersLive4++;

         set_adc_channel(6);
	     delay_us(20);
         if (read_adc() > ActiveVoltageValue)
            ActivityCountersLive5++;

         CycleCountLive++;
      }

      CycleCount = CycleCountLive;

      ActivityCounters0 = ActivityCountersLive0;
      ActivityCounters1 = ActivityCountersLive1;
      ActivityCounters2 = ActivityCountersLive2;
      ActivityCounters3 = ActivityCountersLive3;
      ActivityCounters4 = ActivityCountersLive4;
      ActivityCounters5 = ActivityCountersLive5;
   }

}

#int_ext
void ext_isr(void)
{
   //The controlling pic on this board raises the B0 line to
   //trigger the start of a new cycle.

   SyncSignal = 1;
}

#int_rda
void rda_isr(void)
{
   BYTE value;

   value = getc();

   if (value == REPORT_VALUES)
      SendData();
   else if (value == SET_VOLTAGE)
   {
      ActiveVoltageValue = getc();
      send_rs232_char(0x41); //Acknowledge the update
   }
}

//--------------------------------------------------------------------------
// This function puts a character into the transmit fifo. It also enables
// the transmitter interrupt. Then the PIC will automatically call the
// isr function and send the character.

void send_rs232_char(char c)
{

   if(xmit_fifo_byte_count < XMIT_FIFO_SIZE) // Any space for a new byte ?
   {
      *xmit_fifo_head++ = c; // If so, put byte into fifo, and inc ptr

      if(xmit_fifo_head == (xmit_fifo + XMIT_FIFO_SIZE)) // Did ptr wrap ?
         xmit_fifo_head = xmit_fifo; // If so, reset it to start of fifo

      xmit_fifo_byte_count++; // Increment byte count
   }

   // If there are any bytes to send, then enable xmit interrupts
   if(xmit_fifo_byte_count != 0)
      enable_interrupts(INT_TBE);

}

//--------------------------------------------------------------------------
// This is the transmit isr function. If TBE interrupts are enabled, and if
// the RS-232 transmit buffer (in hardware) is empty, then the PIC will execute
// this function. Then a character will be pulled out of the transmit fifo
// and sent to the PC, via RS-232.

#int_tbe
void rs232_xmit_isr(void)
{
   if(xmit_fifo_byte_count == 0) // Any chars left to send ?
   {
      disable_interrupts(INT_TBE); // If not, then turn off the xmit interrupt
   }
   else // If so, then get a char from the fifo and transmit it.
   {
      putc(*xmit_fifo_tail++); // Send a char and increment fifo ptr

      if(xmit_fifo_tail == (xmit_fifo + XMIT_FIFO_SIZE)) // Did ptr wrap ?
         xmit_fifo_tail = xmit_fifo; // If so, reset ptr to start of fifo

      xmit_fifo_byte_count--; // Decrement byte count
   }

}

//-------------------------------------------------------------------------
// Init variables used by the interrupt driven RS-232 xmit fifo.

void init_rs232_xmit_fifo(void)
{
   xmit_fifo_head = xmit_fifo;
   xmit_fifo_tail = xmit_fifo;
   xmit_fifo_byte_count = 0;
}