TheRobotStudio ROSA
Code for the mbed NXP LPC1768 To be used on The Robot Studio Slave Boards License : Simplified BSD
Diff: main.cpp
- Revision:
- 1:b430b4401fc4
- Parent:
- 0:18d7499b82f3
- Child:
- 2:7ab1d5918efe
--- a/main.cpp Wed Feb 27 14:53:45 2013 +0000
+++ b/main.cpp Wed Mar 06 17:07:47 2013 +0000
@@ -4,6 +4,10 @@
//include files
#include "include/eposCmd.h"
+#define OPEN_ARROW 0x3C //< = 60
+#define CLOSE_ARROW 0x3E //< = 62
+#define NUMBER_OF_ARROWS 5
+
//SPI RxTx FIFO bits
#define TNF 0x02
#define TFE 0x01
@@ -13,14 +17,13 @@
DigitalOut myled(LED1);
DigitalIn sync_master(p25);
-//trs_mbed::MotorCommand motorCmdBuffer[CMD_BUFFER_SIZE];
+DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
+//Timer timer;
+char dataChecksum = 0x00;
+char cmdChecksum = 0x00;
-DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
-Timer timer;
-
-
-uint8_t writeBufferSPI[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG];
-uint8_t readBufferSPI[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG];
+uint8_t writeBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
+uint8_t readBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
int counter = 0;
@@ -65,6 +68,22 @@
}
*/
+bool verifyChecksum()
+{
+ for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
+ {
+ for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
+ {
+ cmdChecksum += readBufferSPI[i][j];
+ }
+ }
+
+ cmdChecksum++; //add 1 to obtain 0x00
+
+ if(cmdChecksum == 0x00) return true;
+ else return false;
+}
+
int16_t getMedianForceVal(const int8_t nodeID)
{
logicPin = 1;
@@ -183,7 +202,7 @@
}
else if(regData == 0x0060414B) //read Statusword
{
- int32_t swData = 0x00000000;
+ //int32_t swData = 0x00000000;
//pc.printf("Node %d - Statusword [%02X]\n", nodeID, canmsg.data[4]);
//pc.printf("Statusword frame : [%02X] [%02X %02X %02X %02X %02X %02X %02X %02X]\n", canmsg.id, canmsg.data[7], canmsg.data[6], canmsg.data[5], canmsg.data[4], canmsg.data[3], canmsg.data[2], canmsg.data[1], canmsg.data[0]);
@@ -256,7 +275,7 @@
void initBufferSPI()
{
//init the SPI arrays
- 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++)
{
@@ -265,16 +284,19 @@
}
}
- for(int n=0; n<NUMBER_MSG_PER_PACKET; n++)
+ for(int n=0; n<NUMBER_MAX_EPOS2_PER_SLAVE; n++)
{
- writeBufferSPI[n][0] = n+1; //CANnodeID
- writeBufferSPI[n][1] = 2; //mode of command
+ //position
+ writeBufferSPI[n][0] = 0x60+n;
+ writeBufferSPI[n][1] = 0x70+n;
+ writeBufferSPI[n][2] = 0x80+n;
+ writeBufferSPI[n][3] = 0x90+n;
- writeBufferSPI[n][2] = 0xA0+n;
- writeBufferSPI[n][3] = 0xB0+n;
+ //current
writeBufferSPI[n][4] = 0xC0+n;
writeBufferSPI[n][5] = 0xD0+n;
- writeBufferSPI[n][6] = 0xE0+n;
+ writeBufferSPI[n][6] = 0xE0+n;
+ writeBufferSPI[n][7] = 0xF0+n;
}
}
@@ -284,21 +306,23 @@
pc.printf("*** Start Slave Main ***\n\r");
logicPin = 0;
- timer.start();
- uint64_t begin = 0;
- uint64_t end = 0;
- int64_t pauseTime = 0;
- //int8_t node = 0; //test
+ //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();
//sync_master = 0;
char rByte = 0x00;
char threeArrows = 0;
- char closeArrowChar = 0x62; //>
- bool startReceiving = false;
+ //char closeArrowChar = 0x62; //>
+ //bool startReceiving = false;
bool threeArrowsFound = false;
bool slaveSelected = false;
+ bool checksumReceived = false;
+ bool cmdValid = false;
int i = 0; //msg
int j = 0; //byte number
@@ -317,31 +341,6 @@
__enable_irq();
device.reply(0x62); //Prime SPI with first reply
-
- /*
- pc.printf("--- Get poti positions for calibration...");
- //set the nodeID and potiPosition field of motorDataSet_msg
- for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
- {
- uint8_t node_id = i+1;
- if(node_id!=5)
- {
- motorDataSet_msg.motorData[i].encPosition = 0;
- motorDataSet_msg.motorData[i].potiPosition = getMedianPotiVal(node_id);
- motorDataSet_msg.motorData[i].current = 0 ;
- motorDataSet_msg.motorData[i].force = -1;
- }
- else
- {
- motorDataSet_msg.motorData[i].encPosition = 0;
- motorDataSet_msg.motorData[i].potiPosition = 9999;
- motorDataSet_msg.motorData[i].current = 0 ;
- motorDataSet_msg.motorData[i].force = -1;
- }
- }
-
- pc.printf("...OK\n\r");
-*/
//gather first pack of data
//get the sensor values
@@ -377,20 +376,11 @@
//then start the main loop
pc.printf("--- Start main loop ---\n\r");
- //setModeOfOperationPDO(1, VALUE_CURRENT_MODE);
+ setModeOfOperationPDO(1, VALUE_POSITION_MODE);
//setCurrent(1, 150);
-
- uint8_t my_val;
-
+
while(1)
- {
- //begin = timer.read_us();
- //logicPin = 1;
-
- //pc.printf("1\n\r");
-
-
-
+ {
//wait, the master will put the pin high at some point, for 10us
while(sync_master == 0)
{
@@ -408,9 +398,7 @@
threeArrows = 0;
threeArrowsFound = false;
- logicPin = 1;
-
- //pc.printf("START - ");
+ logicPin = 1;
__disable_irq();
@@ -418,159 +406,156 @@
{
//SPI polling
if(device.receive())
- {/*
- //pc.printf("startReceiving = true\n\r");
- startReceiving = true;
- //logicPin = 1;
- rByte = device.read();
+ {
+ rByte = device.read(); // Read byte from master
//pc.printf("0x%02X ", rByte);
- //logicPin = 0;
- if(rByte == 0x60)
- {
- threeArrows++;
- //pc.printf("3Arrows++ before while\n\r");
- }
-
- device.reply(0x00);
- wait_us(1);
-
- while(startReceiving)
+ if(threeArrows < 3)
{
- //pc.printf("2\n");
- if(device.receive())
- {*/
- //logicPin = 1;
- rByte = device.read(); // Read byte from master
- //pc.printf("0x%02X ", rByte);
- //wait_ms(100);
- //logicPin = 0;
+ if(rByte == OPEN_ARROW)
+ {
+ threeArrows++;
+ //pc.printf("3A++\n\r");
+ }
+ else
+ {
+ //threeArrows = 0;
+ //startReceiving = false;
+ //pc.printf("error3A\n");
+ //slaveSelected = false;
+ }
+
+ if(threeArrows == 3)
+ {
+ device.reply(writeBufferSPI[i][j]);
+ threeArrowsFound = true;
+ }
+ else
+ {
+ device.reply(0x62); //close arrow : >
+ }
+ }
+ else
+ {
+ readBufferSPI[i][j] = rByte;
+
+ j++; //write next byte next time
+
+ if(j >= NUMBER_BYTES_PER_MSG)
+ {
+ j = 0;
+ i++; //next node
- if(threeArrows < 3)
- {
- if(rByte == 0x60)
- {
- threeArrows++;
- //pc.printf("3A++\n\r");
- }
- else
- {
- //threeArrows = 0;
- //startReceiving = false;
- //pc.printf("error3A\n");
- //slaveSelected = false;
- }
-
- if(threeArrows == 3)
- {
- device.reply(writeBufferSPI[i][j]);
- threeArrowsFound = true;
- }
- else
+ if(i >= NUMBER_MAX_EPOS2_PER_SLAVE)
+ {
+ //finished reading the array
+ /*
+ for(int n=0; n<1; n++)
{
- device.reply(0x62); //close arrow : >
- }
-
- //pc.printf("3Arrows %d \n", threeArrows);
- //if(threeArrows == 3) threeArrowsFound = true;
- }
- else
- {/*
- if(threeArrowsFound) //to reset i and j when receiving a new message
- {
- //pc.printf("3AFound\n\r");
- i = 0;
- j = 0;
- threeArrowsFound = false;
- }*/
-
- //logicPin = 1;
- readBufferSPI[i][j] = rByte;
- //pc.printf("i=%d j=%d\n", i,j);
- //logicPin = 0;
-
- j++; //write next byte next time
-
- if(j >= NUMBER_BYTES_PER_MSG)
- {
- j = 0;
- i++; //next node
-
- if(i >= NUMBER_MSG_PER_PACKET)
- {
- //pc.printf("\n\r");
- //pc.printf("final i=%d j=%d\n\r", i,j);
- //finished reading the array
- /*
- for(int n=0; n<1; n++)
- {
- pc.printf("0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n", readBufferSPI[n][0], readBufferSPI[n][1], readBufferSPI[n][2], readBufferSPI[n][3], readBufferSPI[n][4], readBufferSPI[n][5], readBufferSPI[n][6]);
- }
-
- pc.printf("\n\r");
- */
-
- //reset
- threeArrows = 0;
- i = 0;
- j = 0;
- //startReceiving = false;
- //mbedID_ok = false;
- slaveSelected = false; //to end the while loop
- }
- }
-
- device.reply(writeBufferSPI[i][j]);
- }
-
- //logicPin = 0;
- //}//if
-
- //wait_us(1);
- //}//while startReceiving
+ pc.printf("0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n", readBufferSPI[n][0], readBufferSPI[n][1], readBufferSPI[n][2], readBufferSPI[n][3], readBufferSPI[n][4], readBufferSPI[n][5], readBufferSPI[n][6]);
+ }
+ */
+
+ //reset
+ threeArrows = 0;
+ i = 0;
+ j = 0;
+ slaveSelected = false; //to end the while loop
+ }
+ }
+
+ if(slaveSelected) device.reply(writeBufferSPI[i][j]);
+ else device.reply(dataChecksum); //checksum
+ }
}//if
wait_us(1);
}//while slaveSelected
-
+
+ //read the checksum
+ while(!checksumReceived)
+ {
+ if(device.receive())
+ {
+ cmdChecksum = device.read();
+ cmdValid = verifyChecksum();
+ checksumReceived = true; //exit while loop
+ }
+
+ wait_us(1);
+ }
+
__enable_irq();
- //pc.printf(" - END\n\r");
- logicPin = 0;
+ logicPin = 0;
wait_us(10);
logicPin = 1;
+ //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);
+ }
+
+ cmdValid = false; //reset for next packet
+ }
+
+ logicPin = 0;
+ wait_us(10);
+ logicPin = 1;
+
+
//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!=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)
- {
+ //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;
- wait_us(1);
+ wait_us(10);
/*
//disable interrupts and publish it
@@ -579,23 +564,10 @@
//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;
- /*
- //pc.printf("before end\n\r");
- end = timer.read_us();
- pc.printf("end=%d\n", end);
- pauseTime = LOOP_PERIOD_TIME - end + begin - 12; //12us is the time to compute those 2 lines
- //pc.printf("pauseTime=%d\n", pauseTime);
- pc.printf("begin = %d - pause = %d, end = %d\n", begin, pauseTime, end);
- pc.printf("pauseTime=%d\n", pauseTime);
- if(pauseTime > 0) wait_us(pauseTime);
- */
- //pc.printf("after end\n\r");
+ //commandPlayer();
+
}// main while end
}// main end