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@3:af892e4bf53e, 2013-03-06 (annotated)

Committer:
rrknight
Date:
Wed Mar 06 17:07:36 2013 +0000
Revision:
3:af892e4bf53e
Parent:
2:201618ffa295
Child:
4:7da0cd1fcb8a
hannover work in progress

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 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 1:95d85c81bb11 53
rrknight 1:95d85c81bb11 54 bool fiveArrowsFound = false;
rrknight 1:95d85c81bb11 55 bool msgValid = false;
rrknight 1:95d85c81bb11 56
rrknight 1:95d85c81bb11 57 int move(char *s, int nbBytes) //custom move function (cp from MODESERIAL without the end character)
rrknight 1:95d85c81bb11 58 {
rrknight 1:95d85c81bb11 59 int counter = 0;
rrknight 1:95d85c81bb11 60 char c;
rrknight 2:201618ffa295 61
rrknight 2:201618ffa295 62 while(ros.readable())
rrknight 2:201618ffa295 63 {
rrknight 1:95d85c81bb11 64 c = ros.getc();
rrknight 1:95d85c81bb11 65 *(s++) = c;
rrknight 1:95d85c81bb11 66 counter++;
rrknight 2:201618ffa295 67 if(counter == nbBytes) break;
rrknight 1:95d85c81bb11 68 }
rrknight 2:201618ffa295 69
rrknight 1:95d85c81bb11 70 return counter;
rrknight 1:95d85c81bb11 71 }
rrknight 1:95d85c81bb11 72
rrknight 1:95d85c81bb11 73 bool verifyChecksum(char* data, int length, char checksum)
rrknight 1:95d85c81bb11 74 {
rrknight 1:95d85c81bb11 75 for(int i=0; i<length; i++)
rrknight 1:95d85c81bb11 76 {
rrknight 1:95d85c81bb11 77 checksum += data[i];
rrknight 1:95d85c81bb11 78 }
rrknight 1:95d85c81bb11 79
rrknight 1:95d85c81bb11 80 checksum++; //add 1 to obtain 0x00
rrknight 1:95d85c81bb11 81
rrknight 1:95d85c81bb11 82 if(checksum == 0x00) return true;
rrknight 1:95d85c81bb11 83 else return false;
rrknight 1:95d85c81bb11 84 }
rrknight 0:369222671f3c 85
rrknight 3:af892e4bf53e 86 void calculateSPIChecksum()
rrknight 3:af892e4bf53e 87 {
rrknight 3:af892e4bf53e 88 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 3:af892e4bf53e 89 {
rrknight 3:af892e4bf53e 90 int sum = 0;
rrknight 3:af892e4bf53e 91
rrknight 3:af892e4bf53e 92 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 3:af892e4bf53e 93 {
rrknight 3:af892e4bf53e 94 for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
rrknight 3:af892e4bf53e 95 {
rrknight 3:af892e4bf53e 96 writeChecksum[k] += writeBufferSPI[k][i][j];
rrknight 3:af892e4bf53e 97 }
rrknight 3:af892e4bf53e 98 }
rrknight 3:af892e4bf53e 99
rrknight 3:af892e4bf53e 100 writeChecksum[k] = (char)(~sum);
rrknight 3:af892e4bf53e 101 }
rrknight 3:af892e4bf53e 102 }
rrknight 3:af892e4bf53e 103
rrknight 0:369222671f3c 104 // Called everytime a new character goes into
rrknight 0:369222671f3c 105 // the RX buffer. Test that character for '/'
rrknight 0:369222671f3c 106 // Note, rxGetLastChar() gets the last char that
rrknight 0:369222671f3c 107 // we received but it does NOT remove it from
rrknight 0:369222671f3c 108 // the RX buffer.
rrknight 0:369222671f3c 109 void rxCallback(MODSERIAL_IRQ_INFO *q)
rrknight 0:369222671f3c 110 {
rrknight 0:369222671f3c 111 logicPin = 1;
rrknight 0:369222671f3c 112
rrknight 2:201618ffa295 113 MODSERIAL *serial = q->serial;
rrknight 0:369222671f3c 114 rByte = serial->rxGetLastChar();
rrknight 0:369222671f3c 115
rrknight 1:95d85c81bb11 116 if(!fiveArrowsFound)
rrknight 0:369222671f3c 117 {
rrknight 1:95d85c81bb11 118 if(nbArrows < NUMBER_OF_ARROWS)
rrknight 1:95d85c81bb11 119 {
rrknight 1:95d85c81bb11 120 if(rByte == CLOSE_ARROW)
rrknight 1:95d85c81bb11 121 {
rrknight 1:95d85c81bb11 122 nbArrows++;
rrknight 1:95d85c81bb11 123 }
rrknight 2:201618ffa295 124
rrknight 1:95d85c81bb11 125 if((nbArrows > 0) && (rByte != CLOSE_ARROW))
rrknight 1:95d85c81bb11 126 {
rrknight 1:95d85c81bb11 127 nbArrows = 0; //reset in case the previous arrows was data.
rrknight 1:95d85c81bb11 128 }
rrknight 1:95d85c81bb11 129
rrknight 1:95d85c81bb11 130 if(nbArrows == NUMBER_OF_ARROWS)
rrknight 1:95d85c81bb11 131 {
rrknight 1:95d85c81bb11 132 fiveArrowsFound = true;
rrknight 1:95d85c81bb11 133 }
rrknight 0:369222671f3c 134 }
rrknight 1:95d85c81bb11 135 }
rrknight 1:95d85c81bb11 136 else //fiveArrowsFound, so rByte is the checksum
rrknight 1:95d85c81bb11 137 {
rrknight 1:95d85c81bb11 138 move(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG);
rrknight 1:95d85c81bb11 139 //pc.printf("r cs 0x%02X\n", rByte);
rrknight 1:95d85c81bb11 140 //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 141
rrknight 2:201618ffa295 142 msgValid = verifyChecksum(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, rByte);
rrknight 3:af892e4bf53e 143 //if(msgValid) pc.printf("msgValid\n\r");
rrknight 1:95d85c81bb11 144
rrknight 1:95d85c81bb11 145 //reset
rrknight 1:95d85c81bb11 146 serial->rxBufferFlush();
rrknight 1:95d85c81bb11 147 nbArrows = 0;
rrknight 1:95d85c81bb11 148 fiveArrowsFound = false;
rrknight 1:95d85c81bb11 149 }
rrknight 1:95d85c81bb11 150
rrknight 0:369222671f3c 151 logicPin = 0;
rrknight 0:369222671f3c 152 }
rrknight 0:369222671f3c 153
rrknight 0:369222671f3c 154 int main()
rrknight 0:369222671f3c 155 {
rrknight 2:201618ffa295 156 //Deselect all mbed slaves
rrknight 2:201618ffa295 157 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 2:201618ffa295 158 {
rrknight 2:201618ffa295 159 cs[k] = 1;
rrknight 2:201618ffa295 160 sync_slave[k] = 0;
rrknight 2:201618ffa295 161 }
rrknight 2:201618ffa295 162
rrknight 0:369222671f3c 163 ros.baud(460800); //460800 works //921600 don't
rrknight 0:369222671f3c 164 pc.baud(115200);
rrknight 0:369222671f3c 165
rrknight 2:201618ffa295 166 // Setup the spi for 8 bit data, high steady state clock,
rrknight 2:201618ffa295 167 // second edge capture, with a 1MHz clock rate
rrknight 2:201618ffa295 168 spi.format(8, 0); //spi.format(8,3);
rrknight 2:201618ffa295 169 spi.frequency(1000000); //32000000
rrknight 2:201618ffa295 170
rrknight 2:201618ffa295 171 //init the SPI arrays
rrknight 2:201618ffa295 172 for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
rrknight 2:201618ffa295 173 {
rrknight 3:af892e4bf53e 174 for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
rrknight 2:201618ffa295 175 {
rrknight 3:af892e4bf53e 176 for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
rrknight 2:201618ffa295 177 {
rrknight 2:201618ffa295 178 writeBufferSPI[i][j][k] = 0x00;
rrknight 2:201618ffa295 179 readBufferSPI[i][j][k] = 0x00;
rrknight 2:201618ffa295 180 }
rrknight 2:201618ffa295 181 }
rrknight 3:af892e4bf53e 182
rrknight 3:af892e4bf53e 183 writeChecksum[i] = 0x00;
rrknight 2:201618ffa295 184 }
rrknight 0:369222671f3c 185
rrknight 0:369222671f3c 186 //init alloc
rrknight 0:369222671f3c 187 readBufferSerial = (char*)malloc(NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG*sizeof(char*));
rrknight 0:369222671f3c 188
rrknight 2:201618ffa295 189 //uint8_t my_val;
rrknight 2:201618ffa295 190
rrknight 0:369222671f3c 191 ros.attach(&rxCallback, MODSERIAL::RxIrq);
rrknight 0:369222671f3c 192
rrknight 0:369222671f3c 193 pc.printf("*** Start Master Main ***\n\r");
rrknight 0:369222671f3c 194
rrknight 0:369222671f3c 195 logicPin = 0;
rrknight 0:369222671f3c 196
rrknight 2:201618ffa295 197 // Wait here until we detect a valid message in the serial RX buffer.
rrknight 1:95d85c81bb11 198 while(1)
rrknight 1:95d85c81bb11 199 {
rrknight 2:201618ffa295 200 if(msgValid) //pass it to the SPI bus
rrknight 2:201618ffa295 201 {
rrknight 2:201618ffa295 202 //init the SPI arrays
rrknight 2:201618ffa295 203 for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
rrknight 2:201618ffa295 204 {
rrknight 3:af892e4bf53e 205 for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
rrknight 2:201618ffa295 206 {
rrknight 3:af892e4bf53e 207 for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
rrknight 2:201618ffa295 208 {
rrknight 2:201618ffa295 209 writeBufferSPI[i][j][k] = 0x00; //mode 0 for null command
rrknight 2:201618ffa295 210 readBufferSPI[i][j][k] = 0x00;
rrknight 2:201618ffa295 211 }
rrknight 2:201618ffa295 212 }
rrknight 3:af892e4bf53e 213
rrknight 3:af892e4bf53e 214 writeChecksum[i] = 0x00;
rrknight 2:201618ffa295 215 }
rrknight 2:201618ffa295 216
rrknight 2:201618ffa295 217 //init nb cmds per slave
rrknight 2:201618ffa295 218 for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
rrknight 2:201618ffa295 219 {
rrknight 2:201618ffa295 220 numberCmds[i] = 0;
rrknight 2:201618ffa295 221 }
rrknight 2:201618ffa295 222
rrknight 2:201618ffa295 223
rrknight 2:201618ffa295 224 //sort messages for each slave
rrknight 2:201618ffa295 225 for(int i=0; i<NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG; i+=NUMBER_BYTES_PER_MSG)
rrknight 2:201618ffa295 226 {
rrknight 2:201618ffa295 227 uint8_t nodeID = readBufferSerial[i];
rrknight 2:201618ffa295 228
rrknight 2:201618ffa295 229 if(nodeID>=FIRST_NODE_ID_SLAVE_1 && nodeID<FIRST_NODE_ID_SLAVE_2) //slave 1
rrknight 2:201618ffa295 230 {
rrknight 2:201618ffa295 231 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:201618ffa295 232 {
rrknight 3:af892e4bf53e 233 writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
rrknight 2:201618ffa295 234 }
rrknight 3:af892e4bf53e 235
rrknight 2:201618ffa295 236 numberCmds[0]++;
rrknight 2:201618ffa295 237 }
rrknight 2:201618ffa295 238 else if(nodeID>=FIRST_NODE_ID_SLAVE_2 && nodeID<FIRST_NODE_ID_SLAVE_3) //slave 2
rrknight 2:201618ffa295 239 {
rrknight 2:201618ffa295 240 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:201618ffa295 241 {
rrknight 3:af892e4bf53e 242 writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
rrknight 2:201618ffa295 243 }
rrknight 2:201618ffa295 244 numberCmds[1]++;
rrknight 2:201618ffa295 245 }
rrknight 2:201618ffa295 246 else if(nodeID>=FIRST_NODE_ID_SLAVE_3) //slave 3
rrknight 2:201618ffa295 247 {
rrknight 2:201618ffa295 248 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:201618ffa295 249 {
rrknight 3:af892e4bf53e 250 writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
rrknight 2:201618ffa295 251 }
rrknight 2:201618ffa295 252 numberCmds[2]++;
rrknight 2:201618ffa295 253 }
rrknight 2:201618ffa295 254 }
rrknight 2:201618ffa295 255
rrknight 3:af892e4bf53e 256 //add dummy bytes
rrknight 3:af892e4bf53e 257 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
rrknight 3:af892e4bf53e 258 {
rrknight 3:af892e4bf53e 259 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 3:af892e4bf53e 260 {
rrknight 3:af892e4bf53e 261 for(int j=NUMBER_BYTES_PER_MSG; j<NUMBER_BYTES_TO_READ; j++)
rrknight 3:af892e4bf53e 262 {
rrknight 3:af892e4bf53e 263 writeBufferSPI[k][i][j] = DUMMY_BYTE;
rrknight 3:af892e4bf53e 264 }
rrknight 3:af892e4bf53e 265 }
rrknight 3:af892e4bf53e 266 }
rrknight 3:af892e4bf53e 267
rrknight 3:af892e4bf53e 268 //now all individual SPI buffers for slaves have been created
rrknight 3:af892e4bf53e 269 //compute checksum for each slave and update the variable, it'll be sent later at the end of SPI writting
rrknight 3:af892e4bf53e 270 calculateSPIChecksum(); //this update the writeChecksum[k]
rrknight 3:af892e4bf53e 271
rrknight 3:af892e4bf53e 272 //pc.printf("nbCmd %d %d %d\n", numberCmds[0], numberCmds[1], numberCmds[2]);
rrknight 2:201618ffa295 273 //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 274
rrknight 3:af892e4bf53e 275 //new commands has been grabbed and are ready to be sent to slaves
rrknight 2:201618ffa295 276 for(int k=0; k<NUMBER_SLAVE_BOARDS; k++) //NUMBER_SLAVE_BOARDS for each slave
rrknight 2:201618ffa295 277 {
rrknight 2:201618ffa295 278 sync_slave[k] = 1;
rrknight 2:201618ffa295 279 wait_us(10); //pause so the slave can see it's been selected //TODO array with the other slaves
rrknight 2:201618ffa295 280 sync_slave[k] = 0;
rrknight 2:201618ffa295 281
rrknight 2:201618ffa295 282 cs[k] = 0;
rrknight 2:201618ffa295 283 //while (!(LPC_SSP1->SR & TNF));
rrknight 2:201618ffa295 284 spi.write(OPEN_ARROW);
rrknight 2:201618ffa295 285 //while (!(LPC_SSP1->SR & TFE));
rrknight 2:201618ffa295 286 wait_us(5);
rrknight 2:201618ffa295 287 cs[k] = 1;
rrknight 2:201618ffa295 288 wait_us(10); //wait_us(10);
rrknight 2:201618ffa295 289
rrknight 2:201618ffa295 290 cs[k] = 0;
rrknight 2:201618ffa295 291 //while (!(LPC_SSP1->SR & TNF));
rrknight 2:201618ffa295 292 spi.write(OPEN_ARROW);
rrknight 2:201618ffa295 293 // while (!(LPC_SSP1->SR & TFE));
rrknight 2:201618ffa295 294 wait_us(5);
rrknight 2:201618ffa295 295 cs[k] = 1;
rrknight 2:201618ffa295 296 wait_us(10);
rrknight 2:201618ffa295 297
rrknight 2:201618ffa295 298 cs[k] = 0;
rrknight 2:201618ffa295 299 //while (!(LPC_SSP1->SR & TNF));
rrknight 2:201618ffa295 300 spi.write(OPEN_ARROW);
rrknight 2:201618ffa295 301 //while (!(LPC_SSP1->SR & TFE)); //MAYBE REMOVE COMMENT ?
rrknight 2:201618ffa295 302 wait_us(5);
rrknight 2:201618ffa295 303 cs[k] = 1;
rrknight 2:201618ffa295 304 wait_us(10);
rrknight 2:201618ffa295 305
rrknight 3:af892e4bf53e 306 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 2:201618ffa295 307 {
rrknight 2:201618ffa295 308 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:201618ffa295 309 {
rrknight 2:201618ffa295 310 cs[k] = 0;
rrknight 2:201618ffa295 311 readBufferSPI[k][i][j] = (char)(spi.write(writeBufferSPI[k][i][j]));
rrknight 2:201618ffa295 312 wait_us(5);
rrknight 2:201618ffa295 313 cs[k] = 1;
rrknight 2:201618ffa295 314 wait_us(10);
rrknight 2:201618ffa295 315 }
rrknight 3:af892e4bf53e 316 }
rrknight 3:af892e4bf53e 317
rrknight 3:af892e4bf53e 318 //finally write the command checksum and read the data checksum at the same time
rrknight 3:af892e4bf53e 319 cs[k] = 0;
rrknight 3:af892e4bf53e 320 readChecksum[k] = (char)(spi.write(writeChecksum[k]));
rrknight 3:af892e4bf53e 321 wait_us(5);
rrknight 3:af892e4bf53e 322 cs[k] = 1;
rrknight 3:af892e4bf53e 323 wait_us(10);
rrknight 2:201618ffa295 324 }
rrknight 2:201618ffa295 325
rrknight 2:201618ffa295 326 logicPin = 0;
rrknight 2:201618ffa295 327 wait_us(10);
rrknight 2:201618ffa295 328 logicPin = 1;
rrknight 2:201618ffa295 329 wait_us(10);
rrknight 2:201618ffa295 330 //print the array :
rrknight 2:201618ffa295 331 /*
rrknight 2:201618ffa295 332 for(int i=0; i<2; i++)
rrknight 2:201618ffa295 333 {
rrknight 2:201618ffa295 334 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 335 }
rrknight 2:201618ffa295 336 */
rrknight 2:201618ffa295 337 /* int i=0;
rrknight 2:201618ffa295 338 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 339 i=14;
rrknight 2:201618ffa295 340 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 341
rrknight 2:201618ffa295 342 //pc.printf("\n\r");
rrknight 2:201618ffa295 343 logicPin = 0;
rrknight 2:201618ffa295 344
rrknight 2:201618ffa295 345 //build the motorDataSet_msg
rrknight 2:201618ffa295 346 for(int i=0; i<13; i++)
rrknight 2:201618ffa295 347 {
rrknight 2:201618ffa295 348 motorDataSet_msg.motorData[i].encPosition = 100*i;
rrknight 2:201618ffa295 349 motorDataSet_msg.motorData[i].potiPosition = 10*i;
rrknight 2:201618ffa295 350 motorDataSet_msg.motorData[i].current = -10*i;
rrknight 2:201618ffa295 351 motorDataSet_msg.motorData[i].force = 2*i;
rrknight 2:201618ffa295 352 }
rrknight 2:201618ffa295 353 */
rrknight 2:201618ffa295 354 //TODO write the data msg on serial Tx
rrknight 2:201618ffa295 355 logicPin = 0;
rrknight 2:201618ffa295 356
rrknight 2:201618ffa295 357 msgValid = false; //toggle flag for next message
rrknight 2:201618ffa295 358
rrknight 1:95d85c81bb11 359 }
rrknight 1:95d85c81bb11 360
rrknight 2:201618ffa295 361 wait_us(10);
rrknight 2:201618ffa295 362 }
rrknight 0:369222671f3c 363 }