TheRobotStudio ROSA
/
trs_master
Code for the mbed NXP LPC1768. To be used on The Robot Studio Master Boards. License : Simplified BSD.
main.cpp@2:201618ffa295, 2013-03-06 (annotated)
- Committer:
- rrknight
- Date:
- Wed Mar 06 14:33:01 2013 +0000
- Revision:
- 2:201618ffa295
- Parent:
- 1:95d85c81bb11
- Child:
- 3:af892e4bf53e
Working for 15msg packets
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 | 0:369222671f3c | 9 | #define NUMBER_MSG_PER_PACKET 15 |
rrknight | 0:369222671f3c | 10 | #define NUMBER_BYTES_PER_MSG 7 |
rrknight | 0:369222671f3c | 11 | #define NUMBER_SLAVE_BOARDS 3 |
rrknight | 2:201618ffa295 | 12 | #define FIRST_NODE_ID_SLAVE_1 1 |
rrknight | 2:201618ffa295 | 13 | #define FIRST_NODE_ID_SLAVE_2 FIRST_NODE_ID_SLAVE_1 + NUMBER_MAX_EPOS2_PER_SLAVE |
rrknight | 2:201618ffa295 | 14 | #define FIRST_NODE_ID_SLAVE_3 FIRST_NODE_ID_SLAVE_2 + NUMBER_MAX_EPOS2_PER_SLAVE |
rrknight | 0:369222671f3c | 15 | #define EPOS2_OK 0 |
rrknight | 0:369222671f3c | 16 | #define EPOS2_ERROR -1 |
rrknight | 0:369222671f3c | 17 | #define LOOP_PERIOD_TIME 20000 //20 ms |
rrknight | 0:369222671f3c | 18 | |
rrknight | 0:369222671f3c | 19 | //SPI RxTx FIFO bits |
rrknight | 0:369222671f3c | 20 | #define TNF 0x02 |
rrknight | 0:369222671f3c | 21 | #define TFE 0x01 |
rrknight | 0:369222671f3c | 22 | #define RNE 0x04 |
rrknight | 0:369222671f3c | 23 | |
rrknight | 2:201618ffa295 | 24 | #define OPEN_ARROW 0x3C //< = 60 |
rrknight | 1:95d85c81bb11 | 25 | #define CLOSE_ARROW 0x3E //< = 62 |
rrknight | 1:95d85c81bb11 | 26 | #define NUMBER_OF_ARROWS 5 |
rrknight | 1:95d85c81bb11 | 27 | |
rrknight | 0:369222671f3c | 28 | MODSERIAL ros(p28, p27, 1024, 512); // tx, rx |
rrknight | 0:369222671f3c | 29 | Serial pc(USBTX, USBRX); //terminal for debug |
rrknight | 0:369222671f3c | 30 | DigitalOut ledchain[] = {(LED1), (LED2), (LED3), (LED4)}; //used for debugging |
rrknight | 0:369222671f3c | 31 | DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high. |
rrknight | 0:369222671f3c | 32 | |
rrknight | 2:201618ffa295 | 33 | SPI spi(p5, p6, p7); // mosi, miso, sclk |
rrknight | 2:201618ffa295 | 34 | DigitalOut cs[NUMBER_SLAVE_BOARDS] = {(p8), (p9), (p10)}; //Slave Mbed number 1 //chip select |
rrknight | 2:201618ffa295 | 35 | DigitalOut sync_slave[NUMBER_SLAVE_BOARDS] = {(p25), (p24), (p23)}; //test to sync the slave |
rrknight | 2:201618ffa295 | 36 | |
rrknight | 0:369222671f3c | 37 | 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 | 0:369222671f3c | 38 | uint8_t writeBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG]; //buffer ready to be sent over SPI to different slaves |
rrknight | 0:369222671f3c | 39 | uint8_t readBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG]; //buffer read by the master on SPI bus |
rrknight | 0:369222671f3c | 40 | |
rrknight | 0:369222671f3c | 41 | Timer timer; |
rrknight | 0:369222671f3c | 42 | uint64_t begin, end; |
rrknight | 0:369222671f3c | 43 | uint8_t numberCmds[NUMBER_SLAVE_BOARDS]; |
rrknight | 0:369222671f3c | 44 | |
rrknight | 0:369222671f3c | 45 | bool newCmd_detected = false; |
rrknight | 0:369222671f3c | 46 | uint8_t nbArrows = 0; |
rrknight | 0:369222671f3c | 47 | |
rrknight | 0:369222671f3c | 48 | char rByte = 0x00; |
rrknight | 2:201618ffa295 | 49 | char checksum = 0x00; |
rrknight | 1:95d85c81bb11 | 50 | |
rrknight | 1:95d85c81bb11 | 51 | bool fiveArrowsFound = false; |
rrknight | 1:95d85c81bb11 | 52 | bool msgValid = false; |
rrknight | 1:95d85c81bb11 | 53 | |
rrknight | 1:95d85c81bb11 | 54 | int move(char *s, int nbBytes) //custom move function (cp from MODESERIAL without the end character) |
rrknight | 1:95d85c81bb11 | 55 | { |
rrknight | 1:95d85c81bb11 | 56 | int counter = 0; |
rrknight | 1:95d85c81bb11 | 57 | char c; |
rrknight | 2:201618ffa295 | 58 | |
rrknight | 2:201618ffa295 | 59 | while(ros.readable()) |
rrknight | 2:201618ffa295 | 60 | { |
rrknight | 1:95d85c81bb11 | 61 | c = ros.getc(); |
rrknight | 1:95d85c81bb11 | 62 | *(s++) = c; |
rrknight | 1:95d85c81bb11 | 63 | counter++; |
rrknight | 2:201618ffa295 | 64 | if(counter == nbBytes) break; |
rrknight | 1:95d85c81bb11 | 65 | } |
rrknight | 2:201618ffa295 | 66 | |
rrknight | 1:95d85c81bb11 | 67 | return counter; |
rrknight | 1:95d85c81bb11 | 68 | } |
rrknight | 1:95d85c81bb11 | 69 | |
rrknight | 1:95d85c81bb11 | 70 | bool verifyChecksum(char* data, int length, char checksum) |
rrknight | 1:95d85c81bb11 | 71 | { |
rrknight | 1:95d85c81bb11 | 72 | for(int i=0; i<length; i++) |
rrknight | 1:95d85c81bb11 | 73 | { |
rrknight | 1:95d85c81bb11 | 74 | checksum += data[i]; |
rrknight | 1:95d85c81bb11 | 75 | } |
rrknight | 1:95d85c81bb11 | 76 | |
rrknight | 1:95d85c81bb11 | 77 | checksum++; //add 1 to obtain 0x00 |
rrknight | 1:95d85c81bb11 | 78 | |
rrknight | 1:95d85c81bb11 | 79 | if(checksum == 0x00) return true; |
rrknight | 1:95d85c81bb11 | 80 | else return false; |
rrknight | 1:95d85c81bb11 | 81 | } |
rrknight | 0:369222671f3c | 82 | |
rrknight | 0:369222671f3c | 83 | // Called everytime a new character goes into |
rrknight | 0:369222671f3c | 84 | // the RX buffer. Test that character for '/' |
rrknight | 0:369222671f3c | 85 | // Note, rxGetLastChar() gets the last char that |
rrknight | 0:369222671f3c | 86 | // we received but it does NOT remove it from |
rrknight | 0:369222671f3c | 87 | // the RX buffer. |
rrknight | 0:369222671f3c | 88 | void rxCallback(MODSERIAL_IRQ_INFO *q) |
rrknight | 0:369222671f3c | 89 | { |
rrknight | 0:369222671f3c | 90 | logicPin = 1; |
rrknight | 0:369222671f3c | 91 | |
rrknight | 2:201618ffa295 | 92 | MODSERIAL *serial = q->serial; |
rrknight | 0:369222671f3c | 93 | rByte = serial->rxGetLastChar(); |
rrknight | 0:369222671f3c | 94 | |
rrknight | 1:95d85c81bb11 | 95 | if(!fiveArrowsFound) |
rrknight | 0:369222671f3c | 96 | { |
rrknight | 1:95d85c81bb11 | 97 | if(nbArrows < NUMBER_OF_ARROWS) |
rrknight | 1:95d85c81bb11 | 98 | { |
rrknight | 1:95d85c81bb11 | 99 | if(rByte == CLOSE_ARROW) |
rrknight | 1:95d85c81bb11 | 100 | { |
rrknight | 1:95d85c81bb11 | 101 | nbArrows++; |
rrknight | 1:95d85c81bb11 | 102 | } |
rrknight | 2:201618ffa295 | 103 | |
rrknight | 1:95d85c81bb11 | 104 | if((nbArrows > 0) && (rByte != CLOSE_ARROW)) |
rrknight | 1:95d85c81bb11 | 105 | { |
rrknight | 1:95d85c81bb11 | 106 | nbArrows = 0; //reset in case the previous arrows was data. |
rrknight | 1:95d85c81bb11 | 107 | } |
rrknight | 1:95d85c81bb11 | 108 | |
rrknight | 1:95d85c81bb11 | 109 | if(nbArrows == NUMBER_OF_ARROWS) |
rrknight | 1:95d85c81bb11 | 110 | { |
rrknight | 1:95d85c81bb11 | 111 | fiveArrowsFound = true; |
rrknight | 1:95d85c81bb11 | 112 | } |
rrknight | 0:369222671f3c | 113 | } |
rrknight | 1:95d85c81bb11 | 114 | } |
rrknight | 1:95d85c81bb11 | 115 | else //fiveArrowsFound, so rByte is the checksum |
rrknight | 1:95d85c81bb11 | 116 | { |
rrknight | 1:95d85c81bb11 | 117 | move(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG); |
rrknight | 1:95d85c81bb11 | 118 | //pc.printf("r cs 0x%02X\n", rByte); |
rrknight | 1:95d85c81bb11 | 119 | //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 | 120 | |
rrknight | 2:201618ffa295 | 121 | msgValid = verifyChecksum(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, rByte); |
rrknight | 2:201618ffa295 | 122 | if(msgValid) pc.printf("msgValid\n\r"); |
rrknight | 1:95d85c81bb11 | 123 | |
rrknight | 1:95d85c81bb11 | 124 | //reset |
rrknight | 1:95d85c81bb11 | 125 | serial->rxBufferFlush(); |
rrknight | 1:95d85c81bb11 | 126 | nbArrows = 0; |
rrknight | 1:95d85c81bb11 | 127 | fiveArrowsFound = false; |
rrknight | 1:95d85c81bb11 | 128 | } |
rrknight | 1:95d85c81bb11 | 129 | |
rrknight | 0:369222671f3c | 130 | logicPin = 0; |
rrknight | 0:369222671f3c | 131 | } |
rrknight | 0:369222671f3c | 132 | |
rrknight | 0:369222671f3c | 133 | int main() |
rrknight | 0:369222671f3c | 134 | { |
rrknight | 2:201618ffa295 | 135 | //Deselect all mbed slaves |
rrknight | 2:201618ffa295 | 136 | for(int k=0; k<NUMBER_SLAVE_BOARDS; k++) |
rrknight | 2:201618ffa295 | 137 | { |
rrknight | 2:201618ffa295 | 138 | cs[k] = 1; |
rrknight | 2:201618ffa295 | 139 | sync_slave[k] = 0; |
rrknight | 2:201618ffa295 | 140 | } |
rrknight | 2:201618ffa295 | 141 | |
rrknight | 0:369222671f3c | 142 | ros.baud(460800); //460800 works //921600 don't |
rrknight | 0:369222671f3c | 143 | pc.baud(115200); |
rrknight | 0:369222671f3c | 144 | |
rrknight | 2:201618ffa295 | 145 | // Setup the spi for 8 bit data, high steady state clock, |
rrknight | 2:201618ffa295 | 146 | // second edge capture, with a 1MHz clock rate |
rrknight | 2:201618ffa295 | 147 | spi.format(8, 0); //spi.format(8,3); |
rrknight | 2:201618ffa295 | 148 | spi.frequency(1000000); //32000000 |
rrknight | 2:201618ffa295 | 149 | |
rrknight | 2:201618ffa295 | 150 | //init the SPI arrays |
rrknight | 2:201618ffa295 | 151 | for(int i=0; i<NUMBER_SLAVE_BOARDS; i++) |
rrknight | 2:201618ffa295 | 152 | { |
rrknight | 2:201618ffa295 | 153 | for(int j=0; j<NUMBER_MSG_PER_PACKET; j++) |
rrknight | 2:201618ffa295 | 154 | { |
rrknight | 2:201618ffa295 | 155 | for(int k=0; k<NUMBER_BYTES_PER_MSG; k++) |
rrknight | 2:201618ffa295 | 156 | { |
rrknight | 2:201618ffa295 | 157 | writeBufferSPI[i][j][k] = 0x00; |
rrknight | 2:201618ffa295 | 158 | readBufferSPI[i][j][k] = 0x00; |
rrknight | 2:201618ffa295 | 159 | } |
rrknight | 2:201618ffa295 | 160 | } |
rrknight | 2:201618ffa295 | 161 | } |
rrknight | 0:369222671f3c | 162 | |
rrknight | 0:369222671f3c | 163 | //init alloc |
rrknight | 0:369222671f3c | 164 | readBufferSerial = (char*)malloc(NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG*sizeof(char*)); |
rrknight | 0:369222671f3c | 165 | |
rrknight | 2:201618ffa295 | 166 | //uint8_t my_val; |
rrknight | 2:201618ffa295 | 167 | |
rrknight | 0:369222671f3c | 168 | ros.attach(&rxCallback, MODSERIAL::RxIrq); |
rrknight | 0:369222671f3c | 169 | |
rrknight | 0:369222671f3c | 170 | pc.printf("*** Start Master Main ***\n\r"); |
rrknight | 0:369222671f3c | 171 | |
rrknight | 0:369222671f3c | 172 | logicPin = 0; |
rrknight | 0:369222671f3c | 173 | |
rrknight | 2:201618ffa295 | 174 | // Wait here until we detect a valid message in the serial RX buffer. |
rrknight | 1:95d85c81bb11 | 175 | while(1) |
rrknight | 1:95d85c81bb11 | 176 | { |
rrknight | 2:201618ffa295 | 177 | if(msgValid) //pass it to the SPI bus |
rrknight | 2:201618ffa295 | 178 | { |
rrknight | 2:201618ffa295 | 179 | //init the SPI arrays |
rrknight | 2:201618ffa295 | 180 | for(int i=0; i<NUMBER_SLAVE_BOARDS; i++) |
rrknight | 2:201618ffa295 | 181 | { |
rrknight | 2:201618ffa295 | 182 | for(int j=0; j<NUMBER_MSG_PER_PACKET; j++) |
rrknight | 2:201618ffa295 | 183 | { |
rrknight | 2:201618ffa295 | 184 | for(int k=0; k<NUMBER_BYTES_PER_MSG; k++) |
rrknight | 2:201618ffa295 | 185 | { |
rrknight | 2:201618ffa295 | 186 | writeBufferSPI[i][j][k] = 0x00; //mode 0 for null command |
rrknight | 2:201618ffa295 | 187 | readBufferSPI[i][j][k] = 0x00; |
rrknight | 2:201618ffa295 | 188 | } |
rrknight | 2:201618ffa295 | 189 | } |
rrknight | 2:201618ffa295 | 190 | } |
rrknight | 2:201618ffa295 | 191 | |
rrknight | 2:201618ffa295 | 192 | //init nb cmds per slave |
rrknight | 2:201618ffa295 | 193 | for(int i=0; i<NUMBER_SLAVE_BOARDS; i++) |
rrknight | 2:201618ffa295 | 194 | { |
rrknight | 2:201618ffa295 | 195 | numberCmds[i] = 0; |
rrknight | 2:201618ffa295 | 196 | } |
rrknight | 2:201618ffa295 | 197 | |
rrknight | 2:201618ffa295 | 198 | |
rrknight | 2:201618ffa295 | 199 | //sort messages for each slave |
rrknight | 2:201618ffa295 | 200 | for(int i=0; i<NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG; i+=NUMBER_BYTES_PER_MSG) |
rrknight | 2:201618ffa295 | 201 | { |
rrknight | 2:201618ffa295 | 202 | uint8_t nodeID = readBufferSerial[i]; |
rrknight | 2:201618ffa295 | 203 | |
rrknight | 2:201618ffa295 | 204 | if(nodeID>=FIRST_NODE_ID_SLAVE_1 && nodeID<FIRST_NODE_ID_SLAVE_2) //slave 1 |
rrknight | 2:201618ffa295 | 205 | { |
rrknight | 2:201618ffa295 | 206 | for(int j=0; j<NUMBER_BYTES_PER_MSG; j++) |
rrknight | 2:201618ffa295 | 207 | { |
rrknight | 2:201618ffa295 | 208 | writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j]; |
rrknight | 2:201618ffa295 | 209 | } |
rrknight | 2:201618ffa295 | 210 | numberCmds[0]++; |
rrknight | 2:201618ffa295 | 211 | } |
rrknight | 2:201618ffa295 | 212 | else if(nodeID>=FIRST_NODE_ID_SLAVE_2 && nodeID<FIRST_NODE_ID_SLAVE_3) //slave 2 |
rrknight | 2:201618ffa295 | 213 | { |
rrknight | 2:201618ffa295 | 214 | for(int j=0; j<NUMBER_BYTES_PER_MSG; j++) |
rrknight | 2:201618ffa295 | 215 | { |
rrknight | 2:201618ffa295 | 216 | writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j]; |
rrknight | 2:201618ffa295 | 217 | } |
rrknight | 2:201618ffa295 | 218 | numberCmds[1]++; |
rrknight | 2:201618ffa295 | 219 | } |
rrknight | 2:201618ffa295 | 220 | else if(nodeID>=FIRST_NODE_ID_SLAVE_3) //slave 3 |
rrknight | 2:201618ffa295 | 221 | { |
rrknight | 2:201618ffa295 | 222 | for(int j=0; j<NUMBER_BYTES_PER_MSG; j++) |
rrknight | 2:201618ffa295 | 223 | { |
rrknight | 2:201618ffa295 | 224 | writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j]; |
rrknight | 2:201618ffa295 | 225 | } |
rrknight | 2:201618ffa295 | 226 | numberCmds[2]++; |
rrknight | 2:201618ffa295 | 227 | } |
rrknight | 2:201618ffa295 | 228 | } |
rrknight | 2:201618ffa295 | 229 | |
rrknight | 2:201618ffa295 | 230 | pc.printf("nbCmd %d %d %d\n", numberCmds[0], numberCmds[1], numberCmds[2]); |
rrknight | 2:201618ffa295 | 231 | //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 | 232 | |
rrknight | 2:201618ffa295 | 233 | //new commands has been grabbed and are ready to be sent to slaves |
rrknight | 2:201618ffa295 | 234 | |
rrknight | 2:201618ffa295 | 235 | for(int k=0; k<NUMBER_SLAVE_BOARDS; k++) //NUMBER_SLAVE_BOARDS for each slave |
rrknight | 2:201618ffa295 | 236 | { |
rrknight | 2:201618ffa295 | 237 | sync_slave[k] = 1; |
rrknight | 2:201618ffa295 | 238 | wait_us(10); //pause so the slave can see it's been selected //TODO array with the other slaves |
rrknight | 2:201618ffa295 | 239 | sync_slave[k] = 0; |
rrknight | 2:201618ffa295 | 240 | |
rrknight | 2:201618ffa295 | 241 | cs[k] = 0; |
rrknight | 2:201618ffa295 | 242 | //while (!(LPC_SSP1->SR & TNF)); |
rrknight | 2:201618ffa295 | 243 | spi.write(OPEN_ARROW); |
rrknight | 2:201618ffa295 | 244 | //while (!(LPC_SSP1->SR & TFE)); |
rrknight | 2:201618ffa295 | 245 | wait_us(5); |
rrknight | 2:201618ffa295 | 246 | cs[k] = 1; |
rrknight | 2:201618ffa295 | 247 | wait_us(10); //wait_us(10); |
rrknight | 2:201618ffa295 | 248 | |
rrknight | 2:201618ffa295 | 249 | cs[k] = 0; |
rrknight | 2:201618ffa295 | 250 | //while (!(LPC_SSP1->SR & TNF)); |
rrknight | 2:201618ffa295 | 251 | spi.write(OPEN_ARROW); |
rrknight | 2:201618ffa295 | 252 | // while (!(LPC_SSP1->SR & TFE)); |
rrknight | 2:201618ffa295 | 253 | wait_us(5); |
rrknight | 2:201618ffa295 | 254 | cs[k] = 1; |
rrknight | 2:201618ffa295 | 255 | wait_us(10); |
rrknight | 2:201618ffa295 | 256 | |
rrknight | 2:201618ffa295 | 257 | cs[k] = 0; |
rrknight | 2:201618ffa295 | 258 | //while (!(LPC_SSP1->SR & TNF)); |
rrknight | 2:201618ffa295 | 259 | spi.write(OPEN_ARROW); |
rrknight | 2:201618ffa295 | 260 | //while (!(LPC_SSP1->SR & TFE)); //MAYBE REMOVE COMMENT ? |
rrknight | 2:201618ffa295 | 261 | wait_us(5); |
rrknight | 2:201618ffa295 | 262 | cs[k] = 1; |
rrknight | 2:201618ffa295 | 263 | wait_us(10); |
rrknight | 2:201618ffa295 | 264 | |
rrknight | 2:201618ffa295 | 265 | for(int i=0; i<NUMBER_MSG_PER_PACKET; i++) |
rrknight | 2:201618ffa295 | 266 | { |
rrknight | 2:201618ffa295 | 267 | for(int j=0; j<NUMBER_BYTES_PER_MSG; j++) |
rrknight | 2:201618ffa295 | 268 | { |
rrknight | 2:201618ffa295 | 269 | cs[k] = 0; |
rrknight | 2:201618ffa295 | 270 | //while (!(LPC_SSP1->SR & TNF)); |
rrknight | 2:201618ffa295 | 271 | readBufferSPI[k][i][j] = (char)(spi.write(writeBufferSPI[k][i][j])); |
rrknight | 2:201618ffa295 | 272 | //while (!(LPC_SSP1->SR & TFE)); |
rrknight | 2:201618ffa295 | 273 | wait_us(5); |
rrknight | 2:201618ffa295 | 274 | cs[k] = 1; |
rrknight | 2:201618ffa295 | 275 | wait_us(10); |
rrknight | 2:201618ffa295 | 276 | } |
rrknight | 2:201618ffa295 | 277 | } |
rrknight | 2:201618ffa295 | 278 | } |
rrknight | 2:201618ffa295 | 279 | |
rrknight | 2:201618ffa295 | 280 | logicPin = 0; |
rrknight | 2:201618ffa295 | 281 | wait_us(10); |
rrknight | 2:201618ffa295 | 282 | logicPin = 1; |
rrknight | 2:201618ffa295 | 283 | wait_us(10); |
rrknight | 2:201618ffa295 | 284 | //print the array : |
rrknight | 2:201618ffa295 | 285 | /* |
rrknight | 2:201618ffa295 | 286 | for(int i=0; i<2; i++) |
rrknight | 2:201618ffa295 | 287 | { |
rrknight | 2:201618ffa295 | 288 | 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 | 289 | } |
rrknight | 2:201618ffa295 | 290 | */ |
rrknight | 2:201618ffa295 | 291 | /* int i=0; |
rrknight | 2:201618ffa295 | 292 | 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 | 293 | i=14; |
rrknight | 2:201618ffa295 | 294 | 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 | 295 | |
rrknight | 2:201618ffa295 | 296 | //pc.printf("\n\r"); |
rrknight | 2:201618ffa295 | 297 | logicPin = 0; |
rrknight | 2:201618ffa295 | 298 | |
rrknight | 2:201618ffa295 | 299 | //build the motorDataSet_msg |
rrknight | 2:201618ffa295 | 300 | for(int i=0; i<13; i++) |
rrknight | 2:201618ffa295 | 301 | { |
rrknight | 2:201618ffa295 | 302 | motorDataSet_msg.motorData[i].encPosition = 100*i; |
rrknight | 2:201618ffa295 | 303 | motorDataSet_msg.motorData[i].potiPosition = 10*i; |
rrknight | 2:201618ffa295 | 304 | motorDataSet_msg.motorData[i].current = -10*i; |
rrknight | 2:201618ffa295 | 305 | motorDataSet_msg.motorData[i].force = 2*i; |
rrknight | 2:201618ffa295 | 306 | } |
rrknight | 2:201618ffa295 | 307 | */ |
rrknight | 2:201618ffa295 | 308 | //TODO write the data msg on serial Tx |
rrknight | 2:201618ffa295 | 309 | logicPin = 0; |
rrknight | 2:201618ffa295 | 310 | |
rrknight | 2:201618ffa295 | 311 | msgValid = false; //toggle flag for next message |
rrknight | 2:201618ffa295 | 312 | |
rrknight | 1:95d85c81bb11 | 313 | } |
rrknight | 1:95d85c81bb11 | 314 | |
rrknight | 2:201618ffa295 | 315 | wait_us(10); |
rrknight | 2:201618ffa295 | 316 | } |
rrknight | 0:369222671f3c | 317 | } |