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@8:727350386f46, 2015-02-11 (annotated)

Committer:
therobotstudio
Date:
Wed Feb 11 16:30:54 2015 +0000
Revision:
8:727350386f46
Parent:
7:7ef85554cec5 
Code for the mbed NXP LPC1768.; To be used on The Robot Studio Master Boards.; License : Simplified BSD.

Who changed what in which revision?

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