TheRobotStudio ROSA
Code for the mbed NXP LPC1768 To be used on The Robot Studio Slave Boards License : Simplified BSD
Diff: main.cpp
- Revision:
- 0:18d7499b82f3
- Child:
- 1:b430b4401fc4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Feb 27 14:53:45 2013 +0000
@@ -0,0 +1,602 @@
+#define COMPILE_MAIN_CODE_ROSSERIAL
+#ifdef COMPILE_MAIN_CODE_ROSSERIAL
+
+//include files
+#include "include/eposCmd.h"
+
+//SPI RxTx FIFO bits
+#define TNF 0x02
+#define TFE 0x01
+#define RNE 0x04
+
+SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
+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;
+
+
+uint8_t writeBufferSPI[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG];
+uint8_t readBufferSPI[NUMBER_MSG_PER_PACKET][NUMBER_BYTES_PER_MSG];
+
+int counter = 0;
+
+//5 variables for median poti position
+//int16_t potiVal[NB_SAMPLES_MEDIAN];
+//int16_t sortVal[NB_SAMPLES_MEDIAN];
+
+int16_t forceVal[NB_SAMPLES_MEDIAN];
+int16_t sortForceVal[NB_SAMPLES_MEDIAN];
+//int16_t lastForceVal[NUMBER_EPOS2_BOARDS][NB_SAMPLES_MEDIAN];
+
+/*
+int16_t getMedianPotiVal(const int8_t nodeID)
+{
+ //read poti
+ for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
+ {
+ getPotiPosition(nodeID);
+ wait_us(1000);
+
+ potiVal[m] = potiPosArray[nodeID - 1];
+ sortVal[m] = potiVal[m];
+
+ wait_us(100);
+ }
+
+ //sort values
+ for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
+ {
+ int16_t n;
+ n = sortVal[m];
+ int p;
+
+ for(p = m-1; (p >=0) && (n<sortVal[p]); p--)
+ {
+ sortVal[p+1] = sortVal[p];
+ }
+ sortVal[p+1] = n;
+ }
+
+ return sortVal[2];
+}
+*/
+
+int16_t getMedianForceVal(const int8_t nodeID)
+{
+ logicPin = 1;
+
+ for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
+ {
+ sortForceVal[m] = getForce(nodeID);
+ wait_us(100); //adjust this
+ }
+
+ //sort values
+ for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
+ {
+ int16_t n = sortForceVal[m];
+ int p;
+
+ for(p = m-1; (p >=0) && (n<sortForceVal[p]); p--)
+ {
+ sortForceVal[p+1] = sortForceVal[p];
+ }
+ sortForceVal[p+1] = n;
+ }
+
+ logicPin = 0;
+
+ return sortForceVal[2];
+}
+
+/*** INTERRUPT (for catching Emergency error frames) ***/
+void interrupt()
+{
+ CANMessage canmsg;
+ int64_t data = 0x0000000000000000;
+ int8_t nodeID = 0;
+ int16_t cobID = 0;
+
+ //read the can message that has triggered the interrupt
+ cantoepos.read(canmsg);
+
+ //pc.printf("Interrupt 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]);
+
+ nodeID = 0x00F & canmsg.id;
+ cobID = 0x0FF0 & canmsg.id;
+
+ for(int i=0; i<=canmsg.len; i++)
+ {
+ data = data | (canmsg.data[i]<<i*8);
+ }
+
+ //check nodeID first
+ if((nodeID >= 0) && (nodeID <= NUMBER_EPOS2_BOARDS))
+ {
+ switch(cobID)
+ {
+ case COB_ID_TRANSMIT_PDO_1_ENABLE : //getPosition
+ //pc.printf("getPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
+ encPosition[nodeID - 1] = data;
+ break;
+
+ case COB_ID_TRANSMIT_PDO_2_ENABLE : //getCurrent averaged
+ //pc.printf("getCurrent Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
+ avgCurrent[nodeID - 1] = data;
+ break;
+
+ case COB_ID_TRANSMIT_PDO_3_ENABLE : //getVelocity
+ //pc.printf("getVelocity Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
+ velocity[nodeID - 1] = data;
+ break;
+
+ case COB_ID_TRANSMIT_PDO_4_ENABLE : //getPotiPosition
+ //pc.printf("getPotiPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
+ potiPosArray[nodeID - 1] = data;
+ break;
+
+ case COB_ID_EMCY_DEFAULT : //Emergency frame
+ //pc.printf("Emergency frame, Node ID : [%d], PDO COB-ID [%02X], data = %02X\n", nodeID, cobID, data);
+ //pc.printf("EF [%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]);
+ pc.printf("EF%02X-%02X%02X\n\r", canmsg.id, canmsg.data[1], canmsg.data[0]);
+ ledchain[1] = 1;
+ //nh.logerror("Emergency frame");
+ boardStatus[nodeID-1] = 1;
+ //first step : fault reset on controlword
+ //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
+ faultResetControlword(nodeID);
+ break;
+
+ case COB_ID_SDO_SERVER_TO_CLIENT_DEFAULT : //SDO Acknoledgement frame
+ int32_t regData = 0x00000000;
+ regData = (int32_t)data;
+
+ //pc.printf("Node %d - regData [%02X]\n", nodeID, regData);
+
+ if(regData == 0x00604060) //Controlword
+ {
+ if(boardStatus[nodeID-1] == 1)
+ {
+ boardStatus[nodeID-1] = 2;
+ //second step : shutdown controlword
+ //pc.printf("Node %d - STEP 2 - shutdownControlwordIT\n", nodeID);
+ shutdownControlwordIT(nodeID);
+ }
+ else if(boardStatus[nodeID-1] == 2)
+ {
+ boardStatus[nodeID-1] = 3;
+ //third step : Switch On & Enable Operation on Controlword
+ //pc.printf("Node %d - STEP 3 - switchOnEnableOperationControlwordIT\n", nodeID);
+ switchOnEnableOperationControlwordIT(nodeID);
+ }
+ else if(boardStatus[nodeID-1] == 3)
+ {
+ boardStatus[nodeID-1] = 4;
+ //ask for statusword to check if the board has reset well
+ //pc.printf("Node %d - STEP 4 - getStatusword\n", nodeID);
+ getStatusword(nodeID);
+ }
+ }
+ else if(regData == 0x0060414B) //read Statusword
+ {
+ 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]);
+
+ if(boardStatus[nodeID-1] == 4)
+ {
+ //swData = data >> 32;
+ int8_t fault = 0x00;
+ fault = (canmsg.data[4] & 0x08) >> 3;
+
+ if(fault == 0) //reset OK
+ {
+ boardStatus[nodeID-1] = 0; //Board is reset and enable OK
+ pc.printf("%d OK\n\r", nodeID);
+ ledchain[1] = 0;
+ }
+ else //try to reset again
+ {
+ //pc.printf("Node %d - try to reset again\n", nodeID);
+ boardStatus[nodeID-1] = 1;
+ //go back to first step : fault reset on controlword
+ //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
+ faultResetControlword(nodeID);
+ }
+ }
+ }
+ break;
+
+ default :
+ pc.printf("Unknown frame [%02X][%02X %02X %02X %02X %02X %02X %02X %02X]\n\r", 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]);
+ } //end switch
+ }
+ else
+ {
+ pc.printf("NODEID ERROR\n\r");
+ }
+} //end interrupt
+
+void commandPlayer() //called in main every 20ms
+{/*
+ for(int i= 0; i<CMD_BUFFER_SIZE; i++)
+ {
+ if(motorCmdBuffer[i].beatDelay != -1)
+ {
+ switch (motorCmdBuffer[i].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);
+ 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);
+ 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);
+ break;
+ default:
+ break;
+ }
+
+ //decrement only cmd with delay != -1, so cmd that has not been played yet.
+ motorCmdBuffer[i].beatDelay = motorCmdBuffer[i].beatDelay - 1;
+ }
+ }*/
+}
+
+void initBufferSPI()
+{
+ //init the SPI arrays
+ for(int i=0; i<NUMBER_MSG_PER_PACKET; i++)
+ {
+ for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
+ {
+ //writeBufferSPI[i][j] = 0x00;
+ readBufferSPI[i][j] = 0x00;
+ }
+ }
+
+ for(int n=0; n<NUMBER_MSG_PER_PACKET; n++)
+ {
+ writeBufferSPI[n][0] = n+1; //CANnodeID
+ writeBufferSPI[n][1] = 2; //mode of command
+
+ writeBufferSPI[n][2] = 0xA0+n;
+ writeBufferSPI[n][3] = 0xB0+n;
+ writeBufferSPI[n][4] = 0xC0+n;
+ writeBufferSPI[n][5] = 0xD0+n;
+ writeBufferSPI[n][6] = 0xE0+n;
+ }
+}
+
+int main()
+{
+ pc.baud(115200); //115200 //57600
+ 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
+
+ initBufferSPI();
+ //sync_master = 0;
+ char rByte = 0x00;
+
+ char threeArrows = 0;
+ char closeArrowChar = 0x62; //>
+ bool startReceiving = false;
+ bool threeArrowsFound = false;
+ bool slaveSelected = false;
+
+ int i = 0; //msg
+ 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);
+ }
+
+ ledchain[0] = 1;
+
+ pc.printf("--- Enable Interrupts ---\n\r");
+ //attach the interrupt function
+ cantoepos.attach(&interrupt);
+ __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
+ 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);
+ }
+ }
+
+ //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];
+ }
+ }
+
+
+
+ //then start the main loop
+ pc.printf("--- Start main loop ---\n\r");
+
+ //setModeOfOperationPDO(1, VALUE_CURRENT_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)
+ {
+ wait_us(1);
+ }
+
+ slaveSelected = true;
+
+ 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
+
+ //reset for a new message
+ i = 0;
+ j = 0;
+ threeArrows = 0;
+ threeArrowsFound = false;
+
+ logicPin = 1;
+
+ //pc.printf("START - ");
+
+ __disable_irq();
+
+ while(slaveSelected)
+ {
+ //SPI polling
+ if(device.receive())
+ {/*
+ //pc.printf("startReceiving = true\n\r");
+ startReceiving = true;
+ //logicPin = 1;
+ rByte = device.read();
+ //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)
+ {
+ //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(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
+ {
+ 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
+ }//if
+
+ wait_us(1);
+ }//while slaveSelected
+
+ __enable_irq();
+
+ //pc.printf(" - END\n\r");
+ 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 >= 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;
+ 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);
+
+ /*
+ //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;
+ /*
+ //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");
+ }// main while end
+}// main end
+
+#endif //COMPILE_MAIN_CODE_ROSSERIAL