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