TheRobotStudio ROSA / Mbed 2 deprecated trs_slave

Code for the mbed NXP LPC1768 To be used on The Robot Studio Slave Boards License : Simplified BSD

Dependencies:   mbed

main.cpp@1:b430b4401fc4, 2013-03-06 (annotated)

Committer:
rrknight
Date:
Wed Mar 06 17:07:47 2013 +0000
Revision:
1:b430b4401fc4
Parent:
0:18d7499b82f3
Child:
2:7ab1d5918efe
hannover work in progress

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rrknight 0:18d7499b82f3 1 #define COMPILE_MAIN_CODE_ROSSERIAL
rrknight 0:18d7499b82f3 2 #ifdef COMPILE_MAIN_CODE_ROSSERIAL
rrknight 0:18d7499b82f3 3
rrknight 0:18d7499b82f3 4 //include files
rrknight 0:18d7499b82f3 5 #include "include/eposCmd.h"
rrknight 0:18d7499b82f3 6
rrknight 1:b430b4401fc4 7 #define OPEN_ARROW 0x3C //< = 60
rrknight 1:b430b4401fc4 8 #define CLOSE_ARROW 0x3E //< = 62
rrknight 1:b430b4401fc4 9 #define NUMBER_OF_ARROWS 5
rrknight 1:b430b4401fc4 10
rrknight 0:18d7499b82f3 11 //SPI RxTx FIFO bits
rrknight 0:18d7499b82f3 12 #define TNF 0x02
rrknight 0:18d7499b82f3 13 #define TFE 0x01
rrknight 0:18d7499b82f3 14 #define RNE 0x04
rrknight 0:18d7499b82f3 15
rrknight 0:18d7499b82f3 16 SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
rrknight 0:18d7499b82f3 17 DigitalOut myled(LED1);
rrknight 0:18d7499b82f3 18 DigitalIn sync_master(p25);
rrknight 0:18d7499b82f3 19
rrknight 1:b430b4401fc4 20 DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
rrknight 1:b430b4401fc4 21 //Timer timer;
rrknight 1:b430b4401fc4 22 char dataChecksum = 0x00;
rrknight 1:b430b4401fc4 23 char cmdChecksum = 0x00;
rrknight 0:18d7499b82f3 24
rrknight 1:b430b4401fc4 25 uint8_t writeBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
rrknight 1:b430b4401fc4 26 uint8_t readBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
rrknight 0:18d7499b82f3 27
rrknight 0:18d7499b82f3 28 int counter = 0;
rrknight 0:18d7499b82f3 29
rrknight 0:18d7499b82f3 30 //5 variables for median poti position
rrknight 0:18d7499b82f3 31 //int16_t potiVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 32 //int16_t sortVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 33
rrknight 0:18d7499b82f3 34 int16_t forceVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 35 int16_t sortForceVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 36 //int16_t lastForceVal[NUMBER_EPOS2_BOARDS][NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 37
rrknight 0:18d7499b82f3 38 /*
rrknight 0:18d7499b82f3 39 int16_t getMedianPotiVal(const int8_t nodeID)
rrknight 0:18d7499b82f3 40 {
rrknight 0:18d7499b82f3 41 //read poti
rrknight 0:18d7499b82f3 42 for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
rrknight 0:18d7499b82f3 43 {
rrknight 0:18d7499b82f3 44 getPotiPosition(nodeID);
rrknight 0:18d7499b82f3 45 wait_us(1000);
rrknight 0:18d7499b82f3 46
rrknight 0:18d7499b82f3 47 potiVal[m] = potiPosArray[nodeID - 1];
rrknight 0:18d7499b82f3 48 sortVal[m] = potiVal[m];
rrknight 0:18d7499b82f3 49
rrknight 0:18d7499b82f3 50 wait_us(100);
rrknight 0:18d7499b82f3 51 }
rrknight 0:18d7499b82f3 52
rrknight 0:18d7499b82f3 53 //sort values
rrknight 0:18d7499b82f3 54 for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
rrknight 0:18d7499b82f3 55 {
rrknight 0:18d7499b82f3 56 int16_t n;
rrknight 0:18d7499b82f3 57 n = sortVal[m];
rrknight 0:18d7499b82f3 58 int p;
rrknight 0:18d7499b82f3 59
rrknight 0:18d7499b82f3 60 for(p = m-1; (p >=0) && (n<sortVal[p]); p--)
rrknight 0:18d7499b82f3 61 {
rrknight 0:18d7499b82f3 62 sortVal[p+1] = sortVal[p];
rrknight 0:18d7499b82f3 63 }
rrknight 0:18d7499b82f3 64 sortVal[p+1] = n;
rrknight 0:18d7499b82f3 65 }
rrknight 0:18d7499b82f3 66
rrknight 0:18d7499b82f3 67 return sortVal[2];
rrknight 0:18d7499b82f3 68 }
rrknight 0:18d7499b82f3 69 */
rrknight 0:18d7499b82f3 70
rrknight 1:b430b4401fc4 71 bool verifyChecksum()
rrknight 1:b430b4401fc4 72 {
rrknight 1:b430b4401fc4 73 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 1:b430b4401fc4 74 {
rrknight 1:b430b4401fc4 75 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 1:b430b4401fc4 76 {
rrknight 1:b430b4401fc4 77 cmdChecksum += readBufferSPI[i][j];
rrknight 1:b430b4401fc4 78 }
rrknight 1:b430b4401fc4 79 }
rrknight 1:b430b4401fc4 80
rrknight 1:b430b4401fc4 81 cmdChecksum++; //add 1 to obtain 0x00
rrknight 1:b430b4401fc4 82
rrknight 1:b430b4401fc4 83 if(cmdChecksum == 0x00) return true;
rrknight 1:b430b4401fc4 84 else return false;
rrknight 1:b430b4401fc4 85 }
rrknight 1:b430b4401fc4 86
rrknight 0:18d7499b82f3 87 int16_t getMedianForceVal(const int8_t nodeID)
rrknight 0:18d7499b82f3 88 {
rrknight 0:18d7499b82f3 89 logicPin = 1;
rrknight 0:18d7499b82f3 90
rrknight 0:18d7499b82f3 91 for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
rrknight 0:18d7499b82f3 92 {
rrknight 0:18d7499b82f3 93 sortForceVal[m] = getForce(nodeID);
rrknight 0:18d7499b82f3 94 wait_us(100); //adjust this
rrknight 0:18d7499b82f3 95 }
rrknight 0:18d7499b82f3 96
rrknight 0:18d7499b82f3 97 //sort values
rrknight 0:18d7499b82f3 98 for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
rrknight 0:18d7499b82f3 99 {
rrknight 0:18d7499b82f3 100 int16_t n = sortForceVal[m];
rrknight 0:18d7499b82f3 101 int p;
rrknight 0:18d7499b82f3 102
rrknight 0:18d7499b82f3 103 for(p = m-1; (p >=0) && (n<sortForceVal[p]); p--)
rrknight 0:18d7499b82f3 104 {
rrknight 0:18d7499b82f3 105 sortForceVal[p+1] = sortForceVal[p];
rrknight 0:18d7499b82f3 106 }
rrknight 0:18d7499b82f3 107 sortForceVal[p+1] = n;
rrknight 0:18d7499b82f3 108 }
rrknight 0:18d7499b82f3 109
rrknight 0:18d7499b82f3 110 logicPin = 0;
rrknight 0:18d7499b82f3 111
rrknight 0:18d7499b82f3 112 return sortForceVal[2];
rrknight 0:18d7499b82f3 113 }
rrknight 0:18d7499b82f3 114
rrknight 0:18d7499b82f3 115 /*** INTERRUPT (for catching Emergency error frames) ***/
rrknight 0:18d7499b82f3 116 void interrupt()
rrknight 0:18d7499b82f3 117 {
rrknight 0:18d7499b82f3 118 CANMessage canmsg;
rrknight 0:18d7499b82f3 119 int64_t data = 0x0000000000000000;
rrknight 0:18d7499b82f3 120 int8_t nodeID = 0;
rrknight 0:18d7499b82f3 121 int16_t cobID = 0;
rrknight 0:18d7499b82f3 122
rrknight 0:18d7499b82f3 123 //read the can message that has triggered the interrupt
rrknight 0:18d7499b82f3 124 cantoepos.read(canmsg);
rrknight 0:18d7499b82f3 125
rrknight 0:18d7499b82f3 126 //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]);
rrknight 0:18d7499b82f3 127
rrknight 0:18d7499b82f3 128 nodeID = 0x00F & canmsg.id;
rrknight 0:18d7499b82f3 129 cobID = 0x0FF0 & canmsg.id;
rrknight 0:18d7499b82f3 130
rrknight 0:18d7499b82f3 131 for(int i=0; i<=canmsg.len; i++)
rrknight 0:18d7499b82f3 132 {
rrknight 0:18d7499b82f3 133 data = data | (canmsg.data[i]<<i*8);
rrknight 0:18d7499b82f3 134 }
rrknight 0:18d7499b82f3 135
rrknight 0:18d7499b82f3 136 //check nodeID first
rrknight 0:18d7499b82f3 137 if((nodeID >= 0) && (nodeID <= NUMBER_EPOS2_BOARDS))
rrknight 0:18d7499b82f3 138 {
rrknight 0:18d7499b82f3 139 switch(cobID)
rrknight 0:18d7499b82f3 140 {
rrknight 0:18d7499b82f3 141 case COB_ID_TRANSMIT_PDO_1_ENABLE : //getPosition
rrknight 0:18d7499b82f3 142 //pc.printf("getPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 143 encPosition[nodeID - 1] = data;
rrknight 0:18d7499b82f3 144 break;
rrknight 0:18d7499b82f3 145
rrknight 0:18d7499b82f3 146 case COB_ID_TRANSMIT_PDO_2_ENABLE : //getCurrent averaged
rrknight 0:18d7499b82f3 147 //pc.printf("getCurrent Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 148 avgCurrent[nodeID - 1] = data;
rrknight 0:18d7499b82f3 149 break;
rrknight 0:18d7499b82f3 150
rrknight 0:18d7499b82f3 151 case COB_ID_TRANSMIT_PDO_3_ENABLE : //getVelocity
rrknight 0:18d7499b82f3 152 //pc.printf("getVelocity Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 153 velocity[nodeID - 1] = data;
rrknight 0:18d7499b82f3 154 break;
rrknight 0:18d7499b82f3 155
rrknight 0:18d7499b82f3 156 case COB_ID_TRANSMIT_PDO_4_ENABLE : //getPotiPosition
rrknight 0:18d7499b82f3 157 //pc.printf("getPotiPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 158 potiPosArray[nodeID - 1] = data;
rrknight 0:18d7499b82f3 159 break;
rrknight 0:18d7499b82f3 160
rrknight 0:18d7499b82f3 161 case COB_ID_EMCY_DEFAULT : //Emergency frame
rrknight 0:18d7499b82f3 162 //pc.printf("Emergency frame, Node ID : [%d], PDO COB-ID [%02X], data = %02X\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 163 //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]);
rrknight 0:18d7499b82f3 164 pc.printf("EF%02X-%02X%02X\n\r", canmsg.id, canmsg.data[1], canmsg.data[0]);
rrknight 0:18d7499b82f3 165 ledchain[1] = 1;
rrknight 0:18d7499b82f3 166 //nh.logerror("Emergency frame");
rrknight 0:18d7499b82f3 167 boardStatus[nodeID-1] = 1;
rrknight 0:18d7499b82f3 168 //first step : fault reset on controlword
rrknight 0:18d7499b82f3 169 //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
rrknight 0:18d7499b82f3 170 faultResetControlword(nodeID);
rrknight 0:18d7499b82f3 171 break;
rrknight 0:18d7499b82f3 172
rrknight 0:18d7499b82f3 173 case COB_ID_SDO_SERVER_TO_CLIENT_DEFAULT : //SDO Acknoledgement frame
rrknight 0:18d7499b82f3 174 int32_t regData = 0x00000000;
rrknight 0:18d7499b82f3 175 regData = (int32_t)data;
rrknight 0:18d7499b82f3 176
rrknight 0:18d7499b82f3 177 //pc.printf("Node %d - regData [%02X]\n", nodeID, regData);
rrknight 0:18d7499b82f3 178
rrknight 0:18d7499b82f3 179 if(regData == 0x00604060) //Controlword
rrknight 0:18d7499b82f3 180 {
rrknight 0:18d7499b82f3 181 if(boardStatus[nodeID-1] == 1)
rrknight 0:18d7499b82f3 182 {
rrknight 0:18d7499b82f3 183 boardStatus[nodeID-1] = 2;
rrknight 0:18d7499b82f3 184 //second step : shutdown controlword
rrknight 0:18d7499b82f3 185 //pc.printf("Node %d - STEP 2 - shutdownControlwordIT\n", nodeID);
rrknight 0:18d7499b82f3 186 shutdownControlwordIT(nodeID);
rrknight 0:18d7499b82f3 187 }
rrknight 0:18d7499b82f3 188 else if(boardStatus[nodeID-1] == 2)
rrknight 0:18d7499b82f3 189 {
rrknight 0:18d7499b82f3 190 boardStatus[nodeID-1] = 3;
rrknight 0:18d7499b82f3 191 //third step : Switch On & Enable Operation on Controlword
rrknight 0:18d7499b82f3 192 //pc.printf("Node %d - STEP 3 - switchOnEnableOperationControlwordIT\n", nodeID);
rrknight 0:18d7499b82f3 193 switchOnEnableOperationControlwordIT(nodeID);
rrknight 0:18d7499b82f3 194 }
rrknight 0:18d7499b82f3 195 else if(boardStatus[nodeID-1] == 3)
rrknight 0:18d7499b82f3 196 {
rrknight 0:18d7499b82f3 197 boardStatus[nodeID-1] = 4;
rrknight 0:18d7499b82f3 198 //ask for statusword to check if the board has reset well
rrknight 0:18d7499b82f3 199 //pc.printf("Node %d - STEP 4 - getStatusword\n", nodeID);
rrknight 0:18d7499b82f3 200 getStatusword(nodeID);
rrknight 0:18d7499b82f3 201 }
rrknight 0:18d7499b82f3 202 }
rrknight 0:18d7499b82f3 203 else if(regData == 0x0060414B) //read Statusword
rrknight 0:18d7499b82f3 204 {
rrknight 1:b430b4401fc4 205 //int32_t swData = 0x00000000;
rrknight 0:18d7499b82f3 206
rrknight 0:18d7499b82f3 207 //pc.printf("Node %d - Statusword [%02X]\n", nodeID, canmsg.data[4]);
rrknight 0:18d7499b82f3 208 //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]);
rrknight 0:18d7499b82f3 209
rrknight 0:18d7499b82f3 210 if(boardStatus[nodeID-1] == 4)
rrknight 0:18d7499b82f3 211 {
rrknight 0:18d7499b82f3 212 //swData = data >> 32;
rrknight 0:18d7499b82f3 213 int8_t fault = 0x00;
rrknight 0:18d7499b82f3 214 fault = (canmsg.data[4] & 0x08) >> 3;
rrknight 0:18d7499b82f3 215
rrknight 0:18d7499b82f3 216 if(fault == 0) //reset OK
rrknight 0:18d7499b82f3 217 {
rrknight 0:18d7499b82f3 218 boardStatus[nodeID-1] = 0; //Board is reset and enable OK
rrknight 0:18d7499b82f3 219 pc.printf("%d OK\n\r", nodeID);
rrknight 0:18d7499b82f3 220 ledchain[1] = 0;
rrknight 0:18d7499b82f3 221 }
rrknight 0:18d7499b82f3 222 else //try to reset again
rrknight 0:18d7499b82f3 223 {
rrknight 0:18d7499b82f3 224 //pc.printf("Node %d - try to reset again\n", nodeID);
rrknight 0:18d7499b82f3 225 boardStatus[nodeID-1] = 1;
rrknight 0:18d7499b82f3 226 //go back to first step : fault reset on controlword
rrknight 0:18d7499b82f3 227 //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
rrknight 0:18d7499b82f3 228 faultResetControlword(nodeID);
rrknight 0:18d7499b82f3 229 }
rrknight 0:18d7499b82f3 230 }
rrknight 0:18d7499b82f3 231 }
rrknight 0:18d7499b82f3 232 break;
rrknight 0:18d7499b82f3 233
rrknight 0:18d7499b82f3 234 default :
rrknight 0:18d7499b82f3 235 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]);
rrknight 0:18d7499b82f3 236 } //end switch
rrknight 0:18d7499b82f3 237 }
rrknight 0:18d7499b82f3 238 else
rrknight 0:18d7499b82f3 239 {
rrknight 0:18d7499b82f3 240 pc.printf("NODEID ERROR\n\r");
rrknight 0:18d7499b82f3 241 }
rrknight 0:18d7499b82f3 242 } //end interrupt
rrknight 0:18d7499b82f3 243
rrknight 0:18d7499b82f3 244 void commandPlayer() //called in main every 20ms
rrknight 0:18d7499b82f3 245 {/*
rrknight 0:18d7499b82f3 246 for(int i= 0; i<CMD_BUFFER_SIZE; i++)
rrknight 0:18d7499b82f3 247 {
rrknight 0:18d7499b82f3 248 if(motorCmdBuffer[i].beatDelay != -1)
rrknight 0:18d7499b82f3 249 {
rrknight 0:18d7499b82f3 250 switch (motorCmdBuffer[i].mode)
rrknight 0:18d7499b82f3 251 {
rrknight 0:18d7499b82f3 252 case POSITION: //first change modes of motors that will be triggered later
rrknight 0:18d7499b82f3 253 if((activMode[motorCmdBuffer[i].nodeID-1] != POSITION) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_POSITION_MODE);
rrknight 0:18d7499b82f3 254 if(motorCmdBuffer[i].beatDelay == 0) setPosition(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
rrknight 0:18d7499b82f3 255 break;
rrknight 0:18d7499b82f3 256 case CURRENT: //first change modes of motors that will be triggered later (like CURRENT mode needs some time to be active)
rrknight 0:18d7499b82f3 257 //pc.printf("setCurrent(%d, %d)\n", motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
rrknight 0:18d7499b82f3 258 if((activMode[motorCmdBuffer[i].nodeID-1] != CURRENT) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_CURRENT_MODE);
rrknight 0:18d7499b82f3 259 if(motorCmdBuffer[i].beatDelay == 0) setCurrent(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
rrknight 0:18d7499b82f3 260 break;
rrknight 0:18d7499b82f3 261 case VELOCITY: //first change modes of motors that will be triggered later
rrknight 0:18d7499b82f3 262 if((activMode[motorCmdBuffer[i].nodeID-1] != VELOCITY) && (motorCmdBuffer[i].beatDelay == 1)) setModeOfOperationPDO(motorCmdBuffer[i].nodeID, VALUE_VELOCITY_MODE);
rrknight 0:18d7499b82f3 263 if(motorCmdBuffer[i].beatDelay == 0) setVelocity(motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
rrknight 0:18d7499b82f3 264 break;
rrknight 0:18d7499b82f3 265 default:
rrknight 0:18d7499b82f3 266 break;
rrknight 0:18d7499b82f3 267 }
rrknight 0:18d7499b82f3 268
rrknight 0:18d7499b82f3 269 //decrement only cmd with delay != -1, so cmd that has not been played yet.
rrknight 0:18d7499b82f3 270 motorCmdBuffer[i].beatDelay = motorCmdBuffer[i].beatDelay - 1;
rrknight 0:18d7499b82f3 271 }
rrknight 0:18d7499b82f3 272 }*/
rrknight 0:18d7499b82f3 273 }
rrknight 0:18d7499b82f3 274
rrknight 0:18d7499b82f3 275 void initBufferSPI()
rrknight 0:18d7499b82f3 276 {
rrknight 0:18d7499b82f3 277 //init the SPI arrays
rrknight 1:b430b4401fc4 278 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 0:18d7499b82f3 279 {
rrknight 0:18d7499b82f3 280 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 0:18d7499b82f3 281 {
rrknight 0:18d7499b82f3 282 //writeBufferSPI[i][j] = 0x00;
rrknight 0:18d7499b82f3 283 readBufferSPI[i][j] = 0x00;
rrknight 0:18d7499b82f3 284 }
rrknight 0:18d7499b82f3 285 }
rrknight 0:18d7499b82f3 286
rrknight 1:b430b4401fc4 287 for(int n=0; n<NUMBER_MAX_EPOS2_PER_SLAVE; n++)
rrknight 0:18d7499b82f3 288 {
rrknight 1:b430b4401fc4 289 //position
rrknight 1:b430b4401fc4 290 writeBufferSPI[n][0] = 0x60+n;
rrknight 1:b430b4401fc4 291 writeBufferSPI[n][1] = 0x70+n;
rrknight 1:b430b4401fc4 292 writeBufferSPI[n][2] = 0x80+n;
rrknight 1:b430b4401fc4 293 writeBufferSPI[n][3] = 0x90+n;
rrknight 0:18d7499b82f3 294
rrknight 1:b430b4401fc4 295 //current
rrknight 0:18d7499b82f3 296 writeBufferSPI[n][4] = 0xC0+n;
rrknight 0:18d7499b82f3 297 writeBufferSPI[n][5] = 0xD0+n;
rrknight 1:b430b4401fc4 298 writeBufferSPI[n][6] = 0xE0+n;
rrknight 1:b430b4401fc4 299 writeBufferSPI[n][7] = 0xF0+n;
rrknight 0:18d7499b82f3 300 }
rrknight 0:18d7499b82f3 301 }
rrknight 0:18d7499b82f3 302
rrknight 0:18d7499b82f3 303 int main()
rrknight 0:18d7499b82f3 304 {
rrknight 0:18d7499b82f3 305 pc.baud(115200); //115200 //57600
rrknight 0:18d7499b82f3 306 pc.printf("*** Start Slave Main ***\n\r");
rrknight 0:18d7499b82f3 307
rrknight 0:18d7499b82f3 308 logicPin = 0;
rrknight 1:b430b4401fc4 309 //timer.start();
rrknight 1:b430b4401fc4 310 //uint64_t begin = 0;
rrknight 1:b430b4401fc4 311 //uint64_t end = 0;
rrknight 1:b430b4401fc4 312 //int64_t pauseTime = 0;
rrknight 1:b430b4401fc4 313 uint8_t my_val = 0x00; //to read and empty the SPI FIFO buffer
rrknight 0:18d7499b82f3 314
rrknight 0:18d7499b82f3 315 initBufferSPI();
rrknight 0:18d7499b82f3 316 //sync_master = 0;
rrknight 0:18d7499b82f3 317 char rByte = 0x00;
rrknight 0:18d7499b82f3 318
rrknight 0:18d7499b82f3 319 char threeArrows = 0;
rrknight 1:b430b4401fc4 320 //char closeArrowChar = 0x62; //>
rrknight 1:b430b4401fc4 321 //bool startReceiving = false;
rrknight 0:18d7499b82f3 322 bool threeArrowsFound = false;
rrknight 0:18d7499b82f3 323 bool slaveSelected = false;
rrknight 1:b430b4401fc4 324 bool checksumReceived = false;
rrknight 1:b430b4401fc4 325 bool cmdValid = false;
rrknight 0:18d7499b82f3 326
rrknight 0:18d7499b82f3 327 int i = 0; //msg
rrknight 0:18d7499b82f3 328 int j = 0; //byte number
rrknight 0:18d7499b82f3 329
rrknight 0:18d7499b82f3 330 pc.printf("--- Initialise EPOS2 boards ---\n\r");
rrknight 0:18d7499b82f3 331 for(int i=1; i<=NUMBER_EPOS2_BOARDS; i++)
rrknight 0:18d7499b82f3 332 {
rrknight 0:18d7499b82f3 333 if(i!=5) initEposBoard(i);
rrknight 0:18d7499b82f3 334 }
rrknight 0:18d7499b82f3 335
rrknight 0:18d7499b82f3 336 ledchain[0] = 1;
rrknight 0:18d7499b82f3 337
rrknight 0:18d7499b82f3 338 pc.printf("--- Enable Interrupts ---\n\r");
rrknight 0:18d7499b82f3 339 //attach the interrupt function
rrknight 0:18d7499b82f3 340 cantoepos.attach(&interrupt);
rrknight 0:18d7499b82f3 341 __enable_irq();
rrknight 0:18d7499b82f3 342
rrknight 0:18d7499b82f3 343 device.reply(0x62); //Prime SPI with first reply
rrknight 0:18d7499b82f3 344
rrknight 0:18d7499b82f3 345 //gather first pack of data
rrknight 0:18d7499b82f3 346 //get the sensor values
rrknight 0:18d7499b82f3 347 for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
rrknight 0:18d7499b82f3 348 {
rrknight 0:18d7499b82f3 349 uint8_t node_id = i+1;
rrknight 0:18d7499b82f3 350 if(node_id!=5)
rrknight 0:18d7499b82f3 351 {
rrknight 0:18d7499b82f3 352 getPosition(node_id);
rrknight 0:18d7499b82f3 353 wait_us(300);
rrknight 0:18d7499b82f3 354 getCurrent(node_id);
rrknight 0:18d7499b82f3 355 wait_us(300);
rrknight 0:18d7499b82f3 356 //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);
rrknight 0:18d7499b82f3 357 }
rrknight 0:18d7499b82f3 358 }
rrknight 0:18d7499b82f3 359
rrknight 0:18d7499b82f3 360 //build the motorDataSet_msg
rrknight 0:18d7499b82f3 361 for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
rrknight 0:18d7499b82f3 362 {
rrknight 0:18d7499b82f3 363 uint8_t node_id = i+1;
rrknight 0:18d7499b82f3 364 if(node_id!=5)
rrknight 0:18d7499b82f3 365 {
rrknight 0:18d7499b82f3 366 //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
rrknight 0:18d7499b82f3 367
rrknight 0:18d7499b82f3 368 //motorDataSet_msg.motorData[i].encPosition = encPosition[node_id-1];
rrknight 0:18d7499b82f3 369 //motorDataSet_msg.motorData[i].current = avgCurrent[node_id-1];
rrknight 0:18d7499b82f3 370 //if((node_id >= 2) && (node_id <= 9)) motorDataSet_msg.motorData[i].force = getMedianForceVal(node_id); //medForce[node_id-1];
rrknight 0:18d7499b82f3 371 }
rrknight 0:18d7499b82f3 372 }
rrknight 0:18d7499b82f3 373
rrknight 0:18d7499b82f3 374
rrknight 0:18d7499b82f3 375
rrknight 0:18d7499b82f3 376 //then start the main loop
rrknight 0:18d7499b82f3 377 pc.printf("--- Start main loop ---\n\r");
rrknight 0:18d7499b82f3 378
rrknight 1:b430b4401fc4 379 setModeOfOperationPDO(1, VALUE_POSITION_MODE);
rrknight 0:18d7499b82f3 380 //setCurrent(1, 150);
rrknight 1:b430b4401fc4 381
rrknight 0:18d7499b82f3 382 while(1)
rrknight 1:b430b4401fc4 383 {
rrknight 0:18d7499b82f3 384 //wait, the master will put the pin high at some point, for 10us
rrknight 0:18d7499b82f3 385 while(sync_master == 0)
rrknight 0:18d7499b82f3 386 {
rrknight 0:18d7499b82f3 387 wait_us(1);
rrknight 0:18d7499b82f3 388 }
rrknight 0:18d7499b82f3 389
rrknight 0:18d7499b82f3 390 slaveSelected = true;
rrknight 0:18d7499b82f3 391
rrknight 0:18d7499b82f3 392 while (LPC_SSP1->SR & RNE) // While RNE-Bit = 1 (FIFO receive buffer not empty)...
rrknight 0:18d7499b82f3 393 my_val = LPC_SSP1->DR; // Read the byte in the buffer
rrknight 0:18d7499b82f3 394
rrknight 0:18d7499b82f3 395 //reset for a new message
rrknight 0:18d7499b82f3 396 i = 0;
rrknight 0:18d7499b82f3 397 j = 0;
rrknight 0:18d7499b82f3 398 threeArrows = 0;
rrknight 0:18d7499b82f3 399 threeArrowsFound = false;
rrknight 0:18d7499b82f3 400
rrknight 1:b430b4401fc4 401 logicPin = 1;
rrknight 0:18d7499b82f3 402
rrknight 0:18d7499b82f3 403 __disable_irq();
rrknight 0:18d7499b82f3 404
rrknight 0:18d7499b82f3 405 while(slaveSelected)
rrknight 0:18d7499b82f3 406 {
rrknight 0:18d7499b82f3 407 //SPI polling
rrknight 0:18d7499b82f3 408 if(device.receive())
rrknight 1:b430b4401fc4 409 {
rrknight 1:b430b4401fc4 410 rByte = device.read(); // Read byte from master
rrknight 0:18d7499b82f3 411 //pc.printf("0x%02X ", rByte);
rrknight 0:18d7499b82f3 412
rrknight 1:b430b4401fc4 413 if(threeArrows < 3)
rrknight 0:18d7499b82f3 414 {
rrknight 1:b430b4401fc4 415 if(rByte == OPEN_ARROW)
rrknight 1:b430b4401fc4 416 {
rrknight 1:b430b4401fc4 417 threeArrows++;
rrknight 1:b430b4401fc4 418 //pc.printf("3A++\n\r");
rrknight 1:b430b4401fc4 419 }
rrknight 1:b430b4401fc4 420 else
rrknight 1:b430b4401fc4 421 {
rrknight 1:b430b4401fc4 422 //threeArrows = 0;
rrknight 1:b430b4401fc4 423 //startReceiving = false;
rrknight 1:b430b4401fc4 424 //pc.printf("error3A\n");
rrknight 1:b430b4401fc4 425 //slaveSelected = false;
rrknight 1:b430b4401fc4 426 }
rrknight 1:b430b4401fc4 427
rrknight 1:b430b4401fc4 428 if(threeArrows == 3)
rrknight 1:b430b4401fc4 429 {
rrknight 1:b430b4401fc4 430 device.reply(writeBufferSPI[i][j]);
rrknight 1:b430b4401fc4 431 threeArrowsFound = true;
rrknight 1:b430b4401fc4 432 }
rrknight 1:b430b4401fc4 433 else
rrknight 1:b430b4401fc4 434 {
rrknight 1:b430b4401fc4 435 device.reply(0x62); //close arrow : >
rrknight 1:b430b4401fc4 436 }
rrknight 1:b430b4401fc4 437 }
rrknight 1:b430b4401fc4 438 else
rrknight 1:b430b4401fc4 439 {
rrknight 1:b430b4401fc4 440 readBufferSPI[i][j] = rByte;
rrknight 1:b430b4401fc4 441
rrknight 1:b430b4401fc4 442 j++; //write next byte next time
rrknight 1:b430b4401fc4 443
rrknight 1:b430b4401fc4 444 if(j >= NUMBER_BYTES_PER_MSG)
rrknight 1:b430b4401fc4 445 {
rrknight 1:b430b4401fc4 446 j = 0;
rrknight 1:b430b4401fc4 447 i++; //next node
rrknight 0:18d7499b82f3 448
rrknight 1:b430b4401fc4 449 if(i >= NUMBER_MAX_EPOS2_PER_SLAVE)
rrknight 1:b430b4401fc4 450 {
rrknight 1:b430b4401fc4 451 //finished reading the array
rrknight 1:b430b4401fc4 452 /*
rrknight 1:b430b4401fc4 453 for(int n=0; n<1; n++)
rrknight 0:18d7499b82f3 454 {
rrknight 1:b430b4401fc4 455 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]);
rrknight 1:b430b4401fc4 456 }
rrknight 1:b430b4401fc4 457 */
rrknight 1:b430b4401fc4 458
rrknight 1:b430b4401fc4 459 //reset
rrknight 1:b430b4401fc4 460 threeArrows = 0;
rrknight 1:b430b4401fc4 461 i = 0;
rrknight 1:b430b4401fc4 462 j = 0;
rrknight 1:b430b4401fc4 463 slaveSelected = false; //to end the while loop
rrknight 1:b430b4401fc4 464 }
rrknight 1:b430b4401fc4 465 }
rrknight 1:b430b4401fc4 466
rrknight 1:b430b4401fc4 467 if(slaveSelected) device.reply(writeBufferSPI[i][j]);
rrknight 1:b430b4401fc4 468 else device.reply(dataChecksum); //checksum
rrknight 1:b430b4401fc4 469 }
rrknight 0:18d7499b82f3 470 }//if
rrknight 0:18d7499b82f3 471
rrknight 0:18d7499b82f3 472 wait_us(1);
rrknight 0:18d7499b82f3 473 }//while slaveSelected
rrknight 1:b430b4401fc4 474
rrknight 1:b430b4401fc4 475 //read the checksum
rrknight 1:b430b4401fc4 476 while(!checksumReceived)
rrknight 1:b430b4401fc4 477 {
rrknight 1:b430b4401fc4 478 if(device.receive())
rrknight 1:b430b4401fc4 479 {
rrknight 1:b430b4401fc4 480 cmdChecksum = device.read();
rrknight 1:b430b4401fc4 481 cmdValid = verifyChecksum();
rrknight 1:b430b4401fc4 482 checksumReceived = true; //exit while loop
rrknight 1:b430b4401fc4 483 }
rrknight 1:b430b4401fc4 484
rrknight 1:b430b4401fc4 485 wait_us(1);
rrknight 1:b430b4401fc4 486 }
rrknight 1:b430b4401fc4 487
rrknight 0:18d7499b82f3 488 __enable_irq();
rrknight 0:18d7499b82f3 489
rrknight 1:b430b4401fc4 490 logicPin = 0;
rrknight 0:18d7499b82f3 491 wait_us(10);
rrknight 0:18d7499b82f3 492 logicPin = 1;
rrknight 0:18d7499b82f3 493
rrknight 1:b430b4401fc4 494 //if checksum is correct, then play the cmds
rrknight 1:b430b4401fc4 495 if(cmdValid)
rrknight 1:b430b4401fc4 496 {
rrknight 1:b430b4401fc4 497 //play the commands
rrknight 1:b430b4401fc4 498 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 1:b430b4401fc4 499 {
rrknight 1:b430b4401fc4 500 uint8_t node_ID = readBufferSPI[i][0];
rrknight 1:b430b4401fc4 501 uint8_t node_mode = readBufferSPI[i][1];
rrknight 1:b430b4401fc4 502 int position = readBufferSPI[i][2] + (readBufferSPI[i][3]<<8) + (readBufferSPI[i][4]<<16) + (readBufferSPI[i][5]<<24);
rrknight 1:b430b4401fc4 503
rrknight 1:b430b4401fc4 504 //uint8_t cmd_delay = readBufferSPI[i][6]; //TODO delay
rrknight 1:b430b4401fc4 505
rrknight 1:b430b4401fc4 506 //pc.printf("Cmd[%d][%d][%d][%d]\n", node_ID, node_mode, position, cmd_delay);
rrknight 1:b430b4401fc4 507
rrknight 1:b430b4401fc4 508 if(node_mode == 1)
rrknight 1:b430b4401fc4 509 setPosition(node_ID, position); //test
rrknight 1:b430b4401fc4 510
rrknight 1:b430b4401fc4 511 wait_us(10);
rrknight 1:b430b4401fc4 512 }
rrknight 1:b430b4401fc4 513
rrknight 1:b430b4401fc4 514 cmdValid = false; //reset for next packet
rrknight 1:b430b4401fc4 515 }
rrknight 1:b430b4401fc4 516
rrknight 1:b430b4401fc4 517 logicPin = 0;
rrknight 1:b430b4401fc4 518 wait_us(10);
rrknight 1:b430b4401fc4 519 logicPin = 1;
rrknight 1:b430b4401fc4 520
rrknight 1:b430b4401fc4 521
rrknight 0:18d7499b82f3 522 //get the sensor values
rrknight 0:18d7499b82f3 523 for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
rrknight 0:18d7499b82f3 524 {
rrknight 0:18d7499b82f3 525 uint8_t node_id = i+1;
rrknight 1:b430b4401fc4 526 //if(node_id!=5)
rrknight 1:b430b4401fc4 527 //{
rrknight 1:b430b4401fc4 528 getPosition(node_id);
rrknight 1:b430b4401fc4 529 wait_us(200);
rrknight 1:b430b4401fc4 530 getCurrent(node_id);
rrknight 1:b430b4401fc4 531 wait_us(200);
rrknight 0:18d7499b82f3 532 //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);
rrknight 1:b430b4401fc4 533 //}
rrknight 0:18d7499b82f3 534 }
rrknight 1:b430b4401fc4 535
rrknight 1:b430b4401fc4 536 //update the writeBufferSPI
rrknight 1:b430b4401fc4 537
rrknight 1:b430b4401fc4 538 //compute checksum
rrknight 1:b430b4401fc4 539
rrknight 1:b430b4401fc4 540
rrknight 1:b430b4401fc4 541 /*
rrknight 0:18d7499b82f3 542 //build the motorDataSet_msg
rrknight 0:18d7499b82f3 543 for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
rrknight 0:18d7499b82f3 544 {
rrknight 0:18d7499b82f3 545 uint8_t node_id = i+1;
rrknight 1:b430b4401fc4 546 //if(node_id!=5)
rrknight 1:b430b4401fc4 547 //{
rrknight 0:18d7499b82f3 548 //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
rrknight 0:18d7499b82f3 549
rrknight 0:18d7499b82f3 550 //motorDataSet_msg.motorData[i].encPosition = encPosition[node_id-1];
rrknight 0:18d7499b82f3 551 //motorDataSet_msg.motorData[i].current = avgCurrent[node_id-1];
rrknight 0:18d7499b82f3 552 //if((node_id >= 2) && (node_id <= 9)) motorDataSet_msg.motorData[i].force = getMedianForceVal(node_id); //medForce[node_id-1];
rrknight 1:b430b4401fc4 553 //}
rrknight 0:18d7499b82f3 554 }
rrknight 1:b430b4401fc4 555 */
rrknight 0:18d7499b82f3 556 logicPin = 0;
rrknight 0:18d7499b82f3 557
rrknight 1:b430b4401fc4 558 wait_us(10);
rrknight 0:18d7499b82f3 559
rrknight 0:18d7499b82f3 560 /*
rrknight 0:18d7499b82f3 561 //disable interrupts and publish it
rrknight 0:18d7499b82f3 562 __disable_irq();
rrknight 0:18d7499b82f3 563 motorDataSetPub.publish(&motorDataSet_msg);
rrknight 0:18d7499b82f3 564 //this check if there are some msg published on the topics, and excecute the cb functions
rrknight 0:18d7499b82f3 565 nh.spinOnce();
rrknight 0:18d7499b82f3 566 __enable_irq();
rrknight 1:b430b4401fc4 567 */
rrknight 0:18d7499b82f3 568 //this will excecute cmds of the array that are ready (delay 0)
rrknight 1:b430b4401fc4 569 //commandPlayer();
rrknight 1:b430b4401fc4 570
rrknight 0:18d7499b82f3 571 }// main while end
rrknight 0:18d7499b82f3 572 }// main end
rrknight 0:18d7499b82f3 573
rrknight 0:18d7499b82f3 574 #endif //COMPILE_MAIN_CODE_ROSSERIAL