TheRobotStudio ROSA
Code for the mbed NXP LPC1768. To be used on The Robot Studio Master Boards. License : Simplified BSD.
Diff: main.cpp
- Revision:
- 4:7da0cd1fcb8a
- Parent:
- 3:af892e4bf53e
- Child:
- 5:c73f87d22142
--- a/main.cpp Wed Mar 06 17:07:36 2013 +0000
+++ b/main.cpp Fri Mar 08 17:53:55 2013 +0000
@@ -50,9 +50,11 @@
char rByte = 0x00;
char writeChecksum[NUMBER_SLAVE_BOARDS]; //one checksum per board
char readChecksum[NUMBER_SLAVE_BOARDS];
+char serialTxChecksum = 0x00;
bool fiveArrowsFound = false;
-bool msgValid = false;
+bool cmdValid = false;
+bool dataValid = false;
int move(char *s, int nbBytes) //custom move function (cp from MODESERIAL without the end character)
{
@@ -70,7 +72,7 @@
return counter;
}
-bool verifyChecksum(char* data, int length, char checksum)
+bool verifyCmdChecksum(char* data, int length, char checksum) //verify data comming from he PC
{
for(int i=0; i<length; i++)
{
@@ -83,7 +85,29 @@
else return false;
}
-void calculateSPIChecksum()
+bool verifyDataChecksum() //verify data comming from the slaves on SPI
+{
+ bool allDataValid = true;
+
+ for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
+ {
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
+ {
+ for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
+ {
+ readChecksum[k] += readBufferSPI[k][i][j];
+ }
+ }
+
+ readChecksum[k]++;
+
+ if(readChecksum[k] != 0x00) allDataValid = false; //toggle the flag if one of them is corrupted
+ }
+
+ return allDataValid;
+}
+
+void calculateSPIChecksum() //compute checksum for each slave to send commands over SPI
{
for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
{
@@ -93,14 +117,32 @@
{
for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
{
- writeChecksum[k] += writeBufferSPI[k][i][j];
+ sum += writeBufferSPI[k][i][j];
}
}
- writeChecksum[k] = (char)(~sum);
+ writeChecksum[k] = (char)(~sum); //reverse 0 and 1, and cast as byte
}
}
+void calculateTxChecksum() //compute checksum for all the data sent to the PC over serial
+{
+ int sum = 0;
+
+ for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
+ {
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
+ {
+ for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
+ {
+ sum += readBufferSPI[k][i][j];
+ }
+ }
+ }
+
+ serialTxChecksum = (char)(~sum); //reverse 0 and 1, and cast as byte
+}
+
// Called everytime a new character goes into
// the RX buffer. Test that character for '/'
// Note, rxGetLastChar() gets the last char that
@@ -108,7 +150,7 @@
// the RX buffer.
void rxCallback(MODSERIAL_IRQ_INFO *q)
{
- logicPin = 1;
+ //logicPin = 1;
MODSERIAL *serial = q->serial;
rByte = serial->rxGetLastChar();
@@ -139,8 +181,8 @@
//pc.printf("r cs 0x%02X\n", rByte);
//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]);
- msgValid = verifyChecksum(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, rByte);
- //if(msgValid) pc.printf("msgValid\n\r");
+ cmdValid = verifyCmdChecksum(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, rByte);
+ //if(cmdValid) pc.printf("cmdValid\n\r");
//reset
serial->rxBufferFlush();
@@ -148,7 +190,7 @@
fiveArrowsFound = false;
}
- logicPin = 0;
+ //logicPin = 0;
}
int main()
@@ -197,8 +239,10 @@
// Wait here until we detect a valid message in the serial RX buffer.
while(1)
{
- if(msgValid) //pass it to the SPI bus
- {
+ if(cmdValid) //pass it to the SPI bus
+ {
+ logicPin = 1;
+
//init the SPI arrays
for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
{
@@ -213,13 +257,14 @@
writeChecksum[i] = 0x00;
}
-
- //init nb cmds per slave
+ /*
+ //init nb cmds per slave //useless ?
for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
{
numberCmds[i] = 0;
}
-
+ */
+ logicPin = 0;
//sort messages for each slave
for(int i=0; i<NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG; i+=NUMBER_BYTES_PER_MSG)
@@ -231,28 +276,39 @@
for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
{
writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
- }
-
- numberCmds[0]++;
+ }
+ //numberCmds[0]++;
}
else if(nodeID>=FIRST_NODE_ID_SLAVE_2 && nodeID<FIRST_NODE_ID_SLAVE_3) //slave 2
{
for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
{
- writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
+ writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG-NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
}
- numberCmds[1]++;
+
+ //change nodeID between 1 and 15
+ 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
+
+ //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
+ //numberCmds[1]++;
+ //pc.printf("ID[%d] %d\n", i/NUMBER_BYTES_PER_MSG, writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG][0]);
}
else if(nodeID>=FIRST_NODE_ID_SLAVE_3) //slave 3
{
for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
{
- writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
+ writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG-2*NUMBER_MAX_EPOS2_PER_SLAVE][j] = readBufferSerial[i+j];
}
- numberCmds[2]++;
- }
+
+ //change nodeID between 1 and 15
+ 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
+ //numberCmds[2]++;
+ }
+ //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
}
+ //pc.printf("ID %d\n", writeBufferSPI[1][0][0]);
+
//add dummy bytes
for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
{
@@ -275,43 +331,41 @@
//new commands has been grabbed and are ready to be sent to slaves
for(int k=0; k<NUMBER_SLAVE_BOARDS; k++) //NUMBER_SLAVE_BOARDS for each slave
{
+ if(k<3) ledchain[k] = 1; //switch on LED nb k
+
sync_slave[k] = 1;
- wait_us(10); //pause so the slave can see it's been selected //TODO array with the other slaves
+ wait_us(10); //pause so the slave can see it's been selected
sync_slave[k] = 0;
- cs[k] = 0;
- //while (!(LPC_SSP1->SR & TNF));
+ cs[k] = 0;
spi.write(OPEN_ARROW);
- //while (!(LPC_SSP1->SR & TFE));
wait_us(5);
cs[k] = 1;
- wait_us(10); //wait_us(10);
+ wait_us(8);
cs[k] = 0;
- //while (!(LPC_SSP1->SR & TNF));
spi.write(OPEN_ARROW);
- // while (!(LPC_SSP1->SR & TFE));
wait_us(5);
cs[k] = 1;
- wait_us(10);
+ wait_us(8);
- cs[k] = 0;
- //while (!(LPC_SSP1->SR & TNF));
+ cs[k] = 0;
spi.write(OPEN_ARROW);
- //while (!(LPC_SSP1->SR & TFE)); //MAYBE REMOVE COMMENT ?
wait_us(5);
cs[k] = 1;
- wait_us(10);
+ wait_us(8);
for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
- {
- for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
+ {
+ //writeBufferSPI[k][i][0] = writeBufferSPI[k][i][0] - 0x0F;
+
+ for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
{
cs[k] = 0;
readBufferSPI[k][i][j] = (char)(spi.write(writeBufferSPI[k][i][j]));
wait_us(5);
cs[k] = 1;
- wait_us(10);
+ wait_us(8);
}
}
@@ -320,13 +374,53 @@
readChecksum[k] = (char)(spi.write(writeChecksum[k]));
wait_us(5);
cs[k] = 1;
- wait_us(10);
+ wait_us(8);
+
+ if(k<3) ledchain[k] = 0; //switch on LED nb k
}
+ //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]);
+ //pc.printf("nodeID 1-%d 2-%d 3-%d\n", writeBufferSPI[0][0][0], writeBufferSPI[1][0][0], writeBufferSPI[2][0][0]);
+ //pc.printf("rc 0x%02X wc 0x%02X\n", readChecksum[0], writeChecksum[0]);
+ /*
logicPin = 0;
wait_us(10);
logicPin = 1;
wait_us(10);
+ */
+ //now check the validity of the data
+ dataValid = verifyDataChecksum();
+
+ //Erase Tx serial buffer
+ //ros.txBufferFlush();
+
+ //write the data msg on serial Tx
+ //if(dataValid)
+ //{
+ logicPin = 1;
+
+ //compute checksum for all data from slaves
+ calculateTxChecksum();
+
+ //write data and checksum on Tx
+ for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
+ {
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
+ {
+ for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
+ {
+ ros.putc(readBufferSPI[k][i][j]);
+ }
+ }
+ }
+
+ ros.putc(serialTxChecksum);
+
+ dataValid = false; //toggle flag for next message
+
+ logicPin = 0;
+ //}
+
//print the array :
/*
for(int i=0; i<2; i++)
@@ -351,10 +445,9 @@
motorDataSet_msg.motorData[i].force = 2*i;
}
*/
- //TODO write the data msg on serial Tx
- logicPin = 0;
+ //logicPin = 0;
- msgValid = false; //toggle flag for next message
+ cmdValid = false; //toggle flag for next message
}