File content as of revision 0:7f26f0680202:

//$Id$
/*****************************************************************************************
following interrupts are used:
module    source            freq    prio    blocking    latency    duration
IR: timer1 capture        low        low        N            <200us    med            when IR active f ~ 1kHz
    timer1 overflow        low        low        Y            <10ms    short        f = 28Hz
ext    int0 (ack)            med        low        N            <50us    long        f ~ 2kHz
    spi                 med     low     Y            <5us    short        f ~ 2kHz
485    RX                    high    high    Y            <5us    medium        f = 92kHz burst
    TX                    high    high    Y            <10us    short        f = 92kHz burst
servo    timer0 comp        low        high    Y            <1us    very short    f = 200Hz @4 servos
timer    timer2 comp        med        low        N            <20us    long        f = 10kHz
distance    ADC            med        low        N            <20us    med            f = 9kHz
tx-bridge    eeprom        low        low        Y

the timer 2 interrupt takes care of timeouts (not critical) and event scheduling (servo and extension).
Because timer2 is interruptable these processes are effectively background.
The IR interrupts could be made non-blocking but a race between capture and overflow will result.
The share variable is 'overflow', possibly the capture int could be made non-blocking.
The servo is very sensitive to jitter probably due to latency caused by RX/TX (jitter is now 16us).
This can only be reduced by making RX/TX non-blocking (but this causes other problems) or a hardware 
solution with external latches reset by the timer output.

*****************************************************************************************/

#if 0 //for reference only

/* Fuse settings:
 * SPIEN (default), allows ISP
 * EESAVE (optional), keeps eeprom settings
 * factory default CKDIV8 must be unprogrammed!
 * for SUK_CKSEL  I used ext X-tal >8MHz 16K CK
 * this resulted in: Ext F9, High D7, Low FF
 */

#define DEBUG        //the debug statements are actually defined to drive a pin
#define HW_VERSION    'A'    //can be 'A', 'B' or 'C'
#define HW_EXTENSION    'B'
#define SW_MAJOR    1L
#define SW_MINOR    6L
#define SW_EXTENSION    0x00010000L
#define TA_VERSION    0x08010101 /* copied from real TX data */
#define REVISION    "$Rev: 39 $"


//#include <stdint.h>
#include <string.h>
#include "data.h"
#include "rs485.h"      //uses UART (PORTD0-1), PORTD4 as transmit enable
#include "robo_ext.h"   //uses PORTD5-7 as a0-a2, PORTD2 as ack, SPI (PORTB3-5) 
#include "timer.h"        //uses timer2 as system timer 100us
#include "servo.h"        //uses timer0 as one shot timer, PORTC0-3 as servo outputs
#include "ir.h"            //uses timer1 as free running counter, ICP1 (PORTB0) as IR input
#include "distance.h"    //uses PORTC0-3 as analog inputs
#include "command.h"
#include "stepper.h"
//#include "i2c.h"        //uses TWI (PORTC4-5)

char timeP[] PROGMEM = __TIME__; 
char dateP[] PROGMEM = __DATE__; 
char revP[] PROGMEM = "$Rev: 39 $"; 

message msg = {{0x5502,{0},0,0,0,2,0,1,0},{{{{0,0,0,0},{0,0,0,0},{0,0,0,0,0,0,0,0},{0,0,0,0}},{0,3}}}};
struct _to_ext to_ext[MAXEXT] = {{0x0D,0xFF,0xFFFFFFFF}, //the 0x0D is mandatory, the 0xFF are actually zero's because data is inverted
                                 {0x0D,0xFF,0xFFFFFFFF},
                                 {0x0D,0xFF,0xFFFFFFFF},
                                 {0x0D,0xFF,0xFFFFFFFF},
                                 {0x0D,0xFF,0xFFFFFFFF},
                                 {0x0D,0xFF,0xFFFFFFFF},
                                 {0x0D,0xFF,0xFFFFFFFF}
                                };
struct _from_ext from_ext[MAXEXT];
uint8_t nrofext = 0;
struct _conf config[SLAVES];

//uint32_t slave_address[SLAVES];
//uint32_t ee_slave_address[SLAVES] __attribute__((section(".eeprom"))) = {3,4,5,6,7,8,9,10};

const TA_INFO  my_interface_info[SLAVES] PROGMEM = {
  {"ROBO I/O Ext 1","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
  {"ROBO I/O Ext 2","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
  {"ROBO I/O Ext 3","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
  {"ROBO I/O Ext 4","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
  {"ROBO I/O Ext 5","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
  {"ROBO I/O Ext 6","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
  {"ROBO I/O Ext 7","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
  {"ROBO-TX bridge","00:00:00:00:00:00",0,0,0,0,{{HW_VERSION},{SW_MINOR<<24 | SW_MAJOR<<16},{TA_VERSION},{0x01,0x01,0x01,0x04}}}
};

char ee_names[SLAVES][17] __attribute__((section(".eeprom"))) = { "ROBO I/O Ext 1",
                                                                  "ROBO I/O Ext 2",
                                                                    "ROBO I/O Ext 3",
                                                                  "ROBO I/O Ext 4",
                                                                  "ROBO I/O Ext 5",
                                                                  "ROBO I/O Ext 6",
                                                                  "ROBO I/O Ext 7",
                                                                  "ROBO-TX bridge"
                                                                };
unsigned char ee_thisindex __attribute__((section(".eeprom"))) = 7;//7 corresponds to extension 8, any higher number will not be recognised (slave not present)
unsigned char thisindex = 7; //ram copy of ee_thisindex                                                             
volatile int8_t tx_message=-3;
UINT16 my_session_id[SLAVES]; //rolling session IDs for each slave
static char name[20]; //eeprom buffer
static char *eeptr = name;
static unsigned eesiz;
char eeprom_busy = 0;

UINT16 checksum(UINT16 n)
{ UINT16 sum = 0;
  n += 2; //include length in checksum
  char *p = (char*)&msg.hdr.bytes;
  do
  { sum -= *p++;
  } while (--n);
  return (sum<<8)|(sum>>8);
}

void copy_from_ext(short int idx)
{ short int i;
  if (idx == thisindex)
  { 
#ifdef USE_IR
    get_IR();
#endif
#ifdef USE_DISTANCE
    for (i = 4; i < 8; i++)
      if (config[idx].dist & (1<<i))
        msg.body.cmd102.input.uni[i] = get_distance(i-4);
#endif
#ifdef USE_STEPPER
//copy position reached info and counter values
    for (i = 0; i < 4; i++)
    { msg.body.cmd102.input.counter[i] = steppers[i].pos;
      msg.body.cmd102.input.motor_pos_reached[i] = steppers[i].pos == steppers[i].dest;
    }
#endif
  }
  else //it's an io extension
  { 
#ifdef USE_ROBO_EXT
    for (i = 0; i < 8; i++)
      msg.body.cmd102.input.uni[i] = from_ext[idx].inputs & (1<<i) ? 0 : 1;
    if (!(config[idx].uni & 1))
    { int v = 950 - (from_ext[idx].ax + ((int)(from_ext[idx].high&0x03)<<8));
      if (v<0)
        v = 0;
      else
        v = (v<<2) +  v + (v>>2);
      msg.body.cmd102.input.uni[0] = v;
    }
    if (!(config[idx].uni & 2))
      msg.body.cmd102.input.uni[1] = 1023 - (from_ext[idx].az + ((int)(from_ext[idx].high&0x0c)<<6));
    if (!(config[idx].uni & 4))
    { int v = 1023 - (from_ext[idx].av + ((int)(from_ext[idx].high&0x30)<<4));
      msg.body.cmd102.input.uni[2] = 3*v;
    }
#endif
  }
  //fill the other data with zeros
  for (i = 0; i < N_CNT; i++)
  { msg.body.cmd102.input.cnt_in[i] = 0;
    msg.body.cmd102.input.counter[i] = 0;
  }
  msg.body.cmd102.input.display_button_left = 0;
  msg.body.cmd102.input.display_button_right = 0;
  for (i = 0; i < N_MOTOR; i++)
  { msg.body.cmd102.input.motor_pos_reached[i] = 0;
  }
}

/* the length as set in the block is the length of the header minus the 4 byte leader
   and minus the TA-id (4 bytes) + the size of the payload data + the size of the TA-id
*/

void send_102(short int idx)
{ //msg.hdr.bytes = sizeof(header)+sizeof(TA_INPUT) - 4;
  msg.hdr.bytesH = (sizeof(header)+sizeof(TA_INPUT) - 4) >> 8;
  msg.hdr.bytesL = (sizeof(header)+sizeof(TA_INPUT) - 4) & 0xFF;
  msg.hdr.cmd = 102;
  msg.hdr.rec = msg.hdr.snd;
//  msg.hdr.snd = slave_address[idx];
  msg.hdr.snd = idx+3;
  msg.hdr.session = my_session_id[idx];
  copy_from_ext(idx); 
  msg.body.cmd102.trl.chksum = checksum(sizeof(header)+sizeof(TA_INPUT) - 4); 
  msg.body.cmd102.trl.etx = 3;
//  send_rs485(); 
  rs485_delay = RX_TO_TX_DELAY;
}

void send_105(short int idx)
{ //msg.hdr.bytes = sizeof(header) - 4;
  msg.hdr.bytesH = (sizeof(header) - 4) >> 8;
  msg.hdr.bytesL = (sizeof(header) - 4) & 0xFF;
  msg.hdr.cmd = 105;
  msg.hdr.rec = msg.hdr.snd;
//  msg.hdr.snd = slave_address[idx];
  msg.hdr.snd = idx+3;
  msg.body.cmd105.trl.etx = 3;
//  send_rs485(); 
  if (msg.hdr.session == 0) //first config req
  { msg.hdr.session = ++my_session_id[idx];//reply with the next SID
    msg.body.cmd105.trl.chksum = checksum(sizeof(header) - 4); 
    rs485_delay = 110;
  }
  else //second config req
  { msg.hdr.session = my_session_id[idx];//reply with the same SID
    msg.body.cmd105.trl.chksum = checksum(sizeof(header) - 4); 
    rs485_delay = 55;
  }
}

void send_106(short int idx)
{ //msg.hdr.bytes = sizeof(header)+sizeof(TA_INFO) - 4;
  msg.hdr.bytesH = (sizeof(header)+sizeof(TA_INFO) - 4) >> 8;
  msg.hdr.bytesL = (sizeof(header)+sizeof(TA_INFO) - 4) & 0xFF;
  msg.hdr.cmd = 106;
  msg.hdr.rec = msg.hdr.snd;
//  msg.hdr.snd = slave_address[idx];
  msg.hdr.snd = idx+3;
  my_session_id[idx]++; //bump the session-id 
  my_session_id[idx] |= 1; //and make sure it is odd
  msg.hdr.session = my_session_id[idx]; 
  if (idx==thisindex)//if the bridge is addressed
    idx = 7; //always use the name of the bridge
  memcpy_P(&msg.body.cmd106.info, (PGM_P)&my_interface_info[idx], sizeof(TA_INFO)); 
  if (!eeprom_busy)
    eeprom_read_block(&msg.body.cmd106.info, &ee_names[idx][0], DEV_NAME_LEN+1); //overwrite the name
  //else tough luck, use the original name 
  msg.body.cmd106.trl.chksum = checksum(sizeof(header)+sizeof(TA_INFO) - 4); 
  msg.body.cmd106.trl.etx = 3;
//  send_rs485(); 
  rs485_delay = RX_TO_TX_DELAY;
}

void send_empty_reply(short int idx)
{ msg.hdr.bytesH = (sizeof(header) - 4) >> 8;
  msg.hdr.bytesL = (sizeof(header) - 4) & 0xFF;
  msg.hdr.cmd += 100;
  msg.hdr.rec = msg.hdr.snd;
  msg.hdr.snd = idx+3;
  msg.hdr.session = my_session_id[idx];
  msg.body.empty.trl.chksum = checksum(sizeof(header) - 4); 
  msg.body.empty.trl.etx = 3;
//  send_rs485(); 
  rs485_delay = RX_TO_TX_DELAY;
}

void store_config(short int idx)
{ short int i;
  config[idx].motor = 0;
  config[idx].uni = 0;
  config[idx].dist = 0;
  for (i = 0; i < 4; i++)
    if (msg.body.cmd005.config.motor[i])
      config[idx].motor |= 1<<i;
  for (i = 0; i < 8; i++)
  { if (msg.body.cmd005.config.uni[i].digital)
      config[idx].uni |= 1<<i;
    if (msg.body.cmd005.config.uni[i].mode == 3)
      config[idx].dist |= 1<<i;
  }
#ifdef USE_SERVO
  if (idx == thisindex)
    enable_servos(~config[idx].dist>>4);
#endif
}

/*
short int find_ad(uint32_t ad)
{ short int i;
  for (i = 0; i < SLAVES; i++)
    if (slave_address[i] == ad)
      return i;
  return -1;
}
*/

#if 0
void copy_to_ext(short int idx)
{ //copy motor data from tx msg to io ext
  short int i;
  uint32_t pwm;
  uint32_t ext_pwm = 0;
  char m = 0;
  if (idx == 7)
  { set_servos();
    return;
  }
  for (i = 0; i < 4; i++)
  { if (config[idx].motor & (1<<i))
    { //treat as motor, this must be fixed!!!
      //in fact it does not need a fix because it works fine
      pwm = msg.body.cmd002.output.duty[i<<1];
      pwm /= 57; //reduce from 9 bits to 3 bits
      if (pwm)
      { m |= 1<<(2*i);
        pwm--;
      }
      ext_pwm |= pwm<<(6*i);

      pwm = msg.body.cmd002.output.duty[(i<<1)+1];
      pwm /= 57; //reduce from 9 bits to 3 bits
      if (pwm)
      { m |= 1<<(2*i+1);
        pwm--;
      }
      ext_pwm |= pwm<<(6*i+3);
    }
    else
    { //treat as 2 outputs
      pwm = msg.body.cmd002.output.duty[i<<1];
      pwm /= 57; //reduce from 9 bits to 3 bits
      if (pwm)
      { m |= 1<<(2*i);
        pwm--;
      }
      ext_pwm |= pwm<<(6*i);

      pwm = msg.body.cmd002.output.duty[(i<<1)+1];
      pwm /= 57; //reduce from 9 bits to 3 bits
      if (pwm)
      { m |= 1<<(2*i+1);
        pwm--;
      }
      ext_pwm |= pwm<<(6*i+3);
    }
  }
  to_ext[idx].pwm = ~ext_pwm & 0x00FFFFFF;
  to_ext[idx].motors = ~m;
}
#else
void copy_to_ext(short int idx)
{ //copy motor data from tx msg to io ext
  short int i;
  uint32_t pwm;
  uint32_t ext_pwm = 0;
  char m = 0;
  if (idx == thisindex)
  { 
#ifdef USE_SERVO
    set_servos();
#endif
#ifdef USE_STEPPER
//copy position info and speed info
    for (i = 0; i < 4; i++)
    { steppers[i].dest = msg.body.cmd002.output.distance[i];
      steppers[i].speed = msg.body.cmd002.output.duty[2*i] - msg.body.cmd002.output.duty[2*i+1];
      if (msg.body.cmd002.output.cnt_reset[i])
        steppers[i].pos = 0;
    }
#endif
    return;
  }
#ifdef USE_ROBO_EXT
  for (i = 0; i < 8; i++)
  {   pwm = msg.body.cmd002.output.duty[i];
      pwm /= 57; //reduce from 9 bits to 3 bits, 0-511 is mapped to 0-8, 0 means off and 1-8 are pwm values 0-7
      if (pwm)
      { m |= 1<<i;
        pwm--;
      }
      ext_pwm |= pwm<<(3*i);
  }
  to_ext[idx].pwm = ~ext_pwm & 0x00FFFFFF;
  to_ext[idx].motors = ~m;
#endif
}
#endif

#ifndef COMMAND_H
ISR(EE_READY_vect)//don't make this non_blocking, the eeprom interrupt flag is still pending!
{ EEDR = *eeptr;
  EEAR++;//address must be set BEFORE starting write, therefore we start at address-1
  EECR = _BV(EEMPE) | _BV(EERIE);
  EECR |= _BV(EEPE);
  //if timing is really critical, interrupts could be re-enabled here
  if (*eeptr)//this actually wrong because eeprom_busy is cleared while the last write is still in progress
  { eeptr++;
  }
  else
  { EECR = 0; //disable interrupt
    eeprom_busy = 0;
  }
}
#else
ISR(EE_READY_vect)//don't make this non_blocking, the eeprom interrupt flag is still pending!
{ if (eesiz)
  { EEDR = *eeptr;
    EEAR++;//address must be set BEFORE starting write, therefore we start at address-1
    EECR = _BV(EEMPE) | _BV(EERIE);
    EECR |= _BV(EEPE);
  //if timing is really critical, interrupts could be re-enabled here
    eesiz--;
    eeptr++;
  }
  else
  { EECR = 0; //disable interrupt
    eeprom_busy = 0;
  }
}
#endif

#ifndef COMMAND_H
void eeprom_write_string_noblock(char s[], char *eadr)
{ eeprom_busy = 1;
  eeptr = s;
  EEAR = (unsigned)eadr-1;
  EECR |= _BV(EERIE);
}
#else
void eeprom_write_string_noblock(char s[], char *eadr)
{ eeprom_write_block_nonblocking(s, eadr, strlen(s)+1);
}
#endif

void eeprom_write_block_nonblocking(char *src, char *dst, int size)
{ if (eeprom_busy || size == 0)
    return;
  eeprom_busy = 1;
  eeptr = src;
  eesiz = size;
  EEAR = (unsigned)dst - 1;
  EECR |= _BV(EERIE);
}

void store_index()
{ if (eeprom_busy)
    return;
  eeprom_write_byte(&ee_thisindex, thisindex);
}

void init()
{ DDRB = _BV(PB1) | _BV(PB2); //debug on pins 15 and 16, for historical reasons the macros to set/clear these pins refer to pins 11 and 12
  PORTD = _BV(PD0); //enable the internal pullup on the RX input, pullup not possible on int0 => external pullup
  //recommended by atmel to activate pullup for unused pins
  //unused pins are: pc4-6 (sda,scl,reset), pd3 (int1)
  //pb1-2 (debug) are not really unused because they are outputs
  PORTC = _BV(PC4) | _BV(PC5) | _BV(PC6);
  PORTD |= _BV(PD3);
}

int main()
{ init();
  init_rs485();
#ifdef USE_ROBO_EXT
  init_extension();
#endif
#ifdef USE_IR
  init_IR();
#endif
#ifdef USE_SERVO
  init_servos();
#endif
  init_timer();
#ifdef USE_DISTANCE
  init_distance();
#endif
#ifdef USE_STEPPER
  init_stepper();
#endif
//  eeprom_read_block(slave_address, ee_slave_address, sizeof(slave_address));
  thisindex = eeprom_read_byte(&ee_thisindex);
  sei(); //enable interrupts
  for(;;)
  { if (tx_message>=0)//is there a message?, if there is then rx and tx are disabled
    { int idx = tx_message;//find_ad(msg.hdr.rec);
      if ((/* idx >= 0 && */ idx < nrofext) || idx==thisindex) //is the message for us?
      { switch (msg.hdr.cmd)
        { case 2: if (msg.body.cmd002.trl.chksum != checksum(msg.hdr.bytes))
                  { enable_rx_rs485();
                    break;
                  }
                  copy_to_ext(idx);
                  send_102(idx); //data transfer
                  break;
          case 5: if (msg.body.cmd005.trl.chksum != checksum(msg.hdr.bytes))
                  { enable_rx_rs485();
                    break;
                  }
                  store_config(idx);
                  send_105(idx); //configuration
                  break;
          case 6: if (msg.body.cmd006.trl.chksum != checksum(msg.hdr.bytes))
                  { enable_rx_rs485();
                    break;
                  }
                  send_106(idx); //discovery
                  break;
          case 8: if (msg.body.cmd008.trl.chksum != checksum(msg.hdr.bytes))
                  { enable_rx_rs485();
                    break;
                  }
#ifdef COMMAND_H
                  handle_command(idx);
#else
                  //completely ignore this message
#endif
                  send_empty_reply(idx); //display message
                  break;
          case 9: if (msg.body.cmd009.trl.chksum != checksum(msg.hdr.bytes))
                  { enable_rx_rs485();
                    break;
                  }
#if 0
                  //the next function is too slow and should be replaced by a non-blocking function
                  eeprom_write_block(msg.body.cmd009.name, &ee_names[idx][0], DEV_NAME_LEN+1);
#else
                  strcpy(name, msg.body.cmd009.name); 
                  eeprom_write_string_noblock(name, &ee_names[idx][0]);
#endif
                  send_empty_reply(idx); //name change
                  break;
          default: //SETPIN12; //trigger the logic analyser so the command can be investigated
                   enable_rx_rs485();
                   break;
        }//switch
        //if we arrive here, one of the following is true: 1)the checksum was wrong (we are in receive mode); 
        //                                                 2)a reply was initiated (we are in send mode);
        //                                                 3)an unknown cmd was received (we are in receive mode).
      }//if idx
      else
      { 
        enable_rx_rs485(); //go back to receive mode when msg not for us
      } 
        tx_message = -3; //clear message
    }//if message
  } //for
}//main

#endif