//---------------------------------------------------
// Unicoder CCS C Demo
//
// Copyright 2009, Noetic Design, Inc.
//
// This demonstation program shows off the features
// of the Unicoder ME-210 encoder.
//---------------------------------------------------

#include "me210_picc_demo.h"


//*********************************************************
// DEFINE VARIABLES
//*********************************************************

// timer2 variables
int32 timer2_ticks;                                        // incremented once per 204us; wraps every 10 days
long control_loop_counter;                                 // counts up to 2441 to increment seconds_elapsed
long seconds_elapsed;
int32 incremental_distance;                                // distance managed by incremental inputs


/******************************************************************************/
/*  Setup the Hardware                                                        */
/******************************************************************************/
void setup()
{
   set_tris_a(TRIS_A_VAL);
   set_tris_b(TRIS_B_VAL);
   set_tris_c(TRIS_C_VAL);
   #if __device__==877
   set_tris_d(TRIS_D_VAL);
   set_tris_e(TRIS_E_VAL);
   #endif

   port_b_pullups(TRUE);
   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_CLOCK_DIV_32);
   setup_psp(PSP_DISABLED);

   setup_counters(RTCC_EXT_L_TO_H, WDT_2304MS);            // set up timer 0 input T0CKI for Right CLK input
   set_timer0(255);                                        // max count so interrupts on first edge
   setup_timer_2(T2_DIV_BY_1,255,4);                       // 4883 Hz interrupt rate (204.8us period); 19531 Hz PWM frequency

   //enable_interrupts(INT_RTCC);                          // CLK
   enable_interrupts(INT_TIMER2);                          // time keeping
   enable_interrupts(global);

   printf("Nubotics Unicoder ME-210 CCS C Demo\r\n");

   delay_ms(10);
   delay_ms(10);
}


//************************************************************************
// debug : output status code
// =====
//
void debug(UINT8 code)
{
   printf("Debug: %u\r\n", (byte)code);
}


//************************************************************************
// exec_command : execute Unicoder command
// =====
//
void exec_command(UINT8 mode)
{
    int i, count;
    
    i2c_start();
    i2c_write(ME210_ID);
    for (i=0 ; i<UC_command.send_count ; i++)
        i2c_write(UC_command.cmd[i]);
        
    if (UC_command.get_count)
    {
        if (mode == MODE_RESTART)
        {
            if (UC_command.delay != 0)
                delay_ms(UC_command.delay);
            i2c_start();
        }
        else
        {                                                                                              // must be MODE_STOP_START
            i2c_stop();
            if (UC_command.delay != 0)
                delay_ms(UC_command.delay);
            i2c_start();
        }
        i2c_write(ME210_ID | 0x01);
        for (i = 0; i < (UC_command.get_count - 1); i++)
            UC_command.reply[i] = i2c_read(1);   // ack on all but last
        UC_command.reply[UC_command.get_count-1] = i2c_read(0); // no ack on last byte
    }
    i2c_stop();
}


//************************************************************************
// init_unicoder
// =======
//
UINT16 init_unicoder()
{
   int i;
   UINT16 res = OK;

   printf("Unicoder init:\r\nSync: ");
   for (i=0;i<10;i++)
   {
       printf(".");
       do_sync();
       res = UC_command.status;
       if (res != OK)
          debug(res);
       delay_ms(2);
   }
   if (res != OK)
      return res;
   printf("\r\nEcho: ");
   for (i=0;i<10;i++)
   {
       printf(".");
       do_echo();
       res = UC_command.status;
       if (res != OK)
          debug(res);
       delay_ms(2);
   }
   printf("\r\n");
   return res;
}

//************************************************************************
// do_sync : execute sync command to check WheelCommander unit.
// =======
//
void do_sync(void)
{
    UC_command.cmd[0] = SYNC_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 1;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[0] == '.')
    {
        UC_command.status = OK;
    }
    else
    {
        UC_command.status = SYNC_FAIL;
        debug(UC_command.status);
    }
}


//************************************************************************
// do_get_name : get type and version of device.
// =======
//
UINT16 do_get_name(char *buf)
{
   UC_command.cmd[0] = NAME_CMD;
   UC_command.send_count = 1;
   UC_command.get_count = 5;
   UC_command.delay = 0;
   exec_command(MODE_RESTART);
   if (UC_command.reply[0] == NAME_CMD)
   {
       UC_command.status = OK;
       buf[0] = UC_command.reply[1];
       buf[1] = UC_command.reply[2];
       buf[2] = UC_command.reply[3];
       buf[3] = UC_command.reply[4];
       buf[4] = '\0';
   }
   else
   {
       UC_command.status = GET_NAME_FAIL;
       debug(UC_command.status);
   }
   return ((UINT16)UC_command.reply[2]) + (((UINT16)UC_command.reply[3]) << 8);
}


//************************************************************************
// do_get_constant : configure a WheelCommander constant
// ===============
// Description
//    Execute an 'F' command to read a specified constant.
// Parameters
//    const_address : UINT8   : address of constant (2->34 at 01/03/06)
// Return values
//    Function returns read value : UINT8
// Globals
//    UC_command structure to hold command details and returned data.
//
UINT8  do_get_constant(UINT8 const_address)
{
    UC_command.cmd[0] = CONSTANT_CMD;
    UC_command.cmd[1] = const_address;
    UC_command.send_count = 2;
    UC_command.get_count = 3;
    UC_command.delay = 2;
    exec_command(MODE_STOP_START);
    if (UC_command.reply[0] == CONSTANT_CMD)
    {
        UC_command.status = OK;
        return UC_command.reply[2];
    }
    else
    {
        UC_command.status = GET_CONSTANT_FAIL;
        debug(UC_command.status);
    }
    return 0;
}

//************************************************************************
// do_set_constant : configure a WheelCommander constant
// ===============
//
void do_set_constant(UINT8 const_address, UINT8 new_value)
{
    UC_command.cmd[0] = CONSTANT_CMD;
    UC_command.cmd[1] = const_address;
    UC_command.cmd[2] = new_value;
    UC_command.send_count = 3;
    UC_command.get_count = 1;
    UC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (UC_command.reply[0] == 'a')
    {
        UC_command.status = OK;
    }
    else
    {
        UC_command.status = SET_CONSTANT_FAIL;
        debug(UC_command.status);
    }
}

//************************************************************************
// do_get_status : get current operating mode of WheelCommander
// =============
// Description
//    Execute an 'S' command to read 16-bit status word.
// Parameters
//    None
// Return values
//    None
// Globals
//    UC_command structure to hold command details and returned status data.
//      UC_command.reply[0] = 'S'
//      UC_command.reply[1] = low byte of status word
//      UC_command.reply[2] = high byte of status word
//
UINT16 do_get_status(void)
{
    UC_command.cmd[0] = GET_STATUS_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 3;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[0] == GET_STATUS_CMD)
    {
        UC_command.status = OK;
    }
    else
    {
        UC_command.status = GET_STATUS_FAIL;
        debug(UC_command.status);
    }
    return ((UINT16)UC_command.reply[1]) + (((UINT16)UC_command.reply[2]) << 8);
}



//************************************************************************
// do_get_velocity : get the speed of motion
// ===============
//
INT16 do_get_velocity(void)
{
    UC_command.cmd[0] = VELOCITY_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 3;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[0] == VELOCITY_CMD)
    {
        UC_command.status = OK;
        return(INT16)(((UINT16)UC_command.reply[1]) + (((UINT16)UC_command.reply[2]) << 8));
    }
    else
    {
        UC_command.status = GET_VELOCITY_FAIL;
        debug(UC_command.status);
    }
    return 0;
}


//************************************************************************
// do_get_period : get the time between encoder ticks
// ===============
//
INT32 do_get_period(void)
{
    UC_command.cmd[0] = PERIOD_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 5;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[0] == PERIOD_CMD)
    {
        UC_command.status = OK;
        return(INT32)(
        ((UINT32)UC_command.reply[1]) +
        (((UINT32)UC_command.reply[2]) << 8) +
        (((UINT32)UC_command.reply[3]) << 16) +
        (((UINT32)UC_command.reply[4]) << 24));
    }
    else
    {
        UC_command.status = GET_PERIOD_FAIL;
        debug(UC_command.status);
    }
    return 0;
}


//************************************************************************
// do_get_angle : get the angle we have rotated through
// ===============
//
INT16 do_get_angle(void)
{
    UC_command.cmd[0] = ANGLE_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 3;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[0] == ANGLE_CMD)
    {
        UC_command.status = OK;
        return(INT16)(((UINT16)UC_command.reply[1]) + (((UINT16)UC_command.reply[2]) << 8));
    }
    else
    {
        UC_command.status = GET_ANGLE_FAIL;
        debug(UC_command.status);
    }
    return 0;
}



//************************************************************************
// do_get_distance : get the distance we have moved
// ===============
//
INT32 do_get_distance(void)
{
    UC_command.cmd[0] = DISTANCE_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 5;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[0] == DISTANCE_CMD)
    {
        UC_command.status = OK;
        return(INT32)(
        ((UINT32)UC_command.reply[1]) +
        (((UINT32)UC_command.reply[2]) << 8) +
        (((UINT32)UC_command.reply[3]) << 16) +
        (((UINT32)UC_command.reply[4]) << 24));
    }
    else
    {
        UC_command.status = GET_DISTANCE_FAIL;
        debug(UC_command.status);
    }
    return 0;
}

//************************************************************************
// do_get_time : get current timestamp
// ===============
//
INT32 do_get_time(void)
{
    UC_command.cmd[0] = TIME_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 5;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[0] == TIME_CMD)
    {
        UC_command.status = OK;
        return(INT32)(
        ((UINT32)UC_command.reply[1]) +
        (((UINT32)UC_command.reply[2]) << 8) +
        (((UINT32)UC_command.reply[3]) << 16) +
        (((UINT32)UC_command.reply[4]) << 24));
    }
    else
    {
        UC_command.status = GET_TIME_FAIL;
        debug(UC_command.status);
    }
    return 0;
}

//************************************************************************
// do_coast : switch off motor system
// ========
//
void do_coast(void)
{
    UC_command.cmd[0] = COAST_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 1;
    UC_command.delay = 100;
    exec_command(MODE_RESTART);                                                                 // PLS March 9 2006: must release bus before querying status of coast command.
    if (UC_command.reply[0] == 'a')
    {
        UC_command.status = OK;
    }
    else
    {
        UC_command.status = COAST_FAIL;
        debug(UC_command.status);
    }
}


//************************************************************************
// do_get_pulsewidth : get the servo pulse generator pulsewidth (+/-2047 from neutral)
// ===============
//
INT16 do_get_pulsewidth(void)
{
    UC_command.cmd[0] = MOTOR_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 3;
    UC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (UC_command.reply[0] == MOTOR_CMD)
    {
        UC_command.status = OK;
        return(INT16)(((UINT16)UC_command.reply[1]) + (((UINT16)UC_command.reply[2]) << 8));
    }
    else
    {
        UC_command.status = GET_PULSEWIDTH_FAIL;
        debug(UC_command.status);
    }
    return 0;
}


//************************************************************************
// do_set_pulsewidth : set the servo pulse generator pulsewidth (+/-2047 from neutral)
// ===============
//
void do_set_pulsewidth(INT16 pw)
{
    UC_command.cmd[0] = MOTOR_CMD;
    UC_command.cmd[1] = pw & 0x0ff;
    UC_command.cmd[2] = (pw >> 8) & 0x0ff;
    UC_command.send_count = 3;
    UC_command.get_count = 1;
    UC_command.delay = 0;
    exec_command(MODE_STOP_START);
    if (UC_command.reply[0] == 'a')
    {
        UC_command.status = OK;
    }
    else
    {
        UC_command.status = SET_PULSEWIDTH_FAIL;
        debug(UC_command.status);
    }
}


//************************************************************************
// do_reset : reset all motion parameters
// ========
//
void do_reset(void)
{
    UC_command.cmd[0] = RESET_CMD;
    UC_command.send_count = 1;
    UC_command.get_count = 1;
    UC_command.delay = 100;
    exec_command(MODE_STOP_START);
    if (UC_command.reply[0] == 'a')
    {
        UC_command.status = OK;
    }
    else
    {
        UC_command.status = RESET_FAIL;
        debug(UC_command.status);
    }
}

//************************************************************************
// do_echo : test comms channel
// =======
//
void do_echo(void)
{
    UC_command.cmd[0] = ECHO_CMD;
    UC_command.cmd[1] = 0x0F;
    UC_command.send_count = 2;
    UC_command.get_count = 2;
    UC_command.delay = 0;
    exec_command(MODE_RESTART);
    if (UC_command.reply[1] == 0xF0)
    {
        UC_command.status = OK;
    }
    else
    {
        UC_command.status = ECHO_FAIL;
        debug(UC_command.status);
    }
}


/******************************************************************************/
/* Timer0 ISR (Right Encoder Clk)                                             */
/*                                                                            */
/******************************************************************************/
#int_timer0
void timer0_isr()
{
   set_timer0(255);                                        // max count so first tick gives us an interrupt
   if (input(ENC_DIR))
      incremental_distance++;
   else
      incremental_distance--;
}



/******************************************************************************/
/* Timer 2 Interrupt Handler                                                  */
/*                                                                            */
/* this keeps track of time and updates the servos                            */
/* on a fixed interval                                                        */
/******************************************************************************/
#int_timer2
void timer2_isr()
{
   timer2_ticks++;
   control_loop_counter++;
   if (control_loop_counter == CONTROL_LOOP_COUNT)         // once per 40.96ms = 24.4 Hz
   {
      seconds_elapsed++;                                   // count timer ticks, one per 40.96 ms, 24.4 Hz
      control_loop_counter = 0;
   }
}



/******************************************************************************/
/* Main Program Entry Point                                                   */
/*                                                                            */
/******************************************************************************/

#zero_ram                                                  // don't need to bother to set variables to zero initially; this does it for us
void main()
{
   char buf[5];
   UINT8 i;

   setup();
   if (init_unicoder() != OK)
   {
      printf("Failed to detect Unicoder.  Halting.\r\n");
      for (;;);
   }
   else
   {
      printf("Get status: 0x%lx\r\n", do_get_status());
      do_coast();
      do_get_name(buf);
      printf("Get name: %s\r\n", buf);
      
      printf("Constants\r\n");
      for (i = 0; i < 11; i++)
      {
         printf("%d. 0x%02x\r\n", i, do_get_constant(i));
         delay_ms(1);
      }
      
      printf("Monitoring Unicoder\r\n");
      printf("Time Distance Angle Velocity Period\r\n");
      for (;;)
      {
         printf("%ld %ld ", do_get_time(), do_get_distance());
         printf("%ld %ld ", do_get_angle(), do_get_velocity());
         printf("%ld         \r", do_get_period());
         delay_ms(100);
      }
   }
}