TheRobotStudio ROSA
Code for the mbed NXP LPC1768. To be used on The Robot Studio Master Boards. License : Simplified BSD.
Diff: main.cpp
- Revision:
- 2:201618ffa295
- Parent:
- 1:95d85c81bb11
- Child:
- 3:af892e4bf53e
--- a/main.cpp Tue Mar 05 15:46:46 2013 +0000
+++ b/main.cpp Wed Mar 06 14:33:01 2013 +0000
@@ -9,6 +9,9 @@
#define NUMBER_MSG_PER_PACKET 15
#define NUMBER_BYTES_PER_MSG 7
#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
+#define FIRST_NODE_ID_SLAVE_3 FIRST_NODE_ID_SLAVE_2 + NUMBER_MAX_EPOS2_PER_SLAVE
#define EPOS2_OK 0
#define EPOS2_ERROR -1
#define LOOP_PERIOD_TIME 20000 //20 ms
@@ -18,6 +21,7 @@
#define TFE 0x01
#define RNE 0x04
+#define OPEN_ARROW 0x3C //< = 60
#define CLOSE_ARROW 0x3E //< = 62
#define NUMBER_OF_ARROWS 5
@@ -26,6 +30,10 @@
DigitalOut ledchain[] = {(LED1), (LED2), (LED3), (LED4)}; //used for debugging
DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
+SPI spi(p5, p6, p7); // mosi, miso, sclk
+DigitalOut cs[NUMBER_SLAVE_BOARDS] = {(p8), (p9), (p10)}; //Slave Mbed number 1 //chip select
+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
@@ -36,11 +44,9 @@
bool newCmd_detected = false;
uint8_t nbArrows = 0;
-
-uint8_t i=0;
-uint8_t j=0;
char rByte = 0x00;
+char checksum = 0x00;
bool fiveArrowsFound = false;
bool msgValid = false;
@@ -49,12 +55,15 @@
{
int counter = 0;
char c;
- while(ros.readable()) {
+
+ while(ros.readable())
+ {
c = ros.getc();
*(s++) = c;
counter++;
- if (counter == nbBytes) break;
+ if(counter == nbBytes) break;
}
+
return counter;
}
@@ -80,27 +89,8 @@
{
logicPin = 1;
- MODSERIAL *serial = q->serial;
- /*
- if ( serial->rxGetLastChar() == '/')
- {
- //pc.printf("new1\n");
- //newline_detected = true;
- serial->move(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, '\n');
- serial->rxBufferFlush();
- //pc.printf("new\n");
- //for(int n=0; n<1; n++)
- //{
- //pc.printf("new2\n\r");
- //pc.printf("0x%02X\n", s[0]);
- //pc.printf("0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n", s[0], s[1], s[2], s[3], s[4], s[5], s[6]);
- //}
- }
-
- */
-
+ MODSERIAL *serial = q->serial;
rByte = serial->rxGetLastChar();
- //pc.printf("0x%02X ", rByte);
if(!fiveArrowsFound)
{
@@ -109,13 +99,11 @@
if(rByte == CLOSE_ARROW)
{
nbArrows++;
- //pc.printf("nbArrows %d\n\r", nbArrows);
}
- //else
+
if((nbArrows > 0) && (rByte != CLOSE_ARROW))
{
nbArrows = 0; //reset in case the previous arrows was data.
- //pc.printf("reset\n\r");
}
if(nbArrows == NUMBER_OF_ARROWS)
@@ -130,75 +118,200 @@
//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");
+ msgValid = verifyChecksum(readBufferSerial, NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG, rByte);
+ if(msgValid) pc.printf("msgValid\n\r");
//reset
serial->rxBufferFlush();
nbArrows = 0;
fiveArrowsFound = false;
}
-/*
- else
- {
- readBufferSerial[i][j] = rByte;
- //pc.printf("i=%d j=%d\n", i,j);
-
- j++;
- if(j >= NUMBER_BYTES_PER_MSG)
- {
- j = 0;
- i++; //next cmd
-
- if(i >= NUMBER_MSG_PER_PACKET)
- {
- nbArrows = 0;
- i = 0;
- j = 0;
- serial->rxBufferFlush();
- //serial->txBufferFlush();
-
- for(int n=0; n<1; n++)
- {
- pc.printf("0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n", readBufferSerial[n][0], readBufferSerial[n][1], readBufferSerial[n][2], readBufferSerial[n][3], readBufferSerial[n][4], readBufferSerial[n][5], readBufferSerial[n][6]);
- }
- }
- }
- }
-*/
logicPin = 0;
}
int main()
{
+ //Deselect all mbed slaves
+ for(int k=0; k<NUMBER_SLAVE_BOARDS; k++)
+ {
+ cs[k] = 1;
+ sync_slave[k] = 0;
+ }
+
ros.baud(460800); //460800 works //921600 don't
pc.baud(115200);
- i=0;
- j=0;
+ // Setup the spi for 8 bit data, high steady state clock,
+ // second edge capture, with a 1MHz clock rate
+ spi.format(8, 0); //spi.format(8,3);
+ spi.frequency(1000000); //32000000
+
+ //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 k=0; k<NUMBER_BYTES_PER_MSG; k++)
+ {
+ writeBufferSPI[i][j][k] = 0x00;
+ readBufferSPI[i][j][k] = 0x00;
+ }
+ }
+ }
//init alloc
readBufferSerial = (char*)malloc(NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG*sizeof(char*));
+ //uint8_t my_val;
+
ros.attach(&rxCallback, MODSERIAL::RxIrq);
pc.printf("*** Start Master Main ***\n\r");
logicPin = 0;
- // Wait here until we detect the \n going into the buffer.
+ // Wait here until we detect a valid message in the serial RX buffer.
while(1)
{
- if(msgValid)
- {
- //pass it to the SPI bus
+ if(msgValid) //pass it to the SPI bus
+ {
+ //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 k=0; k<NUMBER_BYTES_PER_MSG; k++)
+ {
+ writeBufferSPI[i][j][k] = 0x00; //mode 0 for null command
+ readBufferSPI[i][j][k] = 0x00;
+ }
+ }
+ }
+
+ //init nb cmds per slave
+ for(int i=0; i<NUMBER_SLAVE_BOARDS; i++)
+ {
+ numberCmds[i] = 0;
+ }
+
+
+ //sort messages for each slave
+ for(int i=0; i<NUMBER_MSG_PER_PACKET*NUMBER_BYTES_PER_MSG; i+=NUMBER_BYTES_PER_MSG)
+ {
+ uint8_t nodeID = readBufferSerial[i];
+
+ if(nodeID>=FIRST_NODE_ID_SLAVE_1 && nodeID<FIRST_NODE_ID_SLAVE_2) //slave 1
+ {
+ for(int j=0; j<NUMBER_BYTES_PER_MSG; 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];
+ }
+ numberCmds[1]++;
+ }
+ else if(nodeID>=FIRST_NODE_ID_SLAVE_3) //slave 3
+ {
+ for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
+ {
+ writeBufferSPI[0][i/NUMBER_BYTES_PER_MSG][j] = readBufferSerial[i+j];
+ }
+ numberCmds[2]++;
+ }
+ }
+
+ 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
+
+ for(int k=0; k<NUMBER_SLAVE_BOARDS; k++) //NUMBER_SLAVE_BOARDS for each slave
+ {
+ sync_slave[k] = 1;
+ wait_us(10); //pause so the slave can see it's been selected //TODO array with the other slaves
+ sync_slave[k] = 0;
+
+ 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(10);
+
+ 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);
+
+ cs[k] = 0;
+ //while (!(LPC_SSP1->SR & TNF));
+ spi.write(OPEN_ARROW);
+ //while (!(LPC_SSP1->SR & TFE)); //MAYBE REMOVE COMMENT ?
+ wait_us(5);
+ cs[k] = 1;
+ wait_us(10);
+
+ for(int i=0; i<NUMBER_MSG_PER_PACKET; 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);
+ }
+ }
+ }
+
+ logicPin = 0;
+ wait_us(10);
+ logicPin = 1;
+ wait_us(10);
+ //print the array :
+ /*
+ for(int i=0; i<2; i++)
+ {
+ 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]);
+ }
+ */
+ /* int i=0;
+ 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]);
+ i=14;
+ 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]);
+
+ //pc.printf("\n\r");
+ logicPin = 0;
+
+ //build the motorDataSet_msg
+ for(int i=0; i<13; i++)
+ {
+ motorDataSet_msg.motorData[i].encPosition = 100*i;
+ motorDataSet_msg.motorData[i].potiPosition = 10*i;
+ motorDataSet_msg.motorData[i].current = -10*i;
+ motorDataSet_msg.motorData[i].force = 2*i;
+ }
+ */
+ //TODO write the data msg on serial Tx
+ logicPin = 0;
+
+ msgValid = false; //toggle flag for next message
+
}
- }
-
- // When we get here the RX buffer now contains a sentence.
- // ...
-
+ wait_us(10);
+ }
}
\ No newline at end of file