Ad van der Weiden / Mbed 2 deprecated ftusbClass

class library to access fischertechnik interfaces via USB

Dependencies:   FatFileSystem mbed myBlueUSB neigbourhood rfcomm sdp

ftlib/ftlibclasscom.cpp@1:4676e8b9b357, 2013-03-11 (annotated)

Committer:
networker
Date:
Mon Mar 11 08:04:37 2013 +0000
Revision:
1:4676e8b9b357
Parent:
0:7da612835693 
first publication of this experimental class, just for sharing wip

Who changed what in which revision?

UserRevisionLine numberNew contents of line
networker 0:7da612835693 1 #include "mbed.h"
networker 0:7da612835693 2 #include "ftlibclasscom.h"
networker 0:7da612835693 3 vector<ftcommdev*> ftcommdev::devs;
networker 0:7da612835693 4
networker 0:7da612835693 5 ftcommdev::ftcommdev(Serial *s, unsigned t, unsigned c): ftdev(t, 0) {
networker 0:7da612835693 6 device = s;
networker 0:7da612835693 7 port = 0;
networker 0:7da612835693 8 FT_TRANSFER_AREA *area = new FT_TRANSFER_AREA;
networker 0:7da612835693 9 memset(area, 0, sizeof(struct _FT_TRANSFER_AREA));
networker 0:7da612835693 10 area->TransferAktiv = 0;
networker 0:7da612835693 11 //query_time = INTERFACE_QUERY_TIME_SERIAL;
networker 0:7da612835693 12 area->RfModulNr = -1;
networker 0:7da612835693 13 if (t == FT_INTELLIGENT_IF)
networker 0:7da612835693 14 area->BusModules = 1;
networker 0:7da612835693 15 else if (t == FT_INTELLIGENT_IF_SLAVE)
networker 0:7da612835693 16 area->BusModules = 2;
networker 0:7da612835693 17 ta = area;
networker 0:7da612835693 18 }
networker 0:7da612835693 19
networker 0:7da612835693 20 //blocking
networker 0:7da612835693 21 int ftcommdev::read(Serial *stream, unsigned char *buf, int n, int timeout_ms) {
networker 0:7da612835693 22 Timer t;
networker 0:7da612835693 23 t.start();
networker 0:7da612835693 24 int i = 0;
networker 0:7da612835693 25 while (t.read_ms() < timeout_ms && i < n) {
networker 0:7da612835693 26 if (stream->readable())
networker 0:7da612835693 27 buf[i++] = stream->getc();
networker 0:7da612835693 28 }
networker 0:7da612835693 29 return i;
networker 0:7da612835693 30 }
networker 0:7da612835693 31
networker 0:7da612835693 32 int ftcommdev::write(Serial *d, unsigned char *ptr, int n, int timeout_ms) {
networker 0:7da612835693 33 Timer t;
networker 0:7da612835693 34 t.start();
networker 0:7da612835693 35 int i = 0;
networker 0:7da612835693 36 while (t.read_ms() < timeout_ms && i < n) {
networker 0:7da612835693 37 if (d->writeable())
networker 0:7da612835693 38 d->putc(ptr[i++]);
networker 0:7da612835693 39 }
networker 0:7da612835693 40 return i;
networker 0:7da612835693 41 }
networker 0:7da612835693 42
networker 0:7da612835693 43 //non-blocking
networker 0:7da612835693 44 int ftcommdev::write() {
networker 0:7da612835693 45 windex = 0;
networker 0:7da612835693 46 writeByte(); //write the first byte, callback will take care of the rest
networker 0:7da612835693 47 return num_write;
networker 0:7da612835693 48 }
networker 0:7da612835693 49 void ftcommdev::writeByte() {
networker 0:7da612835693 50 if (windex < num_write) {
networker 0:7da612835693 51 device->putc(out[windex++]);
networker 0:7da612835693 52 if (windex == num_write)
networker 0:7da612835693 53 rindex = 0;
networker 0:7da612835693 54 }
networker 0:7da612835693 55 }
networker 0:7da612835693 56 void ftcommdev::readByte() {
networker 0:7da612835693 57 if (rindex < num_read) {
networker 0:7da612835693 58 in[rindex++] = device->getc();
networker 0:7da612835693 59 if (rindex == num_read) {
networker 0:7da612835693 60 FtThreadEnd();
networker 0:7da612835693 61 }
networker 0:7da612835693 62 }
networker 0:7da612835693 63 }
networker 0:7da612835693 64
networker 0:7da612835693 65 ftcommdev* ftcommdev::OpenFtCommDevice(unsigned sDevice, unsigned dwTyp, unsigned dwZyklus, unsigned *pdwError) {//makes a (blocking) comm request to the interface
networker 0:7da612835693 66 Serial *dev = 0;
networker 0:7da612835693 67 switch (sDevice) {
networker 0:7da612835693 68 case 1:
networker 0:7da612835693 69 dev = new Serial(p9, p10);
networker 0:7da612835693 70 break;
networker 0:7da612835693 71 case 2:
networker 0:7da612835693 72 dev = new Serial(p13, p14);
networker 0:7da612835693 73 break;
networker 0:7da612835693 74 case 3:
networker 0:7da612835693 75 dev = new Serial(p28, p27);
networker 0:7da612835693 76 break;
networker 0:7da612835693 77 // default: dev = new Serial(p9, p10); break;
networker 0:7da612835693 78 default:
networker 0:7da612835693 79 // dev = &viaUsb;
networker 0:7da612835693 80 break;
networker 0:7da612835693 81 }
networker 0:7da612835693 82 ftcommdev* com = OpenFtCommDevice(dev, dwTyp, dwZyklus, pdwError);
networker 0:7da612835693 83 if (com) {
networker 0:7da612835693 84 com->port = sDevice;
networker 0:7da612835693 85 printf("opening of COM%d OK\n", sDevice);
networker 0:7da612835693 86 }
networker 0:7da612835693 87 return com;
networker 0:7da612835693 88 }
networker 0:7da612835693 89
networker 0:7da612835693 90 ftcommdev* ftcommdev::OpenFtCommDevice(Serial *dev, unsigned dwTyp, unsigned dwZyklus, unsigned *pdwError) {//makes a (blocking) comm request to the interface
networker 0:7da612835693 91 unsigned char in[5];
networker 0:7da612835693 92 unsigned char on[] = " ft-Robo-ON-V1";
networker 0:7da612835693 93 on[0] = 0xA1;
networker 0:7da612835693 94 ftcommdev* ret = 0;
networker 0:7da612835693 95 if (dwTyp != FT_ROBO_IF_COM) {
networker 0:7da612835693 96 ret = new ftcommdevii(dev, dwTyp, dwZyklus);
networker 0:7da612835693 97 dev->baud(BAUDRATE_II);
networker 0:7da612835693 98 } else {// dwTyp == FT_ROBO_IF_COM
networker 0:7da612835693 99 ret = new ftcommdev(dev, dwTyp, dwZyklus);
networker 0:7da612835693 100 dev->baud(BAUDRATE_RI);
networker 0:7da612835693 101 }
networker 0:7da612835693 102 printf("about to send '%s'\n", on);
networker 0:7da612835693 103 if (dwTyp == FT_ROBO_IF_COM) {
networker 0:7da612835693 104 int sent = write(dev, on, strlen((const char*)on), 100/*ms*/);
networker 0:7da612835693 105 // int sent = dev->printf("%s", on);
networker 0:7da612835693 106 printf("sent %d bytes to COM\n", sent);
networker 0:7da612835693 107 if (sent == strlen((const char*)on)) {
networker 0:7da612835693 108 if (read(dev, in, 5, 1000/*ms*/) == 5) { // if (dev->scanf("%5c", in) == 1) { does not work, could have to do with NUL chars or needs a lookahead char
networker 0:7da612835693 109 printf("%02X: interface version: %d.%d.%d.%d\n", in[0], in[4], in[3], in[2], in[1]);
networker 0:7da612835693 110 ret->fw = *(unsigned*)(in+1);
networker 0:7da612835693 111 if ((in[0] ^ on[0]) == 0x0FFU) {
networker 0:7da612835693 112 printf("opening of %s OK\n", "UNKNOWN");
networker 0:7da612835693 113 } else {
networker 0:7da612835693 114 printf("return code is %02X but should be %02X\n", in[0], ~on[0]&0xFF);
networker 0:7da612835693 115 delete ret;
networker 0:7da612835693 116 return NULL;
networker 0:7da612835693 117 }
networker 0:7da612835693 118 } else {
networker 0:7da612835693 119 printf("read did not return 5\n");
networker 0:7da612835693 120 delete ret;
networker 0:7da612835693 121 return NULL;
networker 0:7da612835693 122 }
networker 0:7da612835693 123 } else {
networker 0:7da612835693 124 printf("only %d chars were sent i.o %d\n", sent, strlen((const char*)on));
networker 0:7da612835693 125 delete ret;
networker 0:7da612835693 126 return NULL;
networker 0:7da612835693 127 }
networker 0:7da612835693 128 }
networker 0:7da612835693 129 devs.push_back(ret);
networker 0:7da612835693 130 return ret;
networker 0:7da612835693 131 }
networker 0:7da612835693 132
networker 0:7da612835693 133 unsigned ftcommdev::CloseFtDevice() {//sends a comm request
networker 0:7da612835693 134 unsigned char off = 0xA2;
networker 0:7da612835693 135 unsigned char in[1];
networker 0:7da612835693 136
networker 0:7da612835693 137 while (ta->TransferAktiv != 0) {
networker 0:7da612835693 138 fprintf(stderr, "Transfer ta still active\n");
networker 0:7da612835693 139 sleep(1);
networker 0:7da612835693 140 }
networker 0:7da612835693 141
networker 0:7da612835693 142 if (type == FT_ROBO_IF_COM) {
networker 0:7da612835693 143 if (write(device, &off, 1, 500) != 1 || read(device, in, 1, 1000) != 1 || (in[0]^off != 0xFF)) {
networker 0:7da612835693 144 fprintf(stderr, "CloseFtDevice: Error communicating with serial\n");
networker 0:7da612835693 145 }
networker 0:7da612835693 146 }
networker 0:7da612835693 147 for (vector<ftcommdev*>::iterator it = devs.begin(); it < devs.end(); it++)
networker 0:7da612835693 148 if (*it == this) {
networker 0:7da612835693 149 devs.erase(it);
networker 0:7da612835693 150 delete this;
networker 0:7da612835693 151 return 0;
networker 0:7da612835693 152 }
networker 0:7da612835693 153 return 0;
networker 0:7da612835693 154 }
networker 0:7da612835693 155
networker 0:7da612835693 156 unsigned ftcommdev::GetFtSerialNr() {
networker 0:7da612835693 157 int ret;
networker 0:7da612835693 158 unsigned char buffer[35] = { 0 };
networker 0:7da612835693 159 if (sn > 0)
networker 0:7da612835693 160 return sn;
networker 0:7da612835693 161 switch (type) {
networker 0:7da612835693 162 case FT_INTELLIGENT_IF:
networker 0:7da612835693 163 case FT_INTELLIGENT_IF_SLAVE:
networker 0:7da612835693 164 return 0;
networker 0:7da612835693 165 case FT_ROBO_IF_COM:
networker 0:7da612835693 166 buffer[0] = 0xf0;
networker 0:7da612835693 167 buffer[1] = 0x02;
networker 0:7da612835693 168 ret = write(device, buffer, 2, 500);
networker 0:7da612835693 169 if (ret != 2) {
networker 0:7da612835693 170 fprintf(stderr, "Error writing msg 0xF0 0x02\n");
networker 0:7da612835693 171 return 0;
networker 0:7da612835693 172 }
networker 0:7da612835693 173 ret = read(device, buffer, 5, 1000);
networker 0:7da612835693 174 if (ret != 5) {
networker 0:7da612835693 175 fprintf(stderr, "Error reading msg 0xF0 0x02\n");
networker 0:7da612835693 176 return 0;
networker 0:7da612835693 177 }
networker 0:7da612835693 178 //sn = buffer[1] + buffer[2]*100 + buffer[3]*10000 + buffer[4]*1000000;
networker 0:7da612835693 179 sn = *(unsigned*)(buffer+1);
networker 0:7da612835693 180 break;
networker 0:7da612835693 181 default:
networker 0:7da612835693 182 return FTLIB_ERR_NOT_SUPPORTED;
networker 0:7da612835693 183 }
networker 0:7da612835693 184 return sn;
networker 0:7da612835693 185 }
networker 0:7da612835693 186
networker 0:7da612835693 187 char * ftcommdev::GetFtLongNameStrg() {
networker 0:7da612835693 188 switch (type) {
networker 0:7da612835693 189 case FT_INTELLIGENT_IF:
networker 0:7da612835693 190 return strdup("Intelligent Interface");
networker 0:7da612835693 191 case FT_INTELLIGENT_IF_SLAVE:
networker 0:7da612835693 192 return strdup("Intelligent Interface with Slave");
networker 0:7da612835693 193 case FT_ROBO_IF_IIM:
networker 0:7da612835693 194 return strdup("Robo Interface in Intelligent Interface mode");
networker 0:7da612835693 195 case FT_ROBO_IF_COM:
networker 0:7da612835693 196 return strdup("Robo Interface on Com");
networker 0:7da612835693 197 default:
networker 0:7da612835693 198 return strdup("Unknown interface type");
networker 0:7da612835693 199 }
networker 0:7da612835693 200 }
networker 0:7da612835693 201
networker 0:7da612835693 202 char * ftcommdev::GetFtShortNameStrg() {
networker 0:7da612835693 203 switch (type) {
networker 0:7da612835693 204 case FT_INTELLIGENT_IF:
networker 0:7da612835693 205 return strdup("IIF");
networker 0:7da612835693 206 case FT_INTELLIGENT_IF_SLAVE:
networker 0:7da612835693 207 return strdup("IIF w/Slave");
networker 0:7da612835693 208 case FT_ROBO_IF_IIM:
networker 0:7da612835693 209 return strdup("RI_IIM");
networker 0:7da612835693 210 case FT_ROBO_IF_COM:
networker 0:7da612835693 211 return strdup("RICom");
networker 0:7da612835693 212 default:
networker 0:7da612835693 213 return strdup("Unknown");
networker 0:7da612835693 214 }
networker 0:7da612835693 215 }
networker 0:7da612835693 216
networker 0:7da612835693 217 void ftcommdev::poll() {
networker 0:7da612835693 218 for (int i = 0; i < devs.size(); i++) {
networker 0:7da612835693 219 if (devs[i]->triggered) {
networker 0:7da612835693 220 if (devs[i]->guardedFtThreadBegin())
networker 0:7da612835693 221 devs[i]->triggered = 0;
networker 0:7da612835693 222 }
networker 0:7da612835693 223 }
networker 0:7da612835693 224 }
networker 0:7da612835693 225
networker 0:7da612835693 226 void ftcommdev::FtThreadInit() {//setup buffers for this type of interface
networker 0:7da612835693 227 device->attach(this, &ftcommdev::writeByte, Serial::TxIrq);
networker 0:7da612835693 228 device->attach(this, &ftcommdev::readByte, Serial::RxIrq);
networker 0:7da612835693 229 ftdev::FtThreadInit();
networker 0:7da612835693 230 out[0] = 0xf2;
networker 0:7da612835693 231 num_write = 17;
networker 0:7da612835693 232 num_read = 21;
networker 0:7da612835693 233 }
networker 0:7da612835693 234
networker 0:7da612835693 235 //here the real data exchange starts
networker 0:7da612835693 236 void ftcommdev::FtThreadBegin() {//called every 10ms to issue a request, should be non-blocking
networker 0:7da612835693 237 if (!test_and_set()) {//return when transferarea is in use
networker 0:7da612835693 238 busy = false; //release the mutex, otherwise the thread effectively stops
networker 0:7da612835693 239 return;//return because there is no point in sending a nonsense request, alternatively the lock can be ignored in which case the data may be inconsistent
networker 0:7da612835693 240 }
networker 0:7da612835693 241 out[1] = ta->M_Main;
networker 0:7da612835693 242 out[2] = (ta->MPWM_Main[0] & 0x7) | (ta->MPWM_Main[1]<<3 & 0x38) | (ta->MPWM_Main[2]<<6 & 0xC0);
networker 0:7da612835693 243 out[3] = (ta->MPWM_Main[2] & 0x1) | (ta->MPWM_Main[3]<<1 & 0xE) | (ta->MPWM_Main[4]<<4 & 0x70) | (ta->MPWM_Main[5]<<7 & 0x80);
networker 0:7da612835693 244 out[4] = (ta->MPWM_Main[5] & 0x3) | (ta->MPWM_Main[6]<<2 & 0x1C) | (ta->MPWM_Main[7]<<5 & 0xE0);
networker 0:7da612835693 245 out[5] = ta->M_Sub1;
networker 0:7da612835693 246 out[6] = (ta->MPWM_Sub1[0] & 0x7) | (ta->MPWM_Sub1[1]<<3 & 0x38) | (ta->MPWM_Sub1[2]<<6 & 0xC0);
networker 0:7da612835693 247 out[7] = (ta->MPWM_Sub1[2] & 0x1) | (ta->MPWM_Sub1[3]<<1 & 0xE) | (ta->MPWM_Sub1[4]<<4 & 0x70) | (ta->MPWM_Sub1[5]<<7 & 0x80);
networker 0:7da612835693 248 out[8] = (ta->MPWM_Sub1[5] & 0x3) | (ta->MPWM_Sub1[6]<<2 & 0x1C) | (ta->MPWM_Sub1[7]<<5 & 0xE0);
networker 0:7da612835693 249 out[9] = ta->M_Sub2;
networker 0:7da612835693 250 out[10] = (ta->MPWM_Sub2[0] & 0x7) | (ta->MPWM_Sub2[1]<<3 & 0x38) | (ta->MPWM_Sub2[2]<<6 & 0xC0);
networker 0:7da612835693 251 out[11] = (ta->MPWM_Sub2[2] & 0x1) | (ta->MPWM_Sub2[3]<<1 & 0xE) | (ta->MPWM_Sub2[4]<<4 & 0x70) | (ta->MPWM_Sub2[5]<<7 & 0x80);
networker 0:7da612835693 252 out[12] = (ta->MPWM_Sub2[5] & 0x3) | (ta->MPWM_Sub2[6]<<2 & 0x1C) | (ta->MPWM_Sub2[7]<<5 & 0xE0);
networker 0:7da612835693 253 out[13] = ta->M_Sub3;
networker 0:7da612835693 254 out[14] = (ta->MPWM_Sub3[0] & 0x7) | (ta->MPWM_Sub3[1]<<3 & 0x38) | (ta->MPWM_Sub3[2]<<6 & 0xC0);
networker 0:7da612835693 255 out[15] = (ta->MPWM_Sub3[2] & 0x1) | (ta->MPWM_Sub3[3]<<1 & 0xE) | (ta->MPWM_Sub3[4]<<4 & 0x70) | (ta->MPWM_Sub3[5]<<7 & 0x80);
networker 0:7da612835693 256 out[16] = (ta->MPWM_Sub3[5] & 0x3) | (ta->MPWM_Sub3[6]<<2 & 0x1C) | (ta->MPWM_Sub3[7]<<5 & 0xE0);
networker 0:7da612835693 257 out[17] = 0;
networker 0:7da612835693 258 out[18] = 0;
networker 0:7da612835693 259 out[19] = 0;
networker 0:7da612835693 260 out[20] = 0;
networker 0:7da612835693 261 out[21] = 0;
networker 0:7da612835693 262 out[22] = 0;
networker 0:7da612835693 263 out[23] = 0;
networker 0:7da612835693 264 out[24] = 0;
networker 0:7da612835693 265 out[25] = 0;
networker 0:7da612835693 266 out[26] = 0;
networker 0:7da612835693 267 out[27] = 0;
networker 0:7da612835693 268 out[28] = 0;
networker 0:7da612835693 269 out[29] = 0;
networker 0:7da612835693 270 out[30] = 0;
networker 0:7da612835693 271 out[31] = 0;
networker 0:7da612835693 272
networker 0:7da612835693 273 increment();//release the lock on shared memeory
networker 0:7da612835693 274 write();
networker 0:7da612835693 275 }
networker 0:7da612835693 276 #if 0
networker 0:7da612835693 277 void ftcommdev::FtThreadEnd() {//called by the receiver/dma callback when the reply is complete
networker 0:7da612835693 278 if (!test_and_set()) {//skip when busy
networker 0:7da612835693 279 busy = false;
networker 0:7da612835693 280 return;
networker 0:7da612835693 281 }
networker 0:7da612835693 282 ta->ChangeEg = ta->E_Main != in[0] || ta->E_Sub1 != in[1] || ta->E_Sub2 != in[2] || ta->E_Sub3 != in[3];
networker 0:7da612835693 283 ta->E_Main = in[0];
networker 0:7da612835693 284 ta->E_Sub1 = in[1];
networker 0:7da612835693 285 ta->E_Sub2 = in[2];
networker 0:7da612835693 286 ta->E_Sub3 = in[3];
networker 0:7da612835693 287 ta->ChangeAn = 1; //assume that analog always changes (noise)
networker 0:7da612835693 288 ta->AX = in[4];
networker 0:7da612835693 289 ta->AY = in[5];
networker 0:7da612835693 290 ta->A1 = in[6];
networker 0:7da612835693 291 ta->A2 = in[7];
networker 0:7da612835693 292 ta->AX |= (in[8] & 0x3) << 8;
networker 0:7da612835693 293 ta->AY |= (in[8] & 0xC) << 6;
networker 0:7da612835693 294 ta->A1 |= (in[8] & 0x30) << 4;
networker 0:7da612835693 295 ta->A2 |= (in[8] & 0xC0) << 2;
networker 0:7da612835693 296 ta->AZ = in[9];
networker 0:7da612835693 297 ta->D1 = in[10];
networker 0:7da612835693 298 ta->D2 = in[11];
networker 0:7da612835693 299 ta->AV = in[12];
networker 0:7da612835693 300 ta->AZ |= (in[13] & 0x3) << 8;
networker 0:7da612835693 301 ta->D1 |= (in[13] & 0xC) << 6;
networker 0:7da612835693 302 ta->D2 |= (in[13] & 0x30) << 4;
networker 0:7da612835693 303 ta->AV |= (in[13] & 0xC0) << 2;
networker 0:7da612835693 304 if (ta->IRKeys != in[14])
networker 0:7da612835693 305 ta->ChangeIr = 1;
networker 0:7da612835693 306 ta->IRKeys = in[14];
networker 0:7da612835693 307 ta->BusModules = in[15];
networker 0:7da612835693 308 // 16
networker 0:7da612835693 309 ta->AXS1 = in[17];
networker 0:7da612835693 310 ta->AXS2 = in[18];
networker 0:7da612835693 311 ta->AXS3 = in[19];
networker 0:7da612835693 312 ta->AXS1 |= (in[20] & 0x3) << 8;
networker 0:7da612835693 313 ta->AXS2 |= (in[20] & 0xC) << 6;
networker 0:7da612835693 314 ta->AXS3 |= (in[20] & 0x30) << 4;
networker 0:7da612835693 315 // 21
networker 0:7da612835693 316 ta->AVS1 = in[22];
networker 0:7da612835693 317 ta->AVS2 = in[23];
networker 0:7da612835693 318 ta->AVS3 = in[24];
networker 0:7da612835693 319 ta->AVS1 |= (in[25] & 0x3) << 8;
networker 0:7da612835693 320 ta->AVS2 |= (in[25] & 0xC) << 6;
networker 0:7da612835693 321 ta->AVS3 |= (in[25] & 0x30) << 4;
networker 0:7da612835693 322 // 26...42
networker 0:7da612835693 323 increment();
networker 0:7da612835693 324 interface_connected = 1;
networker 0:7da612835693 325 if (ne.NotificationCallback) {
networker 0:7da612835693 326 (*ne.NotificationCallback)(ne.Context);
networker 0:7da612835693 327 }
networker 0:7da612835693 328 busy = false;
networker 0:7da612835693 329 }
networker 0:7da612835693 330 #endif
networker 0:7da612835693 331
networker 0:7da612835693 332 void ftcommdev::FtThreadFinish() {//called by StopFtTransferArea
networker 0:7da612835693 333 device->attach(0,Serial::RxIrq);
networker 0:7da612835693 334 device->attach(0,Serial::TxIrq);
networker 0:7da612835693 335 ftdev::FtThreadFinish();
networker 0:7da612835693 336 }
networker 0:7da612835693 337
networker 0:7da612835693 338 void ftcommdevii::FtThreadBegin() {//called every 10ms to issue a request, should be non-blocking
networker 0:7da612835693 339 if (!test_and_set()) {//return when transferarea is in use
networker 0:7da612835693 340 busy = false; //release the mutex, otherwise the thread effectively stops
networker 0:7da612835693 341 return;//return because there is no point in sending a nonsense request, alternatively the lock can be ignored in which case the data may be inconsistent
networker 0:7da612835693 342 }
networker 0:7da612835693 343 out[1] = ta->M_Main;
networker 0:7da612835693 344 out[2] = ta->M_Sub1;
networker 0:7da612835693 345
networker 0:7da612835693 346 // For the II we need to simulate different speeds here
networker 0:7da612835693 347 for (int iCurMotor = 0; iCurMotor < 7; iCurMotor++) {
networker 0:7da612835693 348 if (ta->MPWM_Main[iCurMotor] < ii_speed) out[1] &= ~(1 << iCurMotor);
networker 0:7da612835693 349 if (ta->MPWM_Sub1[iCurMotor] < ii_speed) out[2] &= ~(1 << iCurMotor);
networker 0:7da612835693 350 }
networker 0:7da612835693 351 ii_speed++;
networker 0:7da612835693 352 if (ii_speed > 7) ii_speed = 0;
networker 0:7da612835693 353
networker 0:7da612835693 354 num_write=2;
networker 0:7da612835693 355 switch (cycle) {
networker 0:7da612835693 356 case 0:
networker 0:7da612835693 357 num_read=3;
networker 0:7da612835693 358 out[0] = 0xc5;
networker 0:7da612835693 359 break;
networker 0:7da612835693 360 case 1:
networker 0:7da612835693 361 num_read=3;
networker 0:7da612835693 362 out[0] = 0xc9;
networker 0:7da612835693 363 break;
networker 0:7da612835693 364 default:
networker 0:7da612835693 365 num_read=1;
networker 0:7da612835693 366 out[0] = 0xc1;
networker 0:7da612835693 367 break;
networker 0:7da612835693 368 }
networker 0:7da612835693 369 if (type == FT_INTELLIGENT_IF_SLAVE) {
networker 0:7da612835693 370 num_write++;
networker 0:7da612835693 371 num_read++;
networker 0:7da612835693 372 out[0]++;
networker 0:7da612835693 373 }
networker 0:7da612835693 374 if (cycle++ > analogcycle) cycle=0;
networker 0:7da612835693 375 increment();//release the lock on shared memeory
networker 0:7da612835693 376 write();
networker 0:7da612835693 377 }
networker 0:7da612835693 378
networker 0:7da612835693 379 void ftcommdevii::FtThreadEnd() {//called by the receiver/dma callback when the reply is complete
networker 0:7da612835693 380 if (!test_and_set()) {//skip when busy
networker 0:7da612835693 381 busy = false;
networker 0:7da612835693 382 return;
networker 0:7da612835693 383 }
networker 0:7da612835693 384 ta->ChangeEg = ta->E_Main != in[0];
networker 0:7da612835693 385 ta->E_Main = in[0];
networker 0:7da612835693 386 ta->BusModules = type==FT_INTELLIGENT_IF_SLAVE;
networker 0:7da612835693 387 switch (out[0]) {
networker 0:7da612835693 388 case 0xc1:
networker 0:7da612835693 389 break;
networker 0:7da612835693 390 case 0xc5:
networker 0:7da612835693 391 ta->AX = in[1] | (8<<in[2]);
networker 0:7da612835693 392 break;
networker 0:7da612835693 393 case 0xc9:
networker 0:7da612835693 394 ta->AY = in[1] | (8<<in[2]);
networker 0:7da612835693 395 break;
networker 0:7da612835693 396 case 0xc2:
networker 0:7da612835693 397 ta->ChangeEg = ta->ChangeEg || ta->E_Sub1 != in[1];
networker 0:7da612835693 398 ta->E_Sub1 = in[1];
networker 0:7da612835693 399 break;
networker 0:7da612835693 400 case 0xc6:
networker 0:7da612835693 401 ta->ChangeEg = ta->ChangeEg || ta->E_Sub1 != in[1];
networker 0:7da612835693 402 ta->E_Sub1 = in[1];
networker 0:7da612835693 403 ta->AX = in[2] | (8<<in[3]);
networker 0:7da612835693 404 break;
networker 0:7da612835693 405 case 0xca:
networker 0:7da612835693 406 ta->ChangeEg = ta->ChangeEg || ta->E_Sub1 != in[1];
networker 0:7da612835693 407 ta->E_Sub1 = in[1];
networker 0:7da612835693 408 ta->AY = in[2] | (8<<in[3]);
networker 0:7da612835693 409 break;
networker 0:7da612835693 410 }
networker 0:7da612835693 411 increment();
networker 0:7da612835693 412 interface_connected = 1;
networker 0:7da612835693 413 if (ne.NotificationCallback) {
networker 0:7da612835693 414 (*ne.NotificationCallback)(ne.Context);
networker 0:7da612835693 415 }
networker 0:7da612835693 416 busy = false;
networker 0:7da612835693 417 }
networker 0:7da612835693 418
networker 0:7da612835693 419 unsigned ftcommdev::SetFtDistanceSensorMode(unsigned dwMode, unsigned dwTol1, unsigned dwTol2, unsigned dwLevel1, unsigned dwLevel2, unsigned dwRepeat1, unsigned dwRepeat2) {
networker 0:7da612835693 420 unsigned char buffer[11];
networker 0:7da612835693 421
networker 0:7da612835693 422 if (type != FT_ROBO_IF_COM)
networker 0:7da612835693 423 return FTLIB_ERR_NOT_SUPPORTED;
networker 0:7da612835693 424
networker 0:7da612835693 425 buffer[0] = 0xf1;
networker 0:7da612835693 426 buffer[1] = 0x01;
networker 0:7da612835693 427 buffer[2] = dwMode;
networker 0:7da612835693 428 buffer[3] = dwTol1;
networker 0:7da612835693 429 buffer[4] = dwTol2;
networker 0:7da612835693 430 buffer[5] = dwLevel1;
networker 0:7da612835693 431 buffer[6] = dwLevel1>>8;
networker 0:7da612835693 432 buffer[7] = dwLevel2;
networker 0:7da612835693 433 buffer[8] = dwLevel2>>8;
networker 0:7da612835693 434 buffer[9] = dwRepeat1;
networker 0:7da612835693 435 buffer[10] = dwRepeat2;
networker 0:7da612835693 436
networker 0:7da612835693 437 if ((write(device, buffer, 11, 500)) != 11 || (read(device, buffer, 1, 500)) != 1 || buffer[0] != 0x01) {
networker 0:7da612835693 438 fprintf(stderr, "SetFtDistanceSensorMode: Error communicating with serial\n");
networker 0:7da612835693 439 return buffer[0];
networker 0:7da612835693 440 }
networker 0:7da612835693 441 usleep(100000); // wait before continue, else it doesn't always work
networker 0:7da612835693 442 return FTLIB_ERR_SUCCESS;
networker 0:7da612835693 443 }
networker 0:7da612835693 444
networker 0:7da612835693 445 unsigned ftcommdev::pgm_message(unsigned code, unsigned dwMemBlock) {
networker 0:7da612835693 446 unsigned char buffer[2];
networker 0:7da612835693 447 if (type != FT_ROBO_IF_COM) return FTLIB_ERR_NOT_SUPPORTED;
networker 0:7da612835693 448 buffer[0] = code;
networker 0:7da612835693 449 buffer[1] = dwMemBlock;
networker 0:7da612835693 450 if ((write(device, buffer, 2, 500)) != 2 || (read(device, buffer, 1, 500)) != 1) {
networker 0:7da612835693 451 return FTLIB_ERR_IF_NO_PROGRAM;
networker 0:7da612835693 452 }
networker 0:7da612835693 453 if ((buffer[0]) == 0x1) return FTLIB_ERR_SUCCESS;
networker 0:7da612835693 454 else return FTLIB_ERR_IF_NO_PROGRAM;
networker 0:7da612835693 455 }
networker 0:7da612835693 456