Ad van der Weiden
Program the control the fischertechnik robo interface or intelligent interface via tcp socket or via a java gui.
Diff: tx-bridge.c
- Revision:
- 0:7f26f0680202
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/tx-bridge.c Fri Dec 31 14:01:14 2010 +0000
@@ -0,0 +1,532 @@
+//$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
+