TheRobotStudio ROSA
Code for the mbed NXP LPC1768 To be used on The Robot Studio Slave Boards License : Simplified BSD
Diff: main.cpp
- Revision:
- 2:7ab1d5918efe
- Parent:
- 1:b430b4401fc4
- Child:
- 3:c16d726670b2
--- a/main.cpp Wed Mar 06 17:07:47 2013 +0000
+++ b/main.cpp Fri Mar 08 17:54:03 2013 +0000
@@ -1,5 +1,5 @@
-#define COMPILE_MAIN_CODE_ROSSERIAL
-#ifdef COMPILE_MAIN_CODE_ROSSERIAL
+#define COMPILE_MAIN_CODE_TRS_SLAVE
+#ifdef COMPILE_MAIN_CODE_TRS_SLAVE
//include files
#include "include/eposCmd.h"
@@ -14,7 +14,6 @@
#define RNE 0x04
SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
-DigitalOut myled(LED1);
DigitalIn sync_master(p25);
DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
@@ -68,7 +67,7 @@
}
*/
-bool verifyChecksum()
+bool verifyChecksum() //check the data comming from the master over SPI
{
for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
{
@@ -79,14 +78,31 @@
}
cmdChecksum++; //add 1 to obtain 0x00
+
+ //pc.printf("sum 0x%02X\n\r", cmdChecksum);
if(cmdChecksum == 0x00) return true;
else return false;
}
+void calculateSPIChecksum() //compute checksum for the data sent to the master over SPI
+{
+ int sum = 0;
+
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
+ {
+ for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
+ {
+ sum += writeBufferSPI[i][j];
+ }
+ }
+
+ dataChecksum = (char)(~sum); //reverse 0 and 1, and cast as byte
+}
+
int16_t getMedianForceVal(const int8_t nodeID)
{
- logicPin = 1;
+ //logicPin = 1;
for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
{
@@ -107,7 +123,7 @@
sortForceVal[p+1] = n;
}
- logicPin = 0;
+ //logicPin = 0;
return sortForceVal[2];
}
@@ -242,34 +258,86 @@
} //end interrupt
void commandPlayer() //called in main every 20ms
-{/*
- for(int i= 0; i<CMD_BUFFER_SIZE; i++)
+{
+ //at least one cmd played
+ bool cmdPlayLED = false;
+
+ for(int i= 0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
{
- if(motorCmdBuffer[i].beatDelay != -1)
- {
- switch (motorCmdBuffer[i].mode)
+ uint8_t node_ID = readBufferSPI[i][0];
+ uint8_t node_mode = readBufferSPI[i][1];
+ int32_t value = readBufferSPI[i][2] + (readBufferSPI[i][3]<<8) + (readBufferSPI[i][4]<<16) + (readBufferSPI[i][5]<<24);
+
+ if(node_mode != 0xFF)
+ { /*
+ switch (node_mode)
{
case POSITION: //first change modes of motors that will be triggered later
- if((activMode[motorCmdBuffer[i].nodeID-1] != POSITION) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_POSITION_MODE);
- if(motorCmdBuffer[i].beatDelay == 0) setPosition(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
+ if(activMode[node_ID-1] != POSITION) setModeOfOperationPDO(node_ID, VALUE_POSITION_MODE);
+ setPosition(node_ID, value);
break;
case CURRENT: //first change modes of motors that will be triggered later (like CURRENT mode needs some time to be active)
//pc.printf("setCurrent(%d, %d)\n", motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
- if((activMode[motorCmdBuffer[i].nodeID-1] != CURRENT) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_CURRENT_MODE);
- if(motorCmdBuffer[i].beatDelay == 0) setCurrent(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
+ if(activMode[node_ID-1] != CURRENT) setModeOfOperationPDO(node_ID, VALUE_CURRENT_MODE);
+ setCurrent(node_ID, value);
break;
case VELOCITY: //first change modes of motors that will be triggered later
- if((activMode[motorCmdBuffer[i].nodeID-1] != VELOCITY) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_VELOCITY_MODE);
- if(motorCmdBuffer[i].beatDelay == 0) setVelocity(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
+ if(activMode[node_ID-1] != VELOCITY) setModeOfOperationPDO(node_ID, VALUE_VELOCITY_MODE);
+ setVelocity(node_ID, value);
break;
default:
break;
}
-
- //decrement only cmd with delay != -1, so cmd that has not been played yet.
- motorCmdBuffer[i].beatDelay = motorCmdBuffer[i].beatDelay - 1;
+ */
+ ledchain[3] = 1; //switch on when cmd is applied
+ cmdPlayLED = true;
+ }
+ else //idle mode 0xFF
+ {
+ if(!cmdPlayLED) ledchain[3] = 0; //switch off when slave is idle, i.e. all cmd in a set are 0xFF
}
- }*/
+
+ wait_us(10);
+ }
+}
+
+void updateTxSPIBuffer()
+{
+ for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
+ {
+ uint8_t node_id = i+1;
+ //if(node_id!=5)
+ //{
+ getPosition(node_id);
+ wait_us(200);
+ getCurrent(node_id);
+ wait_us(200);
+ //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);
+ //}
+ }
+
+ //build the motorDataSet_msg
+ for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
+ {
+ uint8_t node_id = i+1;
+
+ //position
+ writeBufferSPI[i][0] = encPosition[node_id-1];
+ writeBufferSPI[i][1] = encPosition[node_id-1]>>8;
+ writeBufferSPI[i][2] = encPosition[node_id-1]>>16;
+ writeBufferSPI[i][3] = encPosition[node_id-1]>>24;
+
+ //current
+ writeBufferSPI[i][4] = avgCurrent[node_id-1];
+ writeBufferSPI[i][5] = avgCurrent[node_id-1]>>8;
+
+ //force
+ writeBufferSPI[i][6] = 0;
+ writeBufferSPI[i][7] = 0;
+
+ //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
+ //force = getMedianForceVal(node_id); //medForce[node_id-1];
+ }
}
void initBufferSPI()
@@ -279,7 +347,7 @@
{
for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
{
- //writeBufferSPI[i][j] = 0x00;
+ writeBufferSPI[i][j] = 0x00;
readBufferSPI[i][j] = 0x00;
}
}
@@ -287,29 +355,28 @@
for(int n=0; n<NUMBER_MAX_EPOS2_PER_SLAVE; n++)
{
//position
- writeBufferSPI[n][0] = 0x60+n;
- writeBufferSPI[n][1] = 0x70+n;
- writeBufferSPI[n][2] = 0x80+n;
- writeBufferSPI[n][3] = 0x90+n;
+ writeBufferSPI[n][0] = 0xA0+n;
+ writeBufferSPI[n][1] = 0xB0+n;
+ writeBufferSPI[n][2] = 0x00;
+ writeBufferSPI[n][3] = 0x00;
//current
writeBufferSPI[n][4] = 0xC0+n;
- writeBufferSPI[n][5] = 0xD0+n;
- writeBufferSPI[n][6] = 0xE0+n;
- writeBufferSPI[n][7] = 0xF0+n;
+ writeBufferSPI[n][5] = 0x00;
+
+ //force
+ writeBufferSPI[n][6] = 0xD0+n;
+ writeBufferSPI[n][7] = 0x00;
}
}
int main()
{
- pc.baud(115200); //115200 //57600
+ pc.baud(115200);
pc.printf("*** Start Slave Main ***\n\r");
logicPin = 0;
- //timer.start();
- //uint64_t begin = 0;
- //uint64_t end = 0;
- //int64_t pauseTime = 0;
+
uint8_t my_val = 0x00; //to read and empty the SPI FIFO buffer
initBufferSPI();
@@ -317,8 +384,6 @@
char rByte = 0x00;
char threeArrows = 0;
- //char closeArrowChar = 0x62; //>
- //bool startReceiving = false;
bool threeArrowsFound = false;
bool slaveSelected = false;
bool checksumReceived = false;
@@ -328,12 +393,17 @@
int j = 0; //byte number
pc.printf("--- Initialise EPOS2 boards ---\n\r");
+
+ /*
for(int i=1; i<=NUMBER_EPOS2_BOARDS; i++)
{
- if(i!=5) initEposBoard(i);
+ initEposBoard(i);
}
+ */
+
+ //initEposBoard(0);
- ledchain[0] = 1;
+ //ledchain[0] = 1;
pc.printf("--- Enable Interrupts ---\n\r");
//attach the interrupt function
@@ -343,67 +413,55 @@
device.reply(0x62); //Prime SPI with first reply
//gather first pack of data
- //get the sensor values
- for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
- {
- uint8_t node_id = i+1;
- if(node_id!=5)
- {
- getPosition(node_id);
- wait_us(300);
- getCurrent(node_id);
- wait_us(300);
- //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);
- }
- }
+ //get the sensor values
+ //updateTxSPIBuffer();
- //build the motorDataSet_msg
- for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
- {
- uint8_t node_id = i+1;
- if(node_id!=5)
- {
- //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
-
- //motorDataSet_msg.motorData[i].encPosition = encPosition[node_id-1];
- //motorDataSet_msg.motorData[i].current = avgCurrent[node_id-1];
- //if((node_id >= 2) && (node_id <= 9)) motorDataSet_msg.motorData[i].force = getMedianForceVal(node_id); //medForce[node_id-1];
- }
- }
-
-
+ //update checksum
+ calculateSPIChecksum();
//then start the main loop
pc.printf("--- Start main loop ---\n\r");
-
- setModeOfOperationPDO(1, VALUE_POSITION_MODE);
- //setCurrent(1, 150);
while(1)
- {
+ {
+ ledchain[0] = 0; //not selected by master
+ ledchain[3] = 0; //no commands played
+
//wait, the master will put the pin high at some point, for 10us
while(sync_master == 0)
{
+ // logicPin = 1;
wait_us(1);
+ // logicPin = 0;
}
slaveSelected = true;
+ ledchain[0] = 1;
- while (LPC_SSP1->SR & RNE) // While RNE-Bit = 1 (FIFO receive buffer not empty)...
+ while(LPC_SSP1->SR & RNE) // While RNE-Bit = 1 (FIFO receive buffer not empty)...
my_val = LPC_SSP1->DR; // Read the byte in the buffer
-
+ /*
+ {
+ my_val = LPC_SSP1->DR; // Read the byte in the buffer
+
+ logicPin = 1;
+ wait_us(1);
+ logicPin = 0;
+ }
+ */
//reset for a new message
i = 0;
j = 0;
threeArrows = 0;
threeArrowsFound = false;
+ checksumReceived = false;
logicPin = 1;
__disable_irq();
while(slaveSelected)
- {
+ {
//SPI polling
if(device.receive())
{
@@ -461,6 +519,10 @@
i = 0;
j = 0;
slaveSelected = false; //to end the while loop
+ /*
+ logicPin = 1;
+ wait_us(1);
+ logicPin = 0;*/
}
}
@@ -471,104 +533,62 @@
wait_us(1);
}//while slaveSelected
-
+
+ //wait_us(30);
+ //logicPin = 1;
//read the checksum
while(!checksumReceived)
{
+ //pc.printf("w");
if(device.receive())
{
cmdChecksum = device.read();
+ //pc.printf("cmdChecksum 0x%02X\n", cmdChecksum);
cmdValid = verifyChecksum();
checksumReceived = true; //exit while loop
}
wait_us(1);
- }
-
- __enable_irq();
+ }
- logicPin = 0;
+
+ __enable_irq();
+
+ logicPin = 0;
+
+ /*
+ wait_us(30);
+ logicPin = 1;
wait_us(10);
- logicPin = 1;
-
+ logicPin = 0;
+ wait_us(20);
+ */
//if checksum is correct, then play the cmds
if(cmdValid)
{
- //play the commands
- for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
- {
- uint8_t node_ID = readBufferSPI[i][0];
- uint8_t node_mode = readBufferSPI[i][1];
- int position = readBufferSPI[i][2] + (readBufferSPI[i][3]<<8) + (readBufferSPI[i][4]<<16) + (readBufferSPI[i][5]<<24);
-
- //uint8_t cmd_delay = readBufferSPI[i][6]; //TODO delay
-
- //pc.printf("Cmd[%d][%d][%d][%d]\n", node_ID, node_mode, position, cmd_delay);
-
- if(node_mode == 1)
- setPosition(node_ID, position); //test
-
- wait_us(10);
- }
-
+ logicPin = 1;
+ //play the commands
+ commandPlayer();
cmdValid = false; //reset for next packet
+ logicPin = 0;
}
-
- logicPin = 0;
+ /*
+ wait_us(20);
+ logicPin = 1;
wait_us(10);
- logicPin = 1;
-
+ logicPin = 0;
+ */
+ //get the sensor values ready to be sent for the next loop
+ //update the writeBufferSPI
+ //updateTxSPIBuffer(); //test with known data first
- //get the sensor values
- for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
- {
- uint8_t node_id = i+1;
- //if(node_id!=5)
- //{
- getPosition(node_id);
- wait_us(200);
- getCurrent(node_id);
- wait_us(200);
- //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);
- //}
- }
-
- //update the writeBufferSPI
-
- //compute checksum
-
-
- /*
- //build the motorDataSet_msg
- for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
- {
- uint8_t node_id = i+1;
- //if(node_id!=5)
- //{
- //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
-
- //motorDataSet_msg.motorData[i].encPosition = encPosition[node_id-1];
- //motorDataSet_msg.motorData[i].current = avgCurrent[node_id-1];
- //if((node_id >= 2) && (node_id <= 9)) motorDataSet_msg.motorData[i].force = getMedianForceVal(node_id); //medForce[node_id-1];
- //}
- }
- */
- logicPin = 0;
+ //compute checksum
+ calculateSPIChecksum();
- wait_us(10);
-
- /*
- //disable interrupts and publish it
- __disable_irq();
- motorDataSetPub.publish(&motorDataSet_msg);
- //this check if there are some msg published on the topics, and excecute the cb functions
- nh.spinOnce();
- __enable_irq();
- */
- //this will excecute cmds of the array that are ready (delay 0)
- //commandPlayer();
-
+ //logicPin = 0;
+ //ledchain[0] = 0;
+ wait_us(10);
}// main while end
}// main end
-#endif //COMPILE_MAIN_CODE_ROSSERIAL
+#endif //COMPILE_MAIN_CODE_TRS_SLAVE