Ad van der Weiden / Mbed 2 deprecated tcpft

Program the control the fischertechnik robo interface or intelligent interface via tcp socket or via a java gui.

Dependencies:   mbed ConfigFile

tx-bridge.c@0:7f26f0680202, 2010-12-31 (annotated)

Committer:
networker
Date:
Fri Dec 31 14:01:14 2010 +0000
Revision:
0:7f26f0680202
initial release: comprises ftlib (no usb), ft-over-ip socket server, and the http server (the html page and java jar I still have to publish somewhere)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
networker 0:7f26f0680202 1 //$Id$
networker 0:7f26f0680202 2 /*****************************************************************************************
networker 0:7f26f0680202 3 following interrupts are used:
networker 0:7f26f0680202 4 module source freq prio blocking latency duration
networker 0:7f26f0680202 5 IR: timer1 capture low low N <200us med when IR active f ~ 1kHz
networker 0:7f26f0680202 6 timer1 overflow low low Y <10ms short f = 28Hz
networker 0:7f26f0680202 7 ext int0 (ack) med low N <50us long f ~ 2kHz
networker 0:7f26f0680202 8 spi med low Y <5us short f ~ 2kHz
networker 0:7f26f0680202 9 485 RX high high Y <5us medium f = 92kHz burst
networker 0:7f26f0680202 10 TX high high Y <10us short f = 92kHz burst
networker 0:7f26f0680202 11 servo timer0 comp low high Y <1us very short f = 200Hz @4 servos
networker 0:7f26f0680202 12 timer timer2 comp med low N <20us long f = 10kHz
networker 0:7f26f0680202 13 distance ADC med low N <20us med f = 9kHz
networker 0:7f26f0680202 14 tx-bridge eeprom low low Y
networker 0:7f26f0680202 15
networker 0:7f26f0680202 16 the timer 2 interrupt takes care of timeouts (not critical) and event scheduling (servo and extension).
networker 0:7f26f0680202 17 Because timer2 is interruptable these processes are effectively background.
networker 0:7f26f0680202 18 The IR interrupts could be made non-blocking but a race between capture and overflow will result.
networker 0:7f26f0680202 19 The share variable is 'overflow', possibly the capture int could be made non-blocking.
networker 0:7f26f0680202 20 The servo is very sensitive to jitter probably due to latency caused by RX/TX (jitter is now 16us).
networker 0:7f26f0680202 21 This can only be reduced by making RX/TX non-blocking (but this causes other problems) or a hardware
networker 0:7f26f0680202 22 solution with external latches reset by the timer output.
networker 0:7f26f0680202 23
networker 0:7f26f0680202 24 *****************************************************************************************/
networker 0:7f26f0680202 25
networker 0:7f26f0680202 26 #if 0 //for reference only
networker 0:7f26f0680202 27
networker 0:7f26f0680202 28 /* Fuse settings:
networker 0:7f26f0680202 29 * SPIEN (default), allows ISP
networker 0:7f26f0680202 30 * EESAVE (optional), keeps eeprom settings
networker 0:7f26f0680202 31 * factory default CKDIV8 must be unprogrammed!
networker 0:7f26f0680202 32 * for SUK_CKSEL I used ext X-tal >8MHz 16K CK
networker 0:7f26f0680202 33 * this resulted in: Ext F9, High D7, Low FF
networker 0:7f26f0680202 34 */
networker 0:7f26f0680202 35
networker 0:7f26f0680202 36 #define DEBUG //the debug statements are actually defined to drive a pin
networker 0:7f26f0680202 37 #define HW_VERSION 'A' //can be 'A', 'B' or 'C'
networker 0:7f26f0680202 38 #define HW_EXTENSION 'B'
networker 0:7f26f0680202 39 #define SW_MAJOR 1L
networker 0:7f26f0680202 40 #define SW_MINOR 6L
networker 0:7f26f0680202 41 #define SW_EXTENSION 0x00010000L
networker 0:7f26f0680202 42 #define TA_VERSION 0x08010101 /* copied from real TX data */
networker 0:7f26f0680202 43 #define REVISION "$Rev: 39 $"
networker 0:7f26f0680202 44
networker 0:7f26f0680202 45
networker 0:7f26f0680202 46 //#include <stdint.h>
networker 0:7f26f0680202 47 #include <string.h>
networker 0:7f26f0680202 48 #include "data.h"
networker 0:7f26f0680202 49 #include "rs485.h" //uses UART (PORTD0-1), PORTD4 as transmit enable
networker 0:7f26f0680202 50 #include "robo_ext.h" //uses PORTD5-7 as a0-a2, PORTD2 as ack, SPI (PORTB3-5)
networker 0:7f26f0680202 51 #include "timer.h" //uses timer2 as system timer 100us
networker 0:7f26f0680202 52 #include "servo.h" //uses timer0 as one shot timer, PORTC0-3 as servo outputs
networker 0:7f26f0680202 53 #include "ir.h" //uses timer1 as free running counter, ICP1 (PORTB0) as IR input
networker 0:7f26f0680202 54 #include "distance.h" //uses PORTC0-3 as analog inputs
networker 0:7f26f0680202 55 #include "command.h"
networker 0:7f26f0680202 56 #include "stepper.h"
networker 0:7f26f0680202 57 //#include "i2c.h" //uses TWI (PORTC4-5)
networker 0:7f26f0680202 58
networker 0:7f26f0680202 59 char timeP[] PROGMEM = __TIME__;
networker 0:7f26f0680202 60 char dateP[] PROGMEM = __DATE__;
networker 0:7f26f0680202 61 char revP[] PROGMEM = "$Rev: 39 $";
networker 0:7f26f0680202 62
networker 0:7f26f0680202 63 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}}}};
networker 0:7f26f0680202 64 struct _to_ext to_ext[MAXEXT] = {{0x0D,0xFF,0xFFFFFFFF}, //the 0x0D is mandatory, the 0xFF are actually zero's because data is inverted
networker 0:7f26f0680202 65 {0x0D,0xFF,0xFFFFFFFF},
networker 0:7f26f0680202 66 {0x0D,0xFF,0xFFFFFFFF},
networker 0:7f26f0680202 67 {0x0D,0xFF,0xFFFFFFFF},
networker 0:7f26f0680202 68 {0x0D,0xFF,0xFFFFFFFF},
networker 0:7f26f0680202 69 {0x0D,0xFF,0xFFFFFFFF},
networker 0:7f26f0680202 70 {0x0D,0xFF,0xFFFFFFFF}
networker 0:7f26f0680202 71 };
networker 0:7f26f0680202 72 struct _from_ext from_ext[MAXEXT];
networker 0:7f26f0680202 73 uint8_t nrofext = 0;
networker 0:7f26f0680202 74 struct _conf config[SLAVES];
networker 0:7f26f0680202 75
networker 0:7f26f0680202 76 //uint32_t slave_address[SLAVES];
networker 0:7f26f0680202 77 //uint32_t ee_slave_address[SLAVES] __attribute__((section(".eeprom"))) = {3,4,5,6,7,8,9,10};
networker 0:7f26f0680202 78
networker 0:7f26f0680202 79 const TA_INFO my_interface_info[SLAVES] PROGMEM = {
networker 0:7f26f0680202 80 {"ROBO I/O Ext 1","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
networker 0:7f26f0680202 81 {"ROBO I/O Ext 2","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
networker 0:7f26f0680202 82 {"ROBO I/O Ext 3","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
networker 0:7f26f0680202 83 {"ROBO I/O Ext 4","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
networker 0:7f26f0680202 84 {"ROBO I/O Ext 5","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
networker 0:7f26f0680202 85 {"ROBO I/O Ext 6","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
networker 0:7f26f0680202 86 {"ROBO I/O Ext 7","00:00:00:00:00:00",0,0,0,0,{{HW_EXTENSION},{SW_EXTENSION},{TA_VERSION},"\0\0\0"}},
networker 0:7f26f0680202 87 {"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}}}
networker 0:7f26f0680202 88 };
networker 0:7f26f0680202 89
networker 0:7f26f0680202 90 char ee_names[SLAVES][17] __attribute__((section(".eeprom"))) = { "ROBO I/O Ext 1",
networker 0:7f26f0680202 91 "ROBO I/O Ext 2",
networker 0:7f26f0680202 92 "ROBO I/O Ext 3",
networker 0:7f26f0680202 93 "ROBO I/O Ext 4",
networker 0:7f26f0680202 94 "ROBO I/O Ext 5",
networker 0:7f26f0680202 95 "ROBO I/O Ext 6",
networker 0:7f26f0680202 96 "ROBO I/O Ext 7",
networker 0:7f26f0680202 97 "ROBO-TX bridge"
networker 0:7f26f0680202 98 };
networker 0:7f26f0680202 99 unsigned char ee_thisindex __attribute__((section(".eeprom"))) = 7;//7 corresponds to extension 8, any higher number will not be recognised (slave not present)
networker 0:7f26f0680202 100 unsigned char thisindex = 7; //ram copy of ee_thisindex
networker 0:7f26f0680202 101 volatile int8_t tx_message=-3;
networker 0:7f26f0680202 102 UINT16 my_session_id[SLAVES]; //rolling session IDs for each slave
networker 0:7f26f0680202 103 static char name[20]; //eeprom buffer
networker 0:7f26f0680202 104 static char *eeptr = name;
networker 0:7f26f0680202 105 static unsigned eesiz;
networker 0:7f26f0680202 106 char eeprom_busy = 0;
networker 0:7f26f0680202 107
networker 0:7f26f0680202 108 UINT16 checksum(UINT16 n)
networker 0:7f26f0680202 109 { UINT16 sum = 0;
networker 0:7f26f0680202 110 n += 2; //include length in checksum
networker 0:7f26f0680202 111 char *p = (char*)&msg.hdr.bytes;
networker 0:7f26f0680202 112 do
networker 0:7f26f0680202 113 { sum -= *p++;
networker 0:7f26f0680202 114 } while (--n);
networker 0:7f26f0680202 115 return (sum<<8)|(sum>>8);
networker 0:7f26f0680202 116 }
networker 0:7f26f0680202 117
networker 0:7f26f0680202 118 void copy_from_ext(short int idx)
networker 0:7f26f0680202 119 { short int i;
networker 0:7f26f0680202 120 if (idx == thisindex)
networker 0:7f26f0680202 121 {
networker 0:7f26f0680202 122 #ifdef USE_IR
networker 0:7f26f0680202 123 get_IR();
networker 0:7f26f0680202 124 #endif
networker 0:7f26f0680202 125 #ifdef USE_DISTANCE
networker 0:7f26f0680202 126 for (i = 4; i < 8; i++)
networker 0:7f26f0680202 127 if (config[idx].dist & (1<<i))
networker 0:7f26f0680202 128 msg.body.cmd102.input.uni[i] = get_distance(i-4);
networker 0:7f26f0680202 129 #endif
networker 0:7f26f0680202 130 #ifdef USE_STEPPER
networker 0:7f26f0680202 131 //copy position reached info and counter values
networker 0:7f26f0680202 132 for (i = 0; i < 4; i++)
networker 0:7f26f0680202 133 { msg.body.cmd102.input.counter[i] = steppers[i].pos;
networker 0:7f26f0680202 134 msg.body.cmd102.input.motor_pos_reached[i] = steppers[i].pos == steppers[i].dest;
networker 0:7f26f0680202 135 }
networker 0:7f26f0680202 136 #endif
networker 0:7f26f0680202 137 }
networker 0:7f26f0680202 138 else //it's an io extension
networker 0:7f26f0680202 139 {
networker 0:7f26f0680202 140 #ifdef USE_ROBO_EXT
networker 0:7f26f0680202 141 for (i = 0; i < 8; i++)
networker 0:7f26f0680202 142 msg.body.cmd102.input.uni[i] = from_ext[idx].inputs & (1<<i) ? 0 : 1;
networker 0:7f26f0680202 143 if (!(config[idx].uni & 1))
networker 0:7f26f0680202 144 { int v = 950 - (from_ext[idx].ax + ((int)(from_ext[idx].high&0x03)<<8));
networker 0:7f26f0680202 145 if (v<0)
networker 0:7f26f0680202 146 v = 0;
networker 0:7f26f0680202 147 else
networker 0:7f26f0680202 148 v = (v<<2) + v + (v>>2);
networker 0:7f26f0680202 149 msg.body.cmd102.input.uni[0] = v;
networker 0:7f26f0680202 150 }
networker 0:7f26f0680202 151 if (!(config[idx].uni & 2))
networker 0:7f26f0680202 152 msg.body.cmd102.input.uni[1] = 1023 - (from_ext[idx].az + ((int)(from_ext[idx].high&0x0c)<<6));
networker 0:7f26f0680202 153 if (!(config[idx].uni & 4))
networker 0:7f26f0680202 154 { int v = 1023 - (from_ext[idx].av + ((int)(from_ext[idx].high&0x30)<<4));
networker 0:7f26f0680202 155 msg.body.cmd102.input.uni[2] = 3*v;
networker 0:7f26f0680202 156 }
networker 0:7f26f0680202 157 #endif
networker 0:7f26f0680202 158 }
networker 0:7f26f0680202 159 //fill the other data with zeros
networker 0:7f26f0680202 160 for (i = 0; i < N_CNT; i++)
networker 0:7f26f0680202 161 { msg.body.cmd102.input.cnt_in[i] = 0;
networker 0:7f26f0680202 162 msg.body.cmd102.input.counter[i] = 0;
networker 0:7f26f0680202 163 }
networker 0:7f26f0680202 164 msg.body.cmd102.input.display_button_left = 0;
networker 0:7f26f0680202 165 msg.body.cmd102.input.display_button_right = 0;
networker 0:7f26f0680202 166 for (i = 0; i < N_MOTOR; i++)
networker 0:7f26f0680202 167 { msg.body.cmd102.input.motor_pos_reached[i] = 0;
networker 0:7f26f0680202 168 }
networker 0:7f26f0680202 169 }
networker 0:7f26f0680202 170
networker 0:7f26f0680202 171 /* the length as set in the block is the length of the header minus the 4 byte leader
networker 0:7f26f0680202 172 and minus the TA-id (4 bytes) + the size of the payload data + the size of the TA-id
networker 0:7f26f0680202 173 */
networker 0:7f26f0680202 174
networker 0:7f26f0680202 175 void send_102(short int idx)
networker 0:7f26f0680202 176 { //msg.hdr.bytes = sizeof(header)+sizeof(TA_INPUT) - 4;
networker 0:7f26f0680202 177 msg.hdr.bytesH = (sizeof(header)+sizeof(TA_INPUT) - 4) >> 8;
networker 0:7f26f0680202 178 msg.hdr.bytesL = (sizeof(header)+sizeof(TA_INPUT) - 4) & 0xFF;
networker 0:7f26f0680202 179 msg.hdr.cmd = 102;
networker 0:7f26f0680202 180 msg.hdr.rec = msg.hdr.snd;
networker 0:7f26f0680202 181 // msg.hdr.snd = slave_address[idx];
networker 0:7f26f0680202 182 msg.hdr.snd = idx+3;
networker 0:7f26f0680202 183 msg.hdr.session = my_session_id[idx];
networker 0:7f26f0680202 184 copy_from_ext(idx);
networker 0:7f26f0680202 185 msg.body.cmd102.trl.chksum = checksum(sizeof(header)+sizeof(TA_INPUT) - 4);
networker 0:7f26f0680202 186 msg.body.cmd102.trl.etx = 3;
networker 0:7f26f0680202 187 // send_rs485();
networker 0:7f26f0680202 188 rs485_delay = RX_TO_TX_DELAY;
networker 0:7f26f0680202 189 }
networker 0:7f26f0680202 190
networker 0:7f26f0680202 191 void send_105(short int idx)
networker 0:7f26f0680202 192 { //msg.hdr.bytes = sizeof(header) - 4;
networker 0:7f26f0680202 193 msg.hdr.bytesH = (sizeof(header) - 4) >> 8;
networker 0:7f26f0680202 194 msg.hdr.bytesL = (sizeof(header) - 4) & 0xFF;
networker 0:7f26f0680202 195 msg.hdr.cmd = 105;
networker 0:7f26f0680202 196 msg.hdr.rec = msg.hdr.snd;
networker 0:7f26f0680202 197 // msg.hdr.snd = slave_address[idx];
networker 0:7f26f0680202 198 msg.hdr.snd = idx+3;
networker 0:7f26f0680202 199 msg.body.cmd105.trl.etx = 3;
networker 0:7f26f0680202 200 // send_rs485();
networker 0:7f26f0680202 201 if (msg.hdr.session == 0) //first config req
networker 0:7f26f0680202 202 { msg.hdr.session = ++my_session_id[idx];//reply with the next SID
networker 0:7f26f0680202 203 msg.body.cmd105.trl.chksum = checksum(sizeof(header) - 4);
networker 0:7f26f0680202 204 rs485_delay = 110;
networker 0:7f26f0680202 205 }
networker 0:7f26f0680202 206 else //second config req
networker 0:7f26f0680202 207 { msg.hdr.session = my_session_id[idx];//reply with the same SID
networker 0:7f26f0680202 208 msg.body.cmd105.trl.chksum = checksum(sizeof(header) - 4);
networker 0:7f26f0680202 209 rs485_delay = 55;
networker 0:7f26f0680202 210 }
networker 0:7f26f0680202 211 }
networker 0:7f26f0680202 212
networker 0:7f26f0680202 213 void send_106(short int idx)
networker 0:7f26f0680202 214 { //msg.hdr.bytes = sizeof(header)+sizeof(TA_INFO) - 4;
networker 0:7f26f0680202 215 msg.hdr.bytesH = (sizeof(header)+sizeof(TA_INFO) - 4) >> 8;
networker 0:7f26f0680202 216 msg.hdr.bytesL = (sizeof(header)+sizeof(TA_INFO) - 4) & 0xFF;
networker 0:7f26f0680202 217 msg.hdr.cmd = 106;
networker 0:7f26f0680202 218 msg.hdr.rec = msg.hdr.snd;
networker 0:7f26f0680202 219 // msg.hdr.snd = slave_address[idx];
networker 0:7f26f0680202 220 msg.hdr.snd = idx+3;
networker 0:7f26f0680202 221 my_session_id[idx]++; //bump the session-id
networker 0:7f26f0680202 222 my_session_id[idx] |= 1; //and make sure it is odd
networker 0:7f26f0680202 223 msg.hdr.session = my_session_id[idx];
networker 0:7f26f0680202 224 if (idx==thisindex)//if the bridge is addressed
networker 0:7f26f0680202 225 idx = 7; //always use the name of the bridge
networker 0:7f26f0680202 226 memcpy_P(&msg.body.cmd106.info, (PGM_P)&my_interface_info[idx], sizeof(TA_INFO));
networker 0:7f26f0680202 227 if (!eeprom_busy)
networker 0:7f26f0680202 228 eeprom_read_block(&msg.body.cmd106.info, &ee_names[idx][0], DEV_NAME_LEN+1); //overwrite the name
networker 0:7f26f0680202 229 //else tough luck, use the original name
networker 0:7f26f0680202 230 msg.body.cmd106.trl.chksum = checksum(sizeof(header)+sizeof(TA_INFO) - 4);
networker 0:7f26f0680202 231 msg.body.cmd106.trl.etx = 3;
networker 0:7f26f0680202 232 // send_rs485();
networker 0:7f26f0680202 233 rs485_delay = RX_TO_TX_DELAY;
networker 0:7f26f0680202 234 }
networker 0:7f26f0680202 235
networker 0:7f26f0680202 236 void send_empty_reply(short int idx)
networker 0:7f26f0680202 237 { msg.hdr.bytesH = (sizeof(header) - 4) >> 8;
networker 0:7f26f0680202 238 msg.hdr.bytesL = (sizeof(header) - 4) & 0xFF;
networker 0:7f26f0680202 239 msg.hdr.cmd += 100;
networker 0:7f26f0680202 240 msg.hdr.rec = msg.hdr.snd;
networker 0:7f26f0680202 241 msg.hdr.snd = idx+3;
networker 0:7f26f0680202 242 msg.hdr.session = my_session_id[idx];
networker 0:7f26f0680202 243 msg.body.empty.trl.chksum = checksum(sizeof(header) - 4);
networker 0:7f26f0680202 244 msg.body.empty.trl.etx = 3;
networker 0:7f26f0680202 245 // send_rs485();
networker 0:7f26f0680202 246 rs485_delay = RX_TO_TX_DELAY;
networker 0:7f26f0680202 247 }
networker 0:7f26f0680202 248
networker 0:7f26f0680202 249 void store_config(short int idx)
networker 0:7f26f0680202 250 { short int i;
networker 0:7f26f0680202 251 config[idx].motor = 0;
networker 0:7f26f0680202 252 config[idx].uni = 0;
networker 0:7f26f0680202 253 config[idx].dist = 0;
networker 0:7f26f0680202 254 for (i = 0; i < 4; i++)
networker 0:7f26f0680202 255 if (msg.body.cmd005.config.motor[i])
networker 0:7f26f0680202 256 config[idx].motor |= 1<<i;
networker 0:7f26f0680202 257 for (i = 0; i < 8; i++)
networker 0:7f26f0680202 258 { if (msg.body.cmd005.config.uni[i].digital)
networker 0:7f26f0680202 259 config[idx].uni |= 1<<i;
networker 0:7f26f0680202 260 if (msg.body.cmd005.config.uni[i].mode == 3)
networker 0:7f26f0680202 261 config[idx].dist |= 1<<i;
networker 0:7f26f0680202 262 }
networker 0:7f26f0680202 263 #ifdef USE_SERVO
networker 0:7f26f0680202 264 if (idx == thisindex)
networker 0:7f26f0680202 265 enable_servos(~config[idx].dist>>4);
networker 0:7f26f0680202 266 #endif
networker 0:7f26f0680202 267 }
networker 0:7f26f0680202 268
networker 0:7f26f0680202 269 /*
networker 0:7f26f0680202 270 short int find_ad(uint32_t ad)
networker 0:7f26f0680202 271 { short int i;
networker 0:7f26f0680202 272 for (i = 0; i < SLAVES; i++)
networker 0:7f26f0680202 273 if (slave_address[i] == ad)
networker 0:7f26f0680202 274 return i;
networker 0:7f26f0680202 275 return -1;
networker 0:7f26f0680202 276 }
networker 0:7f26f0680202 277 */
networker 0:7f26f0680202 278
networker 0:7f26f0680202 279 #if 0
networker 0:7f26f0680202 280 void copy_to_ext(short int idx)
networker 0:7f26f0680202 281 { //copy motor data from tx msg to io ext
networker 0:7f26f0680202 282 short int i;
networker 0:7f26f0680202 283 uint32_t pwm;
networker 0:7f26f0680202 284 uint32_t ext_pwm = 0;
networker 0:7f26f0680202 285 char m = 0;
networker 0:7f26f0680202 286 if (idx == 7)
networker 0:7f26f0680202 287 { set_servos();
networker 0:7f26f0680202 288 return;
networker 0:7f26f0680202 289 }
networker 0:7f26f0680202 290 for (i = 0; i < 4; i++)
networker 0:7f26f0680202 291 { if (config[idx].motor & (1<<i))
networker 0:7f26f0680202 292 { //treat as motor, this must be fixed!!!
networker 0:7f26f0680202 293 //in fact it does not need a fix because it works fine
networker 0:7f26f0680202 294 pwm = msg.body.cmd002.output.duty[i<<1];
networker 0:7f26f0680202 295 pwm /= 57; //reduce from 9 bits to 3 bits
networker 0:7f26f0680202 296 if (pwm)
networker 0:7f26f0680202 297 { m |= 1<<(2*i);
networker 0:7f26f0680202 298 pwm--;
networker 0:7f26f0680202 299 }
networker 0:7f26f0680202 300 ext_pwm |= pwm<<(6*i);
networker 0:7f26f0680202 301
networker 0:7f26f0680202 302 pwm = msg.body.cmd002.output.duty[(i<<1)+1];
networker 0:7f26f0680202 303 pwm /= 57; //reduce from 9 bits to 3 bits
networker 0:7f26f0680202 304 if (pwm)
networker 0:7f26f0680202 305 { m |= 1<<(2*i+1);
networker 0:7f26f0680202 306 pwm--;
networker 0:7f26f0680202 307 }
networker 0:7f26f0680202 308 ext_pwm |= pwm<<(6*i+3);
networker 0:7f26f0680202 309 }
networker 0:7f26f0680202 310 else
networker 0:7f26f0680202 311 { //treat as 2 outputs
networker 0:7f26f0680202 312 pwm = msg.body.cmd002.output.duty[i<<1];
networker 0:7f26f0680202 313 pwm /= 57; //reduce from 9 bits to 3 bits
networker 0:7f26f0680202 314 if (pwm)
networker 0:7f26f0680202 315 { m |= 1<<(2*i);
networker 0:7f26f0680202 316 pwm--;
networker 0:7f26f0680202 317 }
networker 0:7f26f0680202 318 ext_pwm |= pwm<<(6*i);
networker 0:7f26f0680202 319
networker 0:7f26f0680202 320 pwm = msg.body.cmd002.output.duty[(i<<1)+1];
networker 0:7f26f0680202 321 pwm /= 57; //reduce from 9 bits to 3 bits
networker 0:7f26f0680202 322 if (pwm)
networker 0:7f26f0680202 323 { m |= 1<<(2*i+1);
networker 0:7f26f0680202 324 pwm--;
networker 0:7f26f0680202 325 }
networker 0:7f26f0680202 326 ext_pwm |= pwm<<(6*i+3);
networker 0:7f26f0680202 327 }
networker 0:7f26f0680202 328 }
networker 0:7f26f0680202 329 to_ext[idx].pwm = ~ext_pwm & 0x00FFFFFF;
networker 0:7f26f0680202 330 to_ext[idx].motors = ~m;
networker 0:7f26f0680202 331 }
networker 0:7f26f0680202 332 #else
networker 0:7f26f0680202 333 void copy_to_ext(short int idx)
networker 0:7f26f0680202 334 { //copy motor data from tx msg to io ext
networker 0:7f26f0680202 335 short int i;
networker 0:7f26f0680202 336 uint32_t pwm;
networker 0:7f26f0680202 337 uint32_t ext_pwm = 0;
networker 0:7f26f0680202 338 char m = 0;
networker 0:7f26f0680202 339 if (idx == thisindex)
networker 0:7f26f0680202 340 {
networker 0:7f26f0680202 341 #ifdef USE_SERVO
networker 0:7f26f0680202 342 set_servos();
networker 0:7f26f0680202 343 #endif
networker 0:7f26f0680202 344 #ifdef USE_STEPPER
networker 0:7f26f0680202 345 //copy position info and speed info
networker 0:7f26f0680202 346 for (i = 0; i < 4; i++)
networker 0:7f26f0680202 347 { steppers[i].dest = msg.body.cmd002.output.distance[i];
networker 0:7f26f0680202 348 steppers[i].speed = msg.body.cmd002.output.duty[2*i] - msg.body.cmd002.output.duty[2*i+1];
networker 0:7f26f0680202 349 if (msg.body.cmd002.output.cnt_reset[i])
networker 0:7f26f0680202 350 steppers[i].pos = 0;
networker 0:7f26f0680202 351 }
networker 0:7f26f0680202 352 #endif
networker 0:7f26f0680202 353 return;
networker 0:7f26f0680202 354 }
networker 0:7f26f0680202 355 #ifdef USE_ROBO_EXT
networker 0:7f26f0680202 356 for (i = 0; i < 8; i++)
networker 0:7f26f0680202 357 { pwm = msg.body.cmd002.output.duty[i];
networker 0:7f26f0680202 358 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
networker 0:7f26f0680202 359 if (pwm)
networker 0:7f26f0680202 360 { m |= 1<<i;
networker 0:7f26f0680202 361 pwm--;
networker 0:7f26f0680202 362 }
networker 0:7f26f0680202 363 ext_pwm |= pwm<<(3*i);
networker 0:7f26f0680202 364 }
networker 0:7f26f0680202 365 to_ext[idx].pwm = ~ext_pwm & 0x00FFFFFF;
networker 0:7f26f0680202 366 to_ext[idx].motors = ~m;
networker 0:7f26f0680202 367 #endif
networker 0:7f26f0680202 368 }
networker 0:7f26f0680202 369 #endif
networker 0:7f26f0680202 370
networker 0:7f26f0680202 371 #ifndef COMMAND_H
networker 0:7f26f0680202 372 ISR(EE_READY_vect)//don't make this non_blocking, the eeprom interrupt flag is still pending!
networker 0:7f26f0680202 373 { EEDR = *eeptr;
networker 0:7f26f0680202 374 EEAR++;//address must be set BEFORE starting write, therefore we start at address-1
networker 0:7f26f0680202 375 EECR = _BV(EEMPE) | _BV(EERIE);
networker 0:7f26f0680202 376 EECR |= _BV(EEPE);
networker 0:7f26f0680202 377 //if timing is really critical, interrupts could be re-enabled here
networker 0:7f26f0680202 378 if (*eeptr)//this actually wrong because eeprom_busy is cleared while the last write is still in progress
networker 0:7f26f0680202 379 { eeptr++;
networker 0:7f26f0680202 380 }
networker 0:7f26f0680202 381 else
networker 0:7f26f0680202 382 { EECR = 0; //disable interrupt
networker 0:7f26f0680202 383 eeprom_busy = 0;
networker 0:7f26f0680202 384 }
networker 0:7f26f0680202 385 }
networker 0:7f26f0680202 386 #else
networker 0:7f26f0680202 387 ISR(EE_READY_vect)//don't make this non_blocking, the eeprom interrupt flag is still pending!
networker 0:7f26f0680202 388 { if (eesiz)
networker 0:7f26f0680202 389 { EEDR = *eeptr;
networker 0:7f26f0680202 390 EEAR++;//address must be set BEFORE starting write, therefore we start at address-1
networker 0:7f26f0680202 391 EECR = _BV(EEMPE) | _BV(EERIE);
networker 0:7f26f0680202 392 EECR |= _BV(EEPE);
networker 0:7f26f0680202 393 //if timing is really critical, interrupts could be re-enabled here
networker 0:7f26f0680202 394 eesiz--;
networker 0:7f26f0680202 395 eeptr++;
networker 0:7f26f0680202 396 }
networker 0:7f26f0680202 397 else
networker 0:7f26f0680202 398 { EECR = 0; //disable interrupt
networker 0:7f26f0680202 399 eeprom_busy = 0;
networker 0:7f26f0680202 400 }
networker 0:7f26f0680202 401 }
networker 0:7f26f0680202 402 #endif
networker 0:7f26f0680202 403
networker 0:7f26f0680202 404 #ifndef COMMAND_H
networker 0:7f26f0680202 405 void eeprom_write_string_noblock(char s[], char *eadr)
networker 0:7f26f0680202 406 { eeprom_busy = 1;
networker 0:7f26f0680202 407 eeptr = s;
networker 0:7f26f0680202 408 EEAR = (unsigned)eadr-1;
networker 0:7f26f0680202 409 EECR |= _BV(EERIE);
networker 0:7f26f0680202 410 }
networker 0:7f26f0680202 411 #else
networker 0:7f26f0680202 412 void eeprom_write_string_noblock(char s[], char *eadr)
networker 0:7f26f0680202 413 { eeprom_write_block_nonblocking(s, eadr, strlen(s)+1);
networker 0:7f26f0680202 414 }
networker 0:7f26f0680202 415 #endif
networker 0:7f26f0680202 416
networker 0:7f26f0680202 417 void eeprom_write_block_nonblocking(char *src, char *dst, int size)
networker 0:7f26f0680202 418 { if (eeprom_busy || size == 0)
networker 0:7f26f0680202 419 return;
networker 0:7f26f0680202 420 eeprom_busy = 1;
networker 0:7f26f0680202 421 eeptr = src;
networker 0:7f26f0680202 422 eesiz = size;
networker 0:7f26f0680202 423 EEAR = (unsigned)dst - 1;
networker 0:7f26f0680202 424 EECR |= _BV(EERIE);
networker 0:7f26f0680202 425 }
networker 0:7f26f0680202 426
networker 0:7f26f0680202 427 void store_index()
networker 0:7f26f0680202 428 { if (eeprom_busy)
networker 0:7f26f0680202 429 return;
networker 0:7f26f0680202 430 eeprom_write_byte(&ee_thisindex, thisindex);
networker 0:7f26f0680202 431 }
networker 0:7f26f0680202 432
networker 0:7f26f0680202 433 void init()
networker 0:7f26f0680202 434 { 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
networker 0:7f26f0680202 435 PORTD = _BV(PD0); //enable the internal pullup on the RX input, pullup not possible on int0 => external pullup
networker 0:7f26f0680202 436 //recommended by atmel to activate pullup for unused pins
networker 0:7f26f0680202 437 //unused pins are: pc4-6 (sda,scl,reset), pd3 (int1)
networker 0:7f26f0680202 438 //pb1-2 (debug) are not really unused because they are outputs
networker 0:7f26f0680202 439 PORTC = _BV(PC4) | _BV(PC5) | _BV(PC6);
networker 0:7f26f0680202 440 PORTD |= _BV(PD3);
networker 0:7f26f0680202 441 }
networker 0:7f26f0680202 442
networker 0:7f26f0680202 443 int main()
networker 0:7f26f0680202 444 { init();
networker 0:7f26f0680202 445 init_rs485();
networker 0:7f26f0680202 446 #ifdef USE_ROBO_EXT
networker 0:7f26f0680202 447 init_extension();
networker 0:7f26f0680202 448 #endif
networker 0:7f26f0680202 449 #ifdef USE_IR
networker 0:7f26f0680202 450 init_IR();
networker 0:7f26f0680202 451 #endif
networker 0:7f26f0680202 452 #ifdef USE_SERVO
networker 0:7f26f0680202 453 init_servos();
networker 0:7f26f0680202 454 #endif
networker 0:7f26f0680202 455 init_timer();
networker 0:7f26f0680202 456 #ifdef USE_DISTANCE
networker 0:7f26f0680202 457 init_distance();
networker 0:7f26f0680202 458 #endif
networker 0:7f26f0680202 459 #ifdef USE_STEPPER
networker 0:7f26f0680202 460 init_stepper();
networker 0:7f26f0680202 461 #endif
networker 0:7f26f0680202 462 // eeprom_read_block(slave_address, ee_slave_address, sizeof(slave_address));
networker 0:7f26f0680202 463 thisindex = eeprom_read_byte(&ee_thisindex);
networker 0:7f26f0680202 464 sei(); //enable interrupts
networker 0:7f26f0680202 465 for(;;)
networker 0:7f26f0680202 466 { if (tx_message>=0)//is there a message?, if there is then rx and tx are disabled
networker 0:7f26f0680202 467 { int idx = tx_message;//find_ad(msg.hdr.rec);
networker 0:7f26f0680202 468 if ((/* idx >= 0 && */ idx < nrofext) || idx==thisindex) //is the message for us?
networker 0:7f26f0680202 469 { switch (msg.hdr.cmd)
networker 0:7f26f0680202 470 { case 2: if (msg.body.cmd002.trl.chksum != checksum(msg.hdr.bytes))
networker 0:7f26f0680202 471 { enable_rx_rs485();
networker 0:7f26f0680202 472 break;
networker 0:7f26f0680202 473 }
networker 0:7f26f0680202 474 copy_to_ext(idx);
networker 0:7f26f0680202 475 send_102(idx); //data transfer
networker 0:7f26f0680202 476 break;
networker 0:7f26f0680202 477 case 5: if (msg.body.cmd005.trl.chksum != checksum(msg.hdr.bytes))
networker 0:7f26f0680202 478 { enable_rx_rs485();
networker 0:7f26f0680202 479 break;
networker 0:7f26f0680202 480 }
networker 0:7f26f0680202 481 store_config(idx);
networker 0:7f26f0680202 482 send_105(idx); //configuration
networker 0:7f26f0680202 483 break;
networker 0:7f26f0680202 484 case 6: if (msg.body.cmd006.trl.chksum != checksum(msg.hdr.bytes))
networker 0:7f26f0680202 485 { enable_rx_rs485();
networker 0:7f26f0680202 486 break;
networker 0:7f26f0680202 487 }
networker 0:7f26f0680202 488 send_106(idx); //discovery
networker 0:7f26f0680202 489 break;
networker 0:7f26f0680202 490 case 8: if (msg.body.cmd008.trl.chksum != checksum(msg.hdr.bytes))
networker 0:7f26f0680202 491 { enable_rx_rs485();
networker 0:7f26f0680202 492 break;
networker 0:7f26f0680202 493 }
networker 0:7f26f0680202 494 #ifdef COMMAND_H
networker 0:7f26f0680202 495 handle_command(idx);
networker 0:7f26f0680202 496 #else
networker 0:7f26f0680202 497 //completely ignore this message
networker 0:7f26f0680202 498 #endif
networker 0:7f26f0680202 499 send_empty_reply(idx); //display message
networker 0:7f26f0680202 500 break;
networker 0:7f26f0680202 501 case 9: if (msg.body.cmd009.trl.chksum != checksum(msg.hdr.bytes))
networker 0:7f26f0680202 502 { enable_rx_rs485();
networker 0:7f26f0680202 503 break;
networker 0:7f26f0680202 504 }
networker 0:7f26f0680202 505 #if 0
networker 0:7f26f0680202 506 //the next function is too slow and should be replaced by a non-blocking function
networker 0:7f26f0680202 507 eeprom_write_block(msg.body.cmd009.name, &ee_names[idx][0], DEV_NAME_LEN+1);
networker 0:7f26f0680202 508 #else
networker 0:7f26f0680202 509 strcpy(name, msg.body.cmd009.name);
networker 0:7f26f0680202 510 eeprom_write_string_noblock(name, &ee_names[idx][0]);
networker 0:7f26f0680202 511 #endif
networker 0:7f26f0680202 512 send_empty_reply(idx); //name change
networker 0:7f26f0680202 513 break;
networker 0:7f26f0680202 514 default: //SETPIN12; //trigger the logic analyser so the command can be investigated
networker 0:7f26f0680202 515 enable_rx_rs485();
networker 0:7f26f0680202 516 break;
networker 0:7f26f0680202 517 }//switch
networker 0:7f26f0680202 518 //if we arrive here, one of the following is true: 1)the checksum was wrong (we are in receive mode);
networker 0:7f26f0680202 519 // 2)a reply was initiated (we are in send mode);
networker 0:7f26f0680202 520 // 3)an unknown cmd was received (we are in receive mode).
networker 0:7f26f0680202 521 }//if idx
networker 0:7f26f0680202 522 else
networker 0:7f26f0680202 523 {
networker 0:7f26f0680202 524 enable_rx_rs485(); //go back to receive mode when msg not for us
networker 0:7f26f0680202 525 }
networker 0:7f26f0680202 526 tx_message = -3; //clear message
networker 0:7f26f0680202 527 }//if message
networker 0:7f26f0680202 528 } //for
networker 0:7f26f0680202 529 }//main
networker 0:7f26f0680202 530
networker 0:7f26f0680202 531 #endif
networker 0:7f26f0680202 532