TheRobotStudio ROSA
Code for the mbed NXP LPC1768. To be used on The Robot Studio Master Boards. License : Simplified BSD.
Diff: main.cpp
- Revision:
- 3:af892e4bf53e
- Parent:
- 2:201618ffa295
- Child:
- 4:7da0cd1fcb8a
--- a/main.cpp Wed Mar 06 14:33:01 2013 +0000
+++ b/main.cpp Wed Mar 06 17:07:36 2013 +0000
@@ -6,8 +6,9 @@
//Define
#define NUMBER_MAX_EPOS2_PER_SLAVE 15
-#define NUMBER_MSG_PER_PACKET 15
-#define NUMBER_BYTES_PER_MSG 7
+#define NUMBER_MSG_PER_PACKET 45
+#define NUMBER_BYTES_PER_MSG 6
+#define NUMBER_BYTES_TO_READ NUMBER_BYTES_PER_MSG + 2
#define NUMBER_SLAVE_BOARDS 3
#define FIRST_NODE_ID_SLAVE_1 1
#define FIRST_NODE_ID_SLAVE_2 FIRST_NODE_ID_SLAVE_1 + NUMBER_MAX_EPOS2_PER_SLAVE
@@ -17,12 +18,13 @@
#define LOOP_PERIOD_TIME 20000 //20 ms
//SPI RxTx FIFO bits
-#define TNF 0x02
-#define TFE 0x01
-#define RNE 0x04
+//#define TNF 0x02
+//#define TFE 0x01
+//#define RNE 0x04
#define OPEN_ARROW 0x3C //< = 60
#define CLOSE_ARROW 0x3E //< = 62
+#define DUMMY_BYTE 0x00
#define NUMBER_OF_ARROWS 5
MODSERIAL ros(p28, p27, 1024, 512); // tx, rx
@@ -35,8 +37,8 @@
DigitalOut sync_slave[NUMBER_SLAVE_BOARDS] = {(p25), (p24), (p23)}; //test to sync the slave
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)
-uint8_t writeBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG]; //buffer ready to be sent over SPI to different slaves
-uint8_t readBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG]; //buffer read by the master on SPI bus
+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
+uint8_t readBufferSPI[NUMBER_SLAVE_BOARDS][NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_TO_READ]; //buffer read by the master on SPI bus
Timer timer;
uint64_t begin, end;
@@ -46,7 +48,8 @@
uint8_t nbArrows = 0;
char rByte = 0x00;
-char checksum = 0x00;
+char writeChecksum[NUMBER_SLAVE_BOARDS]; //one checksum per board
+char readChecksum[NUMBER_SLAVE_BOARDS];
bool fiveArrowsFound = false;
bool msgValid = false;
@@ -80,6 +83,24 @@
else return false;
}
+void calculateSPIChecksum()
+{
+ for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
+ {
+ int sum = 0;
+
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
+ {
+ for(int j=0; j<NUMBER_BYTES_TO_READ; j++)
+ {
+ writeChecksum[k] += writeBufferSPI[k][i][j];
+ }
+ }
+
+ writeChecksum[k] = (char)(~sum);
+ }
+}
+
// Called everytime a new character goes into
// the RX buffer. Test that character for '/'
// Note, rxGetLastChar() gets the last char that
@@ -119,7 +140,7 @@
//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");
+ //if(msgValid) pc.printf("msgValid\n\r");
//reset
serial->rxBufferFlush();
@@ -150,14 +171,16 @@
//init the SPI arrays
for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
{
- for(int j=0; j<NUMBER_MSG_PER_PACKET; j++)
+ for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
{
- for(int k=0; k<NUMBER_BYTES_PER_MSG; k++)
+ for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
{
writeBufferSPI[i][j][k] = 0x00;
readBufferSPI[i][j][k] = 0x00;
}
}
+
+ writeChecksum[i] = 0x00;
}
//init alloc
@@ -179,14 +202,16 @@
//init the SPI arrays
for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
{
- for(int j=0; j<NUMBER_MSG_PER_PACKET; j++)
+ for(int j=0; j<NUMBER_MAX_EPOS2_PER_SLAVE; j++)
{
- for(int k=0; k<NUMBER_BYTES_PER_MSG; k++)
+ for(int k=0; k<NUMBER_BYTES_TO_READ; k++)
{
writeBufferSPI[i][j][k] = 0x00; //mode 0 for null command
readBufferSPI[i][j][k] = 0x00;
}
}
+
+ writeChecksum[i] = 0x00;
}
//init nb cmds per slave
@@ -205,15 +230,16 @@
{
for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
{
- writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
+ writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
}
+
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[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
+ writeBufferSPI[1][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
}
numberCmds[1]++;
}
@@ -221,17 +247,32 @@
{
for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
{
- writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
+ writeBufferSPI[2][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
}
numberCmds[2]++;
}
}
- pc.printf("nbCmd %d %d %d\n", numberCmds[0], numberCmds[1], numberCmds[2]);
+ //add dummy bytes
+ for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
+ {
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
+ {
+ for(int j=NUMBER_BYTES_PER_MSG; j<NUMBER_BYTES_TO_READ; j++)
+ {
+ writeBufferSPI[k][i][j] = DUMMY_BYTE;
+ }
+ }
+ }
+
+ //now all individual SPI buffers for slaves have been created
+ //compute checksum for each slave and update the variable, it'll be sent later at the end of SPI writting
+ calculateSPIChecksum(); //this update the writeChecksum[k]
+
+ //pc.printf("nbCmd %d %d %d\n", numberCmds[0], numberCmds[1], numberCmds[2]);
//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]);
- //new commands has been grabbed and are ready to be sent to slaves
-
+ //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
{
sync_slave[k] = 1;
@@ -262,19 +303,24 @@
cs[k] = 1;
wait_us(10);
- for(int i=0; i<NUMBER_MSG_PER_PACKET; i++)
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
{
for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
{
cs[k] = 0;
- //while (!(LPC_SSP1->SR & TNF));
readBufferSPI[k][i][j] = (char)(spi.write(writeBufferSPI[k][i][j]));
- //while (!(LPC_SSP1->SR & TFE));
wait_us(5);
cs[k] = 1;
wait_us(10);
}
- }
+ }
+
+ //finally write the command checksum and read the data checksum at the same time
+ cs[k] = 0;
+ readChecksum[k] = (char)(spi.write(writeChecksum[k]));
+ wait_us(5);
+ cs[k] = 1;
+ wait_us(10);
}
logicPin = 0;