TheRobotStudio ROSA / Mbed 2 deprecated trs_master

Code for the mbed NXP LPC1768. To be used on The Robot Studio Master Boards. License : Simplified BSD.

Dependencies:   MODSERIAL mbed

main.cpp@6:c24cfbb68fc0, 2014-01-16 (annotated)

Committer:
rrknight
Date:
Thu Jan 16 08:57:10 2014 +0000
Revision:
6:c24cfbb68fc0
Parent:
5:c73f87d22142
Child:
7:7ef85554cec5
3 Slaves version (for use with 2 arms and a head demo)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rrknight 0:369222671f3c 1 #include"mbed.h"
rrknight 0:369222671f3c 2
rrknight 0:369222671f3c 3 #define MODSERIAL_DEFAULT_RX_BUFFER_SIZE 512
rrknight 0:369222671f3c 4 #define MODSERIAL_DEFAULT_TX_BUFFER_SIZE 1024
rrknight 0:369222671f3c 5 #include "MODSERIAL.h"
rrknight 0:369222671f3c 6
rrknight 0:369222671f3c 7 //Define
rrknight 0:369222671f3c 8 #define NUMBER_MAX_EPOS2_PER_SLAVE 15
rrknight 6:c24cfbb68fc0 9 #define NUMBER_SLAVE_BOARDS 3
rrknight 6:c24cfbb68fc0 10 #define NUMBER_MSG_PER_PACKET NUMBER_MAX_EPOS2_PER_SLAVE*NUMBER_SLAVE_BOARDS //45
rrknight 3:af892e4bf53e 11 #define NUMBER_BYTES_PER_MSG 6
rrknight 3:af892e4bf53e 12 #define NUMBER_BYTES_TO_READ NUMBER_BYTES_PER_MSG + 2
rrknight 2:201618ffa295 13 #define FIRST_NODE_ID_SLAVE_1 1
rrknight 2:201618ffa295 14 #define FIRST_NODE_ID_SLAVE_2 FIRST_NODE_ID_SLAVE_1 + NUMBER_MAX_EPOS2_PER_SLAVE
rrknight 2:201618ffa295 15 #define FIRST_NODE_ID_SLAVE_3 FIRST_NODE_ID_SLAVE_2 + NUMBER_MAX_EPOS2_PER_SLAVE
rrknight 6:c24cfbb68fc0 16 #define FIRST_NODE_ID_SLAVE_4 FIRST_NODE_ID_SLAVE_3 + NUMBER_MAX_EPOS2_PER_SLAVE
rrknight 0:369222671f3c 17 #define EPOS2_OK 0
rrknight 0:369222671f3c 18 #define EPOS2_ERROR -1
rrknight 0:369222671f3c 19 #define LOOP_PERIOD_TIME 20000 //20 ms
rrknight 0:369222671f3c 20
rrknight 0:369222671f3c 21 //SPI RxTx FIFO bits
rrknight 3:af892e4bf53e 22 //#define TNF 0x02
rrknight 3:af892e4bf53e 23 //#define TFE 0x01
rrknight 3:af892e4bf53e 24 //#define RNE 0x04
rrknight 0:369222671f3c 25
rrknight 2:201618ffa295 26 #define OPEN_ARROW 0x3C //< = 60
rrknight 1:95d85c81bb11 27 #define CLOSE_ARROW 0x3E //< = 62
rrknight 3:af892e4bf53e 28 #define DUMMY_BYTE 0x00
rrknight 1:95d85c81bb11 29 #define NUMBER_OF_ARROWS 5
rrknight 1:95d85c81bb11 30
rrknight 0:369222671f3c 31 MODSERIAL ros(p28, p27, 1024, 512); // tx, rx
rrknight 0:369222671f3c 32 Serial pc(USBTX, USBRX); //terminal for debug
rrknight 0:369222671f3c 33 DigitalOut ledchain[] = {(LED1), (LED2), (LED3), (LED4)}; //used for debugging
rrknight 0:369222671f3c 34 DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
rrknight 0:369222671f3c 35
rrknight 2:201618ffa295 36 SPI spi(p5, p6, p7); // mosi, miso, sclk
rrknight 2:201618ffa295 37 DigitalOut cs[NUMBER_SLAVE_BOARDS] = {(p8), (p9), (p10)}; //Slave Mbed number 1 //chip select
rrknight 2:201618ffa295 38 DigitalOut sync_slave[NUMBER_SLAVE_BOARDS] = {(p25), (p24), (p23)}; //test to sync the slave
rrknight 2:201618ffa295 39
rrknight 0:369222671f3c 40 char* readBufferSerial; //[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG]; //buffer of packets read by the master (written by the ros node on pc side)
rrknight 3:af892e4bf53e 41 uint8_t writeBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_TO_READ]; //buffer ready to be sent over SPI to different slaves
rrknight 3:af892e4bf53e 42 uint8_t readBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_TO_READ]; //buffer read by the master on SPI bus
rrknight 0:369222671f3c 43
rrknight 0:369222671f3c 44 Timer timer;
rrknight 0:369222671f3c 45 uint64_t begin, end;
rrknight 0:369222671f3c 46 uint8_t numberCmds[NUMBER_SLAVE_BOARDS];
rrknight 0:369222671f3c 47
rrknight 0:369222671f3c 48 bool newCmd_detected = false;
rrknight 0:369222671f3c 49 uint8_t nbArrows = 0;
rrknight 0:369222671f3c 50
rrknight 0:369222671f3c 51 char rByte = 0x00;
rrknight 3:af892e4bf53e 52 char writeChecksum[NUMBER_SLAVE_BOARDS]; //one checksum per board
rrknight 3:af892e4bf53e 53 char readChecksum[NUMBER_SLAVE_BOARDS];
rrknight 4:7da0cd1fcb8a 54 char serialTxChecksum = 0x00;
rrknight 1:95d85c81bb11 55
rrknight 1:95d85c81bb11 56 bool fiveArrowsFound = false;
rrknight 4:7da0cd1fcb8a 57 bool cmdValid = false;
rrknight 4:7da0cd1fcb8a 58 bool dataValid = false;
rrknight 1:95d85c81bb11 59
rrknight 1:95d85c81bb11 60 int move(char *s, int nbBytes) //custom move function (cp from MODESERIAL without the end character)
rrknight 1:95d85c81bb11 61 {
rrknight 1:95d85c81bb11 62 int counter = 0;
rrknight 1:95d85c81bb11 63 char c;
rrknight 2:201618ffa295 64
rrknight 2:201618ffa295 65 while(ros.readable())
rrknight 2:201618ffa295 66 {
rrknight 1:95d85c81bb11 67 c = ros.getc();
rrknight 1:95d85c81bb11 68 *(s++) = c;
rrknight 1:95d85c81bb11 69 counter++;
rrknight 2:201618ffa295 70 if(counter == nbBytes) break;
rrknight 1:95d85c81bb11 71 }
rrknight 2:201618ffa295 72
rrknight 1:95d85c81bb11 73 return counter;
rrknight 1:95d85c81bb11 74 }
rrknight 1:95d85c81bb11 75
rrknight 4:7da0cd1fcb8a 76 bool verifyCmdChecksum(char* data, int length, char checksum) //verify data comming from he PC
rrknight 1:95d85c81bb11 77 {
rrknight 1:95d85c81bb11 78 for(int i=0; i<length; i++)
rrknight 1:95d85c81bb11 79 {
rrknight 1:95d85c81bb11 80 checksum += data[i];
rrknight 1:95d85c81bb11 81 }
rrknight 1:95d85c81bb11 82
rrknight 1:95d85c81bb11 83 checksum++; //add 1 to obtain 0x00
rrknight 1:95d85c81bb11 84
rrknight 1:95d85c81bb11 85 if(checksum == 0x00) return true;
rrknight 1:95d85c81bb11 86 else return false;
rrknight 1:95d85c81bb11 87 }
rrknight 0:369222671f3c 88
rrknight 4:7da0cd1fcb8a 89 bool verifyDataChecksum() //verify data comming from the slaves on SPI
rrknight 4:7da0cd1fcb8a 90 {
rrknight 4:7da0cd1fcb8a 91 bool allDataValid = true;
rrknight 4:7da0cd1fcb8a 92
rrknight 4:7da0cd1fcb8a 93 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 4:7da0cd1fcb8a 94 {
rrknight 4:7da0cd1fcb8a 95 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 4:7da0cd1fcb8a 96 {
rrknight 4:7da0cd1fcb8a 97 for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
rrknight 4:7da0cd1fcb8a 98 {
rrknight 4:7da0cd1fcb8a 99 readChecksum[k] += readBufferSPI[k][i][j];
rrknight 4:7da0cd1fcb8a 100 }
rrknight 4:7da0cd1fcb8a 101 }
rrknight 4:7da0cd1fcb8a 102
rrknight 4:7da0cd1fcb8a 103 readChecksum[k]++;
rrknight 4:7da0cd1fcb8a 104
rrknight 4:7da0cd1fcb8a 105 if(readChecksum[k] != 0x00) allDataValid = false; //toggle the flag if one of them is corrupted
rrknight 4:7da0cd1fcb8a 106 }
rrknight 4:7da0cd1fcb8a 107
rrknight 4:7da0cd1fcb8a 108 return allDataValid;
rrknight 4:7da0cd1fcb8a 109 }
rrknight 4:7da0cd1fcb8a 110
rrknight 4:7da0cd1fcb8a 111 void calculateSPIChecksum() //compute checksum for each slave to send commands over SPI
rrknight 3:af892e4bf53e 112 {
rrknight 3:af892e4bf53e 113 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 3:af892e4bf53e 114 {
rrknight 3:af892e4bf53e 115 int sum = 0;
rrknight 3:af892e4bf53e 116
rrknight 3:af892e4bf53e 117 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 3:af892e4bf53e 118 {
rrknight 3:af892e4bf53e 119 for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
rrknight 3:af892e4bf53e 120 {
rrknight 4:7da0cd1fcb8a 121 sum += writeBufferSPI[k][i][j];
rrknight 3:af892e4bf53e 122 }
rrknight 3:af892e4bf53e 123 }
rrknight 3:af892e4bf53e 124
rrknight 4:7da0cd1fcb8a 125 writeChecksum[k] = (char)(~sum); //reverse 0 and 1, and cast as byte
rrknight 3:af892e4bf53e 126 }
rrknight 3:af892e4bf53e 127 }
rrknight 3:af892e4bf53e 128
rrknight 4:7da0cd1fcb8a 129 void calculateTxChecksum() //compute checksum for all the data sent to the PC over serial
rrknight 4:7da0cd1fcb8a 130 {
rrknight 4:7da0cd1fcb8a 131 int sum = 0;
rrknight 4:7da0cd1fcb8a 132
rrknight 4:7da0cd1fcb8a 133 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 4:7da0cd1fcb8a 134 {
rrknight 4:7da0cd1fcb8a 135 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 4:7da0cd1fcb8a 136 {
rrknight 4:7da0cd1fcb8a 137 for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
rrknight 4:7da0cd1fcb8a 138 {
rrknight 4:7da0cd1fcb8a 139 sum += readBufferSPI[k][i][j];
rrknight 4:7da0cd1fcb8a 140 }
rrknight 4:7da0cd1fcb8a 141 }
rrknight 4:7da0cd1fcb8a 142 }
rrknight 4:7da0cd1fcb8a 143
rrknight 4:7da0cd1fcb8a 144 serialTxChecksum = (char)(~sum); //reverse 0 and 1, and cast as byte
rrknight 4:7da0cd1fcb8a 145 }
rrknight 4:7da0cd1fcb8a 146
rrknight 0:369222671f3c 147 // Called everytime a new character goes into
rrknight 0:369222671f3c 148 // the RX buffer. Test that character for '/'
rrknight 0:369222671f3c 149 // Note, rxGetLastChar() gets the last char that
rrknight 0:369222671f3c 150 // we received but it does NOT remove it from
rrknight 0:369222671f3c 151 // the RX buffer.
rrknight 0:369222671f3c 152 void rxCallback(MODSERIAL_IRQ_INFO *q)
rrknight 0:369222671f3c 153 {
rrknight 4:7da0cd1fcb8a 154 //logicPin = 1;
rrknight 0:369222671f3c 155
rrknight 2:201618ffa295 156 MODSERIAL *serial = q->serial;
rrknight 0:369222671f3c 157 rByte = serial->rxGetLastChar();
rrknight 0:369222671f3c 158
rrknight 1:95d85c81bb11 159 if(!fiveArrowsFound)
rrknight 0:369222671f3c 160 {
rrknight 1:95d85c81bb11 161 if(nbArrows < NUMBER_OF_ARROWS)
rrknight 1:95d85c81bb11 162 {
rrknight 1:95d85c81bb11 163 if(rByte == CLOSE_ARROW)
rrknight 1:95d85c81bb11 164 {
rrknight 1:95d85c81bb11 165 nbArrows++;
rrknight 1:95d85c81bb11 166 }
rrknight 2:201618ffa295 167
rrknight 1:95d85c81bb11 168 if((nbArrows > 0) && (rByte != CLOSE_ARROW))
rrknight 1:95d85c81bb11 169 {
rrknight 1:95d85c81bb11 170 nbArrows = 0; //reset in case the previous arrows was data.
rrknight 1:95d85c81bb11 171 }
rrknight 1:95d85c81bb11 172
rrknight 1:95d85c81bb11 173 if(nbArrows == NUMBER_OF_ARROWS)
rrknight 1:95d85c81bb11 174 {
rrknight 1:95d85c81bb11 175 fiveArrowsFound = true;
rrknight 1:95d85c81bb11 176 }
rrknight 0:369222671f3c 177 }
rrknight 1:95d85c81bb11 178 }
rrknight 1:95d85c81bb11 179 else //fiveArrowsFound, so rByte is the checksum
rrknight 1:95d85c81bb11 180 {
rrknight 1:95d85c81bb11 181 move(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG);
rrknight 1:95d85c81bb11 182 //pc.printf("r cs 0x%02X\n", rByte);
rrknight 1:95d85c81bb11 183 //pc.printf("move %02X %02X %02X %02X %02X %02X %02X \n", readBufferSerial[0], readBufferSerial[1], readBufferSerial[2], readBufferSerial[3], readBufferSerial[4], readBufferSerial[5], readBufferSerial[6]);
rrknight 0:369222671f3c 184
rrknight 4:7da0cd1fcb8a 185 cmdValid = verifyCmdChecksum(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, rByte);
rrknight 4:7da0cd1fcb8a 186 //if(cmdValid) pc.printf("cmdValid\n\r");
rrknight 1:95d85c81bb11 187
rrknight 1:95d85c81bb11 188 //reset
rrknight 1:95d85c81bb11 189 serial->rxBufferFlush();
rrknight 1:95d85c81bb11 190 nbArrows = 0;
rrknight 1:95d85c81bb11 191 fiveArrowsFound = false;
rrknight 1:95d85c81bb11 192 }
rrknight 1:95d85c81bb11 193
rrknight 4:7da0cd1fcb8a 194 //logicPin = 0;
rrknight 0:369222671f3c 195 }
rrknight 0:369222671f3c 196
rrknight 0:369222671f3c 197 int main()
rrknight 0:369222671f3c 198 {
rrknight 2:201618ffa295 199 //Deselect all mbed slaves
rrknight 2:201618ffa295 200 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 2:201618ffa295 201 {
rrknight 2:201618ffa295 202 cs[k] = 1;
rrknight 2:201618ffa295 203 sync_slave[k] = 0;
rrknight 2:201618ffa295 204 }
rrknight 2:201618ffa295 205
rrknight 0:369222671f3c 206 ros.baud(460800); //460800 works //921600 don't
rrknight 0:369222671f3c 207 pc.baud(115200);
rrknight 0:369222671f3c 208
rrknight 2:201618ffa295 209 // Setup the spi for 8 bit data, high steady state clock,
rrknight 2:201618ffa295 210 // second edge capture, with a 1MHz clock rate
rrknight 2:201618ffa295 211 spi.format(8, 0); //spi.format(8,3);
rrknight 2:201618ffa295 212 spi.frequency(1000000); //32000000
rrknight 2:201618ffa295 213
rrknight 2:201618ffa295 214 //init the SPI arrays
rrknight 2:201618ffa295 215 for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
rrknight 2:201618ffa295 216 {
rrknight 3:af892e4bf53e 217 for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
rrknight 2:201618ffa295 218 {
rrknight 3:af892e4bf53e 219 for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
rrknight 2:201618ffa295 220 {
rrknight 2:201618ffa295 221 writeBufferSPI[i][j][k] = 0x00;
rrknight 2:201618ffa295 222 readBufferSPI[i][j][k] = 0x00;
rrknight 2:201618ffa295 223 }
rrknight 2:201618ffa295 224 }
rrknight 3:af892e4bf53e 225
rrknight 3:af892e4bf53e 226 writeChecksum[i] = 0x00;
rrknight 2:201618ffa295 227 }
rrknight 0:369222671f3c 228
rrknight 0:369222671f3c 229 //init alloc
rrknight 0:369222671f3c 230 readBufferSerial = (char*)malloc(NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG*sizeof(char*));
rrknight 0:369222671f3c 231
rrknight 2:201618ffa295 232 //uint8_t my_val;
rrknight 2:201618ffa295 233
rrknight 0:369222671f3c 234 ros.attach(&rxCallback, MODSERIAL::RxIrq);
rrknight 0:369222671f3c 235
rrknight 0:369222671f3c 236 pc.printf("*** Start Master Main ***\n\r");
rrknight 0:369222671f3c 237
rrknight 0:369222671f3c 238 logicPin = 0;
rrknight 0:369222671f3c 239
rrknight 2:201618ffa295 240 // Wait here until we detect a valid message in the serial RX buffer.
rrknight 1:95d85c81bb11 241 while(1)
rrknight 1:95d85c81bb11 242 {
rrknight 4:7da0cd1fcb8a 243 if(cmdValid) //pass it to the SPI bus
rrknight 4:7da0cd1fcb8a 244 {
rrknight 4:7da0cd1fcb8a 245 logicPin = 1;
rrknight 4:7da0cd1fcb8a 246
rrknight 2:201618ffa295 247 //init the SPI arrays
rrknight 2:201618ffa295 248 for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
rrknight 2:201618ffa295 249 {
rrknight 3:af892e4bf53e 250 for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
rrknight 2:201618ffa295 251 {
rrknight 3:af892e4bf53e 252 for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
rrknight 2:201618ffa295 253 {
rrknight 2:201618ffa295 254 writeBufferSPI[i][j][k] = 0x00; //mode 0 for null command
rrknight 2:201618ffa295 255 readBufferSPI[i][j][k] = 0x00;
rrknight 2:201618ffa295 256 }
rrknight 2:201618ffa295 257 }
rrknight 3:af892e4bf53e 258
rrknight 3:af892e4bf53e 259 writeChecksum[i] = 0x00;
rrknight 2:201618ffa295 260 }
rrknight 4:7da0cd1fcb8a 261 /*
rrknight 4:7da0cd1fcb8a 262 //init nb cmds per slave //useless ?
rrknight 2:201618ffa295 263 for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
rrknight 2:201618ffa295 264 {
rrknight 2:201618ffa295 265 numberCmds[i] = 0;
rrknight 2:201618ffa295 266 }
rrknight 4:7da0cd1fcb8a 267 */
rrknight 4:7da0cd1fcb8a 268 logicPin = 0;
rrknight 2:201618ffa295 269
rrknight 2:201618ffa295 270 //sort messages for each slave
rrknight 2:201618ffa295 271 for(int i=0; i<NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG; i+=NUMBER_BYTES_PER_MSG)
rrknight 2:201618ffa295 272 {
rrknight 2:201618ffa295 273 uint8_t nodeID = readBufferSerial[i];
rrknight 2:201618ffa295 274
rrknight 2:201618ffa295 275 if(nodeID>=FIRST_NODE_ID_SLAVE_1 && nodeID<FIRST_NODE_ID_SLAVE_2) //slave 1
rrknight 2:201618ffa295 276 {
rrknight 2:201618ffa295 277 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:201618ffa295 278 {
rrknight 3:af892e4bf53e 279 writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
rrknight 4:7da0cd1fcb8a 280 }
rrknight 4:7da0cd1fcb8a 281 //numberCmds[0]++;
rrknight 2:201618ffa295 282 }
rrknight 2:201618ffa295 283 else if(nodeID>=FIRST_NODE_ID_SLAVE_2 && nodeID<FIRST_NODE_ID_SLAVE_3) //slave 2
rrknight 2:201618ffa295 284 {
rrknight 2:201618ffa295 285 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:201618ffa295 286 {
rrknight 4:7da0cd1fcb8a 287 writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG-NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
rrknight 2:201618ffa295 288 }
rrknight 4:7da0cd1fcb8a 289
rrknight 4:7da0cd1fcb8a 290 //change nodeID between 1 and 15
rrknight 4:7da0cd1fcb8a 291 writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG-NUMBER_MAX_EPOS2_PER_SLAVE][0] -= NUMBER_MAX_EPOS2_PER_SLAVE; //substract a multiple of 15, example : nodeID 16 will be nodeID 1 for slave nb 2
rrknight 4:7da0cd1fcb8a 292
rrknight 4:7da0cd1fcb8a 293 //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
rrknight 4:7da0cd1fcb8a 294 //numberCmds[1]++;
rrknight 4:7da0cd1fcb8a 295 //pc.printf("ID[%d] %d\n", i/NUMBER_BYTES_PER_MSG, writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG][0]);
rrknight 2:201618ffa295 296 }
rrknight 2:201618ffa295 297 else if(nodeID>=FIRST_NODE_ID_SLAVE_3) //slave 3
rrknight 2:201618ffa295 298 {
rrknight 2:201618ffa295 299 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:201618ffa295 300 {
rrknight 4:7da0cd1fcb8a 301 writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG-2*NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
rrknight 2:201618ffa295 302 }
rrknight 4:7da0cd1fcb8a 303
rrknight 4:7da0cd1fcb8a 304 //change nodeID between 1 and 15
rrknight 5:c73f87d22142 305 writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG-2*NUMBER_MAX_EPOS2_PER_SLAVE][0] -= 2*NUMBER_MAX_EPOS2_PER_SLAVE; //substract a multiple of 15, example : nodeID 16 will be nodeID 1 for slave nb 2
rrknight 4:7da0cd1fcb8a 306 //numberCmds[2]++;
rrknight 4:7da0cd1fcb8a 307 }
rrknight 4:7da0cd1fcb8a 308 //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
rrknight 2:201618ffa295 309 }
rrknight 2:201618ffa295 310
rrknight 4:7da0cd1fcb8a 311 //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
rrknight 4:7da0cd1fcb8a 312
rrknight 3:af892e4bf53e 313 //add dummy bytes
rrknight 3:af892e4bf53e 314 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 3:af892e4bf53e 315 {
rrknight 3:af892e4bf53e 316 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 3:af892e4bf53e 317 {
rrknight 3:af892e4bf53e 318 for(int j=NUMBER_BYTES_PER_MSG; j<NUMBER_BYTES_TO_READ; j++)
rrknight 3:af892e4bf53e 319 {
rrknight 3:af892e4bf53e 320 writeBufferSPI[k][i][j] = DUMMY_BYTE;
rrknight 3:af892e4bf53e 321 }
rrknight 3:af892e4bf53e 322 }
rrknight 3:af892e4bf53e 323 }
rrknight 3:af892e4bf53e 324
rrknight 3:af892e4bf53e 325 //now all individual SPI buffers for slaves have been created
rrknight 3:af892e4bf53e 326 //compute checksum for each slave and update the variable, it'll be sent later at the end of SPI writting
rrknight 3:af892e4bf53e 327 calculateSPIChecksum(); //this update the writeChecksum[k]
rrknight 3:af892e4bf53e 328
rrknight 3:af892e4bf53e 329 //pc.printf("nbCmd %d %d %d\n", numberCmds[0], numberCmds[1], numberCmds[2]);
rrknight 2:201618ffa295 330 //pc.printf("1st Cmd %02X %02X %02X %02X %02X %02X %02X\n", writeBufferSPI[0][0][0], writeBufferSPI[0][0][1], writeBufferSPI[0][0][2], writeBufferSPI[0][0][3], writeBufferSPI[0][0][4], writeBufferSPI[0][0][5], writeBufferSPI[0][0][6]);
rrknight 2:201618ffa295 331
rrknight 3:af892e4bf53e 332 //new commands has been grabbed and are ready to be sent to slaves
rrknight 2:201618ffa295 333 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++) //NUMBER_SLAVE_BOARDS for each slave
rrknight 2:201618ffa295 334 {
rrknight 4:7da0cd1fcb8a 335 if(k<3) ledchain[k] = 1; //switch on LED nb k
rrknight 4:7da0cd1fcb8a 336
rrknight 2:201618ffa295 337 sync_slave[k] = 1;
rrknight 4:7da0cd1fcb8a 338 wait_us(10); //pause so the slave can see it's been selected
rrknight 2:201618ffa295 339 sync_slave[k] = 0;
rrknight 2:201618ffa295 340
rrknight 4:7da0cd1fcb8a 341 cs[k] = 0;
rrknight 2:201618ffa295 342 spi.write(OPEN_ARROW);
rrknight 2:201618ffa295 343 wait_us(5);
rrknight 2:201618ffa295 344 cs[k] = 1;
rrknight 4:7da0cd1fcb8a 345 wait_us(8);
rrknight 2:201618ffa295 346
rrknight 2:201618ffa295 347 cs[k] = 0;
rrknight 2:201618ffa295 348 spi.write(OPEN_ARROW);
rrknight 2:201618ffa295 349 wait_us(5);
rrknight 2:201618ffa295 350 cs[k] = 1;
rrknight 4:7da0cd1fcb8a 351 wait_us(8);
rrknight 2:201618ffa295 352
rrknight 4:7da0cd1fcb8a 353 cs[k] = 0;
rrknight 2:201618ffa295 354 spi.write(OPEN_ARROW);
rrknight 2:201618ffa295 355 wait_us(5);
rrknight 2:201618ffa295 356 cs[k] = 1;
rrknight 4:7da0cd1fcb8a 357 wait_us(8);
rrknight 2:201618ffa295 358
rrknight 3:af892e4bf53e 359 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 4:7da0cd1fcb8a 360 {
rrknight 4:7da0cd1fcb8a 361 //writeBufferSPI[k][i][0] = writeBufferSPI[k][i][0] - 0x0F;
rrknight 4:7da0cd1fcb8a 362
rrknight 4:7da0cd1fcb8a 363 for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
rrknight 2:201618ffa295 364 {
rrknight 2:201618ffa295 365 cs[k] = 0;
rrknight 2:201618ffa295 366 readBufferSPI[k][i][j] = (char)(spi.write(writeBufferSPI[k][i][j]));
rrknight 2:201618ffa295 367 wait_us(5);
rrknight 2:201618ffa295 368 cs[k] = 1;
rrknight 4:7da0cd1fcb8a 369 wait_us(8);
rrknight 2:201618ffa295 370 }
rrknight 3:af892e4bf53e 371 }
rrknight 3:af892e4bf53e 372
rrknight 3:af892e4bf53e 373 //finally write the command checksum and read the data checksum at the same time
rrknight 3:af892e4bf53e 374 cs[k] = 0;
rrknight 3:af892e4bf53e 375 readChecksum[k] = (char)(spi.write(writeChecksum[k]));
rrknight 3:af892e4bf53e 376 wait_us(5);
rrknight 3:af892e4bf53e 377 cs[k] = 1;
rrknight 4:7da0cd1fcb8a 378 wait_us(8);
rrknight 4:7da0cd1fcb8a 379
rrknight 4:7da0cd1fcb8a 380 if(k<3) ledchain[k] = 0; //switch on LED nb k
rrknight 2:201618ffa295 381 }
rrknight 2:201618ffa295 382
rrknight 4:7da0cd1fcb8a 383 //pc.printf("nodeID 1 0x%02X 2 0x%02X 3 0x%02X\n", writeBufferSPI[0][0][0], writeBufferSPI[1][0][0], writeBufferSPI[2][0][0]);
rrknight 4:7da0cd1fcb8a 384 //pc.printf("nodeID 1-%d 2-%d 3-%d\n", writeBufferSPI[0][0][0], writeBufferSPI[1][0][0], writeBufferSPI[2][0][0]);
rrknight 4:7da0cd1fcb8a 385 //pc.printf("rc 0x%02X wc 0x%02X\n", readChecksum[0], writeChecksum[0]);
rrknight 5:c73f87d22142 386 //pc.printf("%02X\n", writeChecksum[2]);
rrknight 4:7da0cd1fcb8a 387 /*
rrknight 2:201618ffa295 388 logicPin = 0;
rrknight 2:201618ffa295 389 wait_us(10);
rrknight 2:201618ffa295 390 logicPin = 1;
rrknight 2:201618ffa295 391 wait_us(10);
rrknight 4:7da0cd1fcb8a 392 */
rrknight 4:7da0cd1fcb8a 393 //now check the validity of the data
rrknight 4:7da0cd1fcb8a 394 dataValid = verifyDataChecksum();
rrknight 4:7da0cd1fcb8a 395
rrknight 4:7da0cd1fcb8a 396 //Erase Tx serial buffer
rrknight 4:7da0cd1fcb8a 397 //ros.txBufferFlush();
rrknight 4:7da0cd1fcb8a 398
rrknight 4:7da0cd1fcb8a 399 //write the data msg on serial Tx
rrknight 4:7da0cd1fcb8a 400 //if(dataValid)
rrknight 4:7da0cd1fcb8a 401 //{
rrknight 4:7da0cd1fcb8a 402 logicPin = 1;
rrknight 4:7da0cd1fcb8a 403
rrknight 4:7da0cd1fcb8a 404 //compute checksum for all data from slaves
rrknight 4:7da0cd1fcb8a 405 calculateTxChecksum();
rrknight 4:7da0cd1fcb8a 406
rrknight 4:7da0cd1fcb8a 407 //write data and checksum on Tx
rrknight 4:7da0cd1fcb8a 408 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 4:7da0cd1fcb8a 409 {
rrknight 4:7da0cd1fcb8a 410 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 4:7da0cd1fcb8a 411 {
rrknight 4:7da0cd1fcb8a 412 for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
rrknight 4:7da0cd1fcb8a 413 {
rrknight 4:7da0cd1fcb8a 414 ros.putc(readBufferSPI[k][i][j]);
rrknight 4:7da0cd1fcb8a 415 }
rrknight 4:7da0cd1fcb8a 416 }
rrknight 4:7da0cd1fcb8a 417 }
rrknight 4:7da0cd1fcb8a 418
rrknight 4:7da0cd1fcb8a 419 ros.putc(serialTxChecksum);
rrknight 4:7da0cd1fcb8a 420
rrknight 4:7da0cd1fcb8a 421 dataValid = false; //toggle flag for next message
rrknight 4:7da0cd1fcb8a 422
rrknight 4:7da0cd1fcb8a 423 logicPin = 0;
rrknight 4:7da0cd1fcb8a 424 //}
rrknight 4:7da0cd1fcb8a 425
rrknight 2:201618ffa295 426 //print the array :
rrknight 2:201618ffa295 427 /*
rrknight 2:201618ffa295 428 for(int i=0; i<2; i++)
rrknight 2:201618ffa295 429 {
rrknight 2:201618ffa295 430 pc.printf("%02X %02X %02X %02X %02X %02X %02X\n\r", readBufferSPI[0][i][0], readBufferSPI[0][i][1], readBufferSPI[0][i][2], readBufferSPI[0][i][3], readBufferSPI[0][i][4], readBufferSPI[0][i][5], readBufferSPI[0][i][6]);
rrknight 2:201618ffa295 431 }
rrknight 2:201618ffa295 432 */
rrknight 2:201618ffa295 433 /* int i=0;
rrknight 2:201618ffa295 434 pc.printf("%02X %02X %02X %02X %02X %02X %02X\n", readBufferSPI[0][i][0], readBufferSPI[0][i][1], readBufferSPI[0][i][2], readBufferSPI[0][i][3], readBufferSPI[0][i][4], readBufferSPI[0][i][5], readBufferSPI[0][i][6]);
rrknight 2:201618ffa295 435 i=14;
rrknight 2:201618ffa295 436 pc.printf("%02X %02X %02X %02X %02X %02X %02X\n\r", readBufferSPI[0][i][0], readBufferSPI[0][i][1], readBufferSPI[0][i][2], readBufferSPI[0][i][3], readBufferSPI[0][i][4], readBufferSPI[0][i][5], readBufferSPI[0][i][6]);
rrknight 2:201618ffa295 437
rrknight 2:201618ffa295 438 //pc.printf("\n\r");
rrknight 2:201618ffa295 439 logicPin = 0;
rrknight 2:201618ffa295 440
rrknight 2:201618ffa295 441 //build the motorDataSet_msg
rrknight 2:201618ffa295 442 for(int i=0; i<13; i++)
rrknight 2:201618ffa295 443 {
rrknight 2:201618ffa295 444 motorDataSet_msg.motorData[i].encPosition = 100*i;
rrknight 2:201618ffa295 445 motorDataSet_msg.motorData[i].potiPosition = 10*i;
rrknight 2:201618ffa295 446 motorDataSet_msg.motorData[i].current = -10*i;
rrknight 2:201618ffa295 447 motorDataSet_msg.motorData[i].force = 2*i;
rrknight 2:201618ffa295 448 }
rrknight 2:201618ffa295 449 */
rrknight 4:7da0cd1fcb8a 450 //logicPin = 0;
rrknight 2:201618ffa295 451
rrknight 4:7da0cd1fcb8a 452 cmdValid = false; //toggle flag for next message
rrknight 2:201618ffa295 453
rrknight 1:95d85c81bb11 454 }
rrknight 1:95d85c81bb11 455
rrknight 2:201618ffa295 456 wait_us(10);
rrknight 2:201618ffa295 457 }
rrknight 0:369222671f3c 458 }