// File: curlycrt.c
//
// This is the control program for the CurlyCart, which is a
// modified Fisher-Price Power Wheels ride-on toy.
// The program is meant for a PIC 16F874 microcontroller.
// This program uses I2C to interface with a Microchip
// 256Kbit Serial EEPROM, which stores movements and allows them
// to be retrieved later.
//  
// For full details of the CurlyCart, refer to:
// http://www.media.mit.edu/~spiegel/CurlyCart
//
// Matthew Lee
// mattlee@media.mit.edu
// March 7, 2001
//
// Program written by Matthew Lee and Adam Smith
//
#include <16F874.H>

// Configure PIC to use: HS clock, Watchdog Timer, 
// no code protection, enable Power Up Timer
//
#fuses HS,NOWDT,NOPROTECT,PUT,NOBROWNOUT

// Tell compiler clock is 10MHz.  This is required for delay_ms()
// and for all serial I/O (such as printf(...).  These functions
// use software delay loops, so the compiler needs to know the
// processor speed.
//
#use DELAY(clock=10000000)

// Declare that we'll manually establish the data direction of
// each I/O pin on port B.
//
#use fast_io(A)
#use fast_io(B)
#use fast_io(D)
#use fast_io(E)

// definitions for the curly cart
//
#define MOTORLEFT_FWD   PIN_B1  
#define MOTORLEFT_REV   PIN_E1  
#define MOTORRIGHT_FWD  PIN_B0  
#define MOTORRIGHT_REV  PIN_B4  

#define STICKLEFT_FWD   PIN_A1  
#define STICKLEFT_REV   PIN_A2  
#define STICKRIGHT_FWD  PIN_A3  
#define STICKRIGHT_REV  PIN_A4  
#define GAS_PEDAL  	PIN_A5  
#define MODE_BUTTON	PIN_E0

#define SERIALRAM_CLK	PIN_C6
#define SERIALRAM_DATA	PIN_C7

#define RED_LED		PIN_B5
#define RECORD_LED	PIN_B6
#define PLAY_LED	PIN_B7

// Macros to simplify I/O operations
//
#define RED_LED_ON      output_low(RED_LED)
#define RED_LED_OFF     output_high(RED_LED)
#define RECORD_LED_ON   output_high(RECORD_LED)
#define RECORD_LED_OFF  output_low(RECORD_LED)
#define PLAY_LED_ON     output_high(PLAY_LED)
#define PLAY_LED_OFF    output_low(PLAY_LED)

// state defines for the three run states
//
#define STATE_NORMAL    0
#define STATE_RECORD    1
#define STATE_PLAYBACK  2

#define MAX_SAMPLES 65535

// Default tri-state port direction bits: all PORT B bits are
// output except for IR_SENSOR (bit 4) and RC232_RCV (bit 5).
//
#define IRX_A_TRIS      0b11111110
#define IRX_B_TRIS      0b00000000
#define IRX_D_TRIS      0b00000000
#define IRX_E_TRIS      0b00000001

int debounce;
byte curlyState = STATE_NORMAL;
long sampleCount = 0;
long currentSample = 0;

#inline
void left_motor_stop(void)
{
  output_low(MOTORLEFT_FWD);
  output_low(MOTORLEFT_REV);
}

#inline
void left_motor_fwd(void)
{
  output_low(MOTORLEFT_REV);
  output_high(MOTORLEFT_FWD);
}

#inline
void left_motor_rev(void)
{
  output_low(MOTORLEFT_FWD);
  output_high(MOTORLEFT_REV);
}

#inline
void right_motor_stop(void)
{
  output_low(MOTORRIGHT_FWD);
  output_low(MOTORRIGHT_REV);
}

#inline
void right_motor_fwd(void)
{
  output_low(MOTORRIGHT_REV);
  output_high(MOTORRIGHT_FWD);
}

#inline
void right_motor_rev(void)
{
  output_low(MOTORRIGHT_FWD);
  output_high(MOTORRIGHT_REV);
}

void all_off(void)
{
  left_motor_stop();
  right_motor_stop();
  RED_LED_OFF;
  RECORD_LED_OFF;
  PLAY_LED_OFF;
}

byte get_stick_positions(void)
{
  byte result;
  /*
  if (!input(GAS_PEDAL))
  {
    return 0;
  }
  */
  result = 0xF0;
  if (input(STICKLEFT_FWD)) result += 0x01;
  else if (input(STICKLEFT_REV)) result += 0x02;
  if (input(STICKRIGHT_FWD)) result += 0x04;
  else if (input(STICKRIGHT_REV)) result += 0x08;
  return result;
}

void perform_motor_actions(byte input)
{
  /*
  if (!input(GAS_PEDAL))
  {
    left_motor_stop();
    right_motor_stop();
    return;
  }
  */
  if (input & 0x01)
  {
    left_motor_fwd();
  }
  else if (input & 0x02)
  {
    left_motor_rev();
  }
  else
  {
    left_motor_stop();
  }
  if (input & 0x04)
  {
    right_motor_fwd();
  }
  else if (input & 0x08)
  {
    right_motor_rev();
  }
  else
  {
    right_motor_stop();
  }
}

#use i2c(MASTER, SDA=SERIALRAM_DATA, SCL=SERIALRAM_CLK, FORCE_SW)

void init_ram() {
   output_high(SERIALRAM_DATA);
   output_high(SERIALRAM_CLK);
}

void write_ram(long address, byte data) {
   byte b;
   i2c_start();
   i2c_write(0b10100000);
   b = (byte)((address >> 8) & 0xFF);
   i2c_write(b);
   b = (byte)(address & 0xFF);
   i2c_write(b);
   i2c_write(data);
   i2c_stop();
}

void write_ram_old(byte addresshi, byte addresslo, byte data) {
   i2c_start();
   i2c_write(0b10100000);
   i2c_write(addresshi);
   i2c_write(addresslo);
   i2c_write(data);
   i2c_stop();
}

byte read_ram(long address) {
   byte data;

   byte b;
   i2c_start();
   i2c_write(0b10100000);
   b = (byte)((address >> 8) & 0xFF);
   i2c_write(b);
   b = (byte)(address & 0xFF);
   i2c_write(b);
   i2c_start();
   i2c_write(0b10100001);
   data=i2c_read();
   i2c_stop();
   return(data);
}

#inline
void check_mode_button()
{
  if (debounce > 0)
  {
    debounce--;
    return;
  }
  if (input(MODE_BUTTON))
  {
    delay_ms(50);
    while(input(MODE_BUTTON))
    {
      delay_ms(10);
    }
    debounce = 10;
    curlyState++;
    if (curlyState > STATE_PLAYBACK) curlyState = STATE_NORMAL;
    if (curlyState == STATE_RECORD)
    {
      sampleCount = 0;
      currentSample = 0;
    }
    else if (curlyState == STATE_PLAYBACK)
    {
      currentSample = 0;
    }
  }
}

void display_mode_indicators()
{
    if (curlyState == STATE_NORMAL)
    {
      RECORD_LED_OFF;
      PLAY_LED_OFF;
    }
    else if (curlyState == STATE_RECORD)
    {
      RECORD_LED_ON;
      PLAY_LED_OFF;
    }
    else if (curlyState == STATE_PLAYBACK)
    {
      RECORD_LED_OFF;
      PLAY_LED_ON;
    }
}

void main() {
  byte inputbyte;
  byte heartbeat;
  // since we've declared #use fast_io(B) (above), we MUST 
  // include a call to set_tris_b() at startup.
  // 
  set_tris_a(IRX_A_TRIS);
  set_tris_b(IRX_B_TRIS);
  set_tris_d(IRX_D_TRIS);
  set_tris_e(IRX_E_TRIS);
  setup_port_a(NO_ANALOGS);
  port_b_pullups(TRUE);

  all_off();

  RED_LED_ON;                    // reality check at startup
  RECORD_LED_ON;
  PLAY_LED_ON;
  left_motor_fwd();
  delay_ms(250);
  left_motor_rev();
  delay_ms(250);
  left_motor_stop();
  right_motor_fwd();
  delay_ms(250);
  right_motor_rev();
  delay_ms(250);
  right_motor_stop();
  RED_LED_OFF;
  RECORD_LED_OFF;
  PLAY_LED_OFF;
  delay_ms(500);

  init_ram();

  while (1) {
    check_mode_button();
    display_mode_indicators();
    inputbyte = get_stick_positions();
    if (curlyState == STATE_NORMAL)
    {
      perform_motor_actions(inputbyte);
    }
    else if (curlyState == STATE_RECORD)
    {
      perform_motor_actions(inputbyte);
      write_ram(sampleCount, inputbyte);
      sampleCount++;
      if (sampleCount >= MAX_SAMPLES)
      {
        curlyState = STATE_PLAYBACK;
        currentSample = 0;
      }
    }
    else if (curlyState == STATE_PLAYBACK)
    {
      inputbyte = read_ram(currentSample);
      if ((inputbyte & 0xF0) == 0xF0)
        perform_motor_actions(inputbyte);
      currentSample++;
      if (currentSample >= sampleCount)
        currentSample = 0;
    }
    if (heartbeat == 0)
    {
      RED_LED_ON; heartbeat = 1;
    }
    else
    {
      RED_LED_OFF; heartbeat = 0;
    }
    delay_ms(50);
  }
}