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@4:396433e30e64, 2013-04-05 (annotated)

Committer:
rrknight
Date:
Fri Apr 05 19:06:48 2013 +0000
Revision:
4:396433e30e64
Parent:
3:c16d726670b2
Child:
5:52e3137c2831
Hannover version friday

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rrknight 2:7ab1d5918efe 1 #define COMPILE_MAIN_CODE_TRS_SLAVE
rrknight 2:7ab1d5918efe 2 #ifdef COMPILE_MAIN_CODE_TRS_SLAVE
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 4:396433e30e64 16 #define CALIB_DELAY 1500
rrknight 4:396433e30e64 17 #define NUMBER_OF_CALIB 3
rrknight 4:396433e30e64 18
rrknight 0:18d7499b82f3 19 SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
rrknight 3:c16d726670b2 20 DigitalIn sync_master(p11); //previously p25
rrknight 0:18d7499b82f3 21
rrknight 4:396433e30e64 22 DigitalIn calibDonePin(p23);
rrknight 1:b430b4401fc4 23 DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
rrknight 3:c16d726670b2 24
rrknight 3:c16d726670b2 25 uint8_t numberEpos2Boards = 1;
rrknight 3:c16d726670b2 26
rrknight 1:b430b4401fc4 27 char dataChecksum = 0x00;
rrknight 1:b430b4401fc4 28 char cmdChecksum = 0x00;
rrknight 0:18d7499b82f3 29
rrknight 1:b430b4401fc4 30 uint8_t writeBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
rrknight 1:b430b4401fc4 31 uint8_t readBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
rrknight 0:18d7499b82f3 32
rrknight 4:396433e30e64 33 int32_t tempCalibEnc[NUMBER_OF_CALIB][NUMBER_MAX_EPOS2_PER_SLAVE];
rrknight 4:396433e30e64 34 int32_t meanCalibEnc[NUMBER_MAX_EPOS2_PER_SLAVE];
rrknight 4:396433e30e64 35 int32_t distanceFromMean[NUMBER_OF_CALIB][NUMBER_MAX_EPOS2_PER_SLAVE];
rrknight 4:396433e30e64 36 int32_t minDistFromMean[NUMBER_MAX_EPOS2_PER_SLAVE];
rrknight 4:396433e30e64 37 int32_t maxDistFromMean[NUMBER_MAX_EPOS2_PER_SLAVE];
rrknight 4:396433e30e64 38
rrknight 4:396433e30e64 39 int32_t calibOffset[NUMBER_MAX_EPOS2_PER_SLAVE];
rrknight 4:396433e30e64 40
rrknight 0:18d7499b82f3 41 int counter = 0;
rrknight 0:18d7499b82f3 42
rrknight 0:18d7499b82f3 43 //5 variables for median poti position
rrknight 0:18d7499b82f3 44 //int16_t potiVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 45 //int16_t sortVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 46
rrknight 0:18d7499b82f3 47 int16_t forceVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 48 int16_t sortForceVal[NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 49 //int16_t lastForceVal[NUMBER_EPOS2_BOARDS][NB_SAMPLES_MEDIAN];
rrknight 0:18d7499b82f3 50
rrknight 0:18d7499b82f3 51 /*
rrknight 0:18d7499b82f3 52 int16_t getMedianPotiVal(const int8_t nodeID)
rrknight 0:18d7499b82f3 53 {
rrknight 0:18d7499b82f3 54 //read poti
rrknight 0:18d7499b82f3 55 for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
rrknight 0:18d7499b82f3 56 {
rrknight 0:18d7499b82f3 57 getPotiPosition(nodeID);
rrknight 0:18d7499b82f3 58 wait_us(1000);
rrknight 0:18d7499b82f3 59
rrknight 0:18d7499b82f3 60 potiVal[m] = potiPosArray[nodeID - 1];
rrknight 0:18d7499b82f3 61 sortVal[m] = potiVal[m];
rrknight 0:18d7499b82f3 62
rrknight 0:18d7499b82f3 63 wait_us(100);
rrknight 0:18d7499b82f3 64 }
rrknight 0:18d7499b82f3 65
rrknight 0:18d7499b82f3 66 //sort values
rrknight 0:18d7499b82f3 67 for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
rrknight 0:18d7499b82f3 68 {
rrknight 0:18d7499b82f3 69 int16_t n;
rrknight 0:18d7499b82f3 70 n = sortVal[m];
rrknight 0:18d7499b82f3 71 int p;
rrknight 0:18d7499b82f3 72
rrknight 0:18d7499b82f3 73 for(p = m-1; (p >=0) && (n<sortVal[p]); p--)
rrknight 0:18d7499b82f3 74 {
rrknight 0:18d7499b82f3 75 sortVal[p+1] = sortVal[p];
rrknight 0:18d7499b82f3 76 }
rrknight 0:18d7499b82f3 77 sortVal[p+1] = n;
rrknight 0:18d7499b82f3 78 }
rrknight 0:18d7499b82f3 79
rrknight 0:18d7499b82f3 80 return sortVal[2];
rrknight 0:18d7499b82f3 81 }
rrknight 0:18d7499b82f3 82 */
rrknight 0:18d7499b82f3 83
rrknight 2:7ab1d5918efe 84 bool verifyChecksum() //check the data comming from the master over SPI
rrknight 1:b430b4401fc4 85 {
rrknight 1:b430b4401fc4 86 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 1:b430b4401fc4 87 {
rrknight 1:b430b4401fc4 88 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 1:b430b4401fc4 89 {
rrknight 1:b430b4401fc4 90 cmdChecksum += readBufferSPI[i][j];
rrknight 1:b430b4401fc4 91 }
rrknight 1:b430b4401fc4 92 }
rrknight 1:b430b4401fc4 93
rrknight 1:b430b4401fc4 94 cmdChecksum++; //add 1 to obtain 0x00
rrknight 2:7ab1d5918efe 95
rrknight 2:7ab1d5918efe 96 //pc.printf("sum 0x%02X\n\r", cmdChecksum);
rrknight 1:b430b4401fc4 97
rrknight 1:b430b4401fc4 98 if(cmdChecksum == 0x00) return true;
rrknight 1:b430b4401fc4 99 else return false;
rrknight 1:b430b4401fc4 100 }
rrknight 1:b430b4401fc4 101
rrknight 2:7ab1d5918efe 102 void calculateSPIChecksum() //compute checksum for the data sent to the master over SPI
rrknight 2:7ab1d5918efe 103 {
rrknight 2:7ab1d5918efe 104 int sum = 0;
rrknight 2:7ab1d5918efe 105
rrknight 2:7ab1d5918efe 106 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 2:7ab1d5918efe 107 {
rrknight 2:7ab1d5918efe 108 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 2:7ab1d5918efe 109 {
rrknight 2:7ab1d5918efe 110 sum += writeBufferSPI[i][j];
rrknight 2:7ab1d5918efe 111 }
rrknight 2:7ab1d5918efe 112 }
rrknight 2:7ab1d5918efe 113
rrknight 2:7ab1d5918efe 114 dataChecksum = (char)(~sum); //reverse 0 and 1, and cast as byte
rrknight 2:7ab1d5918efe 115 }
rrknight 2:7ab1d5918efe 116
rrknight 0:18d7499b82f3 117 int16_t getMedianForceVal(const int8_t nodeID)
rrknight 0:18d7499b82f3 118 {
rrknight 2:7ab1d5918efe 119 //logicPin = 1;
rrknight 0:18d7499b82f3 120
rrknight 0:18d7499b82f3 121 for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
rrknight 0:18d7499b82f3 122 {
rrknight 0:18d7499b82f3 123 sortForceVal[m] = getForce(nodeID);
rrknight 0:18d7499b82f3 124 wait_us(100); //adjust this
rrknight 0:18d7499b82f3 125 }
rrknight 0:18d7499b82f3 126
rrknight 0:18d7499b82f3 127 //sort values
rrknight 0:18d7499b82f3 128 for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
rrknight 0:18d7499b82f3 129 {
rrknight 0:18d7499b82f3 130 int16_t n = sortForceVal[m];
rrknight 0:18d7499b82f3 131 int p;
rrknight 0:18d7499b82f3 132
rrknight 0:18d7499b82f3 133 for(p = m-1; (p >=0) && (n<sortForceVal[p]); p--)
rrknight 0:18d7499b82f3 134 {
rrknight 0:18d7499b82f3 135 sortForceVal[p+1] = sortForceVal[p];
rrknight 0:18d7499b82f3 136 }
rrknight 0:18d7499b82f3 137 sortForceVal[p+1] = n;
rrknight 0:18d7499b82f3 138 }
rrknight 0:18d7499b82f3 139
rrknight 2:7ab1d5918efe 140 //logicPin = 0;
rrknight 0:18d7499b82f3 141
rrknight 0:18d7499b82f3 142 return sortForceVal[2];
rrknight 0:18d7499b82f3 143 }
rrknight 0:18d7499b82f3 144
rrknight 0:18d7499b82f3 145 /*** INTERRUPT (for catching Emergency error frames) ***/
rrknight 0:18d7499b82f3 146 void interrupt()
rrknight 0:18d7499b82f3 147 {
rrknight 0:18d7499b82f3 148 CANMessage canmsg;
rrknight 0:18d7499b82f3 149 int64_t data = 0x0000000000000000;
rrknight 0:18d7499b82f3 150 int8_t nodeID = 0;
rrknight 0:18d7499b82f3 151 int16_t cobID = 0;
rrknight 0:18d7499b82f3 152
rrknight 0:18d7499b82f3 153 //read the can message that has triggered the interrupt
rrknight 0:18d7499b82f3 154 cantoepos.read(canmsg);
rrknight 0:18d7499b82f3 155
rrknight 0:18d7499b82f3 156 //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 157
rrknight 0:18d7499b82f3 158 nodeID = 0x00F & canmsg.id;
rrknight 0:18d7499b82f3 159 cobID = 0x0FF0 & canmsg.id;
rrknight 0:18d7499b82f3 160
rrknight 0:18d7499b82f3 161 for(int i=0; i<=canmsg.len; i++)
rrknight 0:18d7499b82f3 162 {
rrknight 0:18d7499b82f3 163 data = data | (canmsg.data[i]<<i*8);
rrknight 0:18d7499b82f3 164 }
rrknight 0:18d7499b82f3 165
rrknight 0:18d7499b82f3 166 //check nodeID first
rrknight 3:c16d726670b2 167 if((nodeID >= 1) && (nodeID <= numberEpos2Boards)) //was nodeID >= 0 before ???
rrknight 0:18d7499b82f3 168 {
rrknight 0:18d7499b82f3 169 switch(cobID)
rrknight 0:18d7499b82f3 170 {
rrknight 0:18d7499b82f3 171 case COB_ID_TRANSMIT_PDO_1_ENABLE : //getPosition
rrknight 0:18d7499b82f3 172 //pc.printf("getPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 173 encPosition[nodeID - 1] = data;
rrknight 0:18d7499b82f3 174 break;
rrknight 0:18d7499b82f3 175
rrknight 0:18d7499b82f3 176 case COB_ID_TRANSMIT_PDO_2_ENABLE : //getCurrent averaged
rrknight 0:18d7499b82f3 177 //pc.printf("getCurrent Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 178 avgCurrent[nodeID - 1] = data;
rrknight 0:18d7499b82f3 179 break;
rrknight 0:18d7499b82f3 180
rrknight 0:18d7499b82f3 181 case COB_ID_TRANSMIT_PDO_3_ENABLE : //getVelocity
rrknight 0:18d7499b82f3 182 //pc.printf("getVelocity Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 183 velocity[nodeID - 1] = data;
rrknight 0:18d7499b82f3 184 break;
rrknight 0:18d7499b82f3 185
rrknight 0:18d7499b82f3 186 case COB_ID_TRANSMIT_PDO_4_ENABLE : //getPotiPosition
rrknight 0:18d7499b82f3 187 //pc.printf("getPotiPosition Node ID : [%d], PDO COB-ID [%02X], data = %d\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 188 potiPosArray[nodeID - 1] = data;
rrknight 0:18d7499b82f3 189 break;
rrknight 0:18d7499b82f3 190
rrknight 0:18d7499b82f3 191 case COB_ID_EMCY_DEFAULT : //Emergency frame
rrknight 0:18d7499b82f3 192 //pc.printf("Emergency frame, Node ID : [%d], PDO COB-ID [%02X], data = %02X\n", nodeID, cobID, data);
rrknight 0:18d7499b82f3 193 //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 194 pc.printf("EF%02X-%02X%02X\n\r", canmsg.id, canmsg.data[1], canmsg.data[0]);
rrknight 0:18d7499b82f3 195 ledchain[1] = 1;
rrknight 0:18d7499b82f3 196 //nh.logerror("Emergency frame");
rrknight 0:18d7499b82f3 197 boardStatus[nodeID-1] = 1;
rrknight 0:18d7499b82f3 198 //first step : fault reset on controlword
rrknight 0:18d7499b82f3 199 //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
rrknight 0:18d7499b82f3 200 faultResetControlword(nodeID);
rrknight 0:18d7499b82f3 201 break;
rrknight 0:18d7499b82f3 202
rrknight 0:18d7499b82f3 203 case COB_ID_SDO_SERVER_TO_CLIENT_DEFAULT : //SDO Acknoledgement frame
rrknight 0:18d7499b82f3 204 int32_t regData = 0x00000000;
rrknight 0:18d7499b82f3 205 regData = (int32_t)data;
rrknight 0:18d7499b82f3 206
rrknight 0:18d7499b82f3 207 //pc.printf("Node %d - regData [%02X]\n", nodeID, regData);
rrknight 0:18d7499b82f3 208
rrknight 0:18d7499b82f3 209 if(regData == 0x00604060) //Controlword
rrknight 0:18d7499b82f3 210 {
rrknight 0:18d7499b82f3 211 if(boardStatus[nodeID-1] == 1)
rrknight 0:18d7499b82f3 212 {
rrknight 0:18d7499b82f3 213 boardStatus[nodeID-1] = 2;
rrknight 0:18d7499b82f3 214 //second step : shutdown controlword
rrknight 0:18d7499b82f3 215 //pc.printf("Node %d - STEP 2 - shutdownControlwordIT\n", nodeID);
rrknight 0:18d7499b82f3 216 shutdownControlwordIT(nodeID);
rrknight 0:18d7499b82f3 217 }
rrknight 0:18d7499b82f3 218 else if(boardStatus[nodeID-1] == 2)
rrknight 0:18d7499b82f3 219 {
rrknight 0:18d7499b82f3 220 boardStatus[nodeID-1] = 3;
rrknight 0:18d7499b82f3 221 //third step : Switch On & Enable Operation on Controlword
rrknight 0:18d7499b82f3 222 //pc.printf("Node %d - STEP 3 - switchOnEnableOperationControlwordIT\n", nodeID);
rrknight 0:18d7499b82f3 223 switchOnEnableOperationControlwordIT(nodeID);
rrknight 0:18d7499b82f3 224 }
rrknight 0:18d7499b82f3 225 else if(boardStatus[nodeID-1] == 3)
rrknight 0:18d7499b82f3 226 {
rrknight 0:18d7499b82f3 227 boardStatus[nodeID-1] = 4;
rrknight 0:18d7499b82f3 228 //ask for statusword to check if the board has reset well
rrknight 0:18d7499b82f3 229 //pc.printf("Node %d - STEP 4 - getStatusword\n", nodeID);
rrknight 0:18d7499b82f3 230 getStatusword(nodeID);
rrknight 0:18d7499b82f3 231 }
rrknight 0:18d7499b82f3 232 }
rrknight 0:18d7499b82f3 233 else if(regData == 0x0060414B) //read Statusword
rrknight 0:18d7499b82f3 234 {
rrknight 1:b430b4401fc4 235 //int32_t swData = 0x00000000;
rrknight 0:18d7499b82f3 236
rrknight 0:18d7499b82f3 237 //pc.printf("Node %d - Statusword [%02X]\n", nodeID, canmsg.data[4]);
rrknight 0:18d7499b82f3 238 //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 239
rrknight 0:18d7499b82f3 240 if(boardStatus[nodeID-1] == 4)
rrknight 0:18d7499b82f3 241 {
rrknight 0:18d7499b82f3 242 //swData = data >> 32;
rrknight 0:18d7499b82f3 243 int8_t fault = 0x00;
rrknight 0:18d7499b82f3 244 fault = (canmsg.data[4] & 0x08) >> 3;
rrknight 0:18d7499b82f3 245
rrknight 0:18d7499b82f3 246 if(fault == 0) //reset OK
rrknight 0:18d7499b82f3 247 {
rrknight 0:18d7499b82f3 248 boardStatus[nodeID-1] = 0; //Board is reset and enable OK
rrknight 0:18d7499b82f3 249 pc.printf("%d OK\n\r", nodeID);
rrknight 0:18d7499b82f3 250 ledchain[1] = 0;
rrknight 0:18d7499b82f3 251 }
rrknight 0:18d7499b82f3 252 else //try to reset again
rrknight 0:18d7499b82f3 253 {
rrknight 0:18d7499b82f3 254 //pc.printf("Node %d - try to reset again\n", nodeID);
rrknight 0:18d7499b82f3 255 boardStatus[nodeID-1] = 1;
rrknight 0:18d7499b82f3 256 //go back to first step : fault reset on controlword
rrknight 0:18d7499b82f3 257 //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
rrknight 0:18d7499b82f3 258 faultResetControlword(nodeID);
rrknight 0:18d7499b82f3 259 }
rrknight 0:18d7499b82f3 260 }
rrknight 0:18d7499b82f3 261 }
rrknight 0:18d7499b82f3 262 break;
rrknight 0:18d7499b82f3 263
rrknight 0:18d7499b82f3 264 default :
rrknight 0:18d7499b82f3 265 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 266 } //end switch
rrknight 0:18d7499b82f3 267 }
rrknight 0:18d7499b82f3 268 else
rrknight 0:18d7499b82f3 269 {
rrknight 0:18d7499b82f3 270 pc.printf("NODEID ERROR\n\r");
rrknight 0:18d7499b82f3 271 }
rrknight 0:18d7499b82f3 272 } //end interrupt
rrknight 0:18d7499b82f3 273
rrknight 0:18d7499b82f3 274 void commandPlayer() //called in main every 20ms
rrknight 2:7ab1d5918efe 275 {
rrknight 2:7ab1d5918efe 276 //at least one cmd played
rrknight 2:7ab1d5918efe 277 bool cmdPlayLED = false;
rrknight 2:7ab1d5918efe 278
rrknight 2:7ab1d5918efe 279 for(int i= 0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 0:18d7499b82f3 280 {
rrknight 2:7ab1d5918efe 281 uint8_t node_ID = readBufferSPI[i][0];
rrknight 2:7ab1d5918efe 282 uint8_t node_mode = readBufferSPI[i][1];
rrknight 2:7ab1d5918efe 283 int32_t value = readBufferSPI[i][2] + (readBufferSPI[i][3]<<8) + (readBufferSPI[i][4]<<16) + (readBufferSPI[i][5]<<24);
rrknight 3:c16d726670b2 284
rrknight 3:c16d726670b2 285 if((node_ID >= 1) && (node_ID <= numberEpos2Boards))
rrknight 3:c16d726670b2 286 {
rrknight 3:c16d726670b2 287 if(node_mode != 0xFF)
rrknight 3:c16d726670b2 288 {
rrknight 3:c16d726670b2 289 switch (node_mode)
rrknight 3:c16d726670b2 290 {
rrknight 3:c16d726670b2 291 //WARNING : changing mode requires 1 cycle, so "else" case has been added, commands needs to be sent at least twice to be applied when the mode change.
rrknight 3:c16d726670b2 292
rrknight 3:c16d726670b2 293 case POSITION: //first change modes of motors that will be triggered later
rrknight 3:c16d726670b2 294 if(activMode[node_ID-1] != POSITION) setModeOfOperationPDO(node_ID, VALUE_POSITION_MODE);
rrknight 4:396433e30e64 295 else setPosition(node_ID, value + calibOffset[node_ID-1]);
rrknight 3:c16d726670b2 296 break;
rrknight 3:c16d726670b2 297 case CURRENT: //first change modes of motors that will be triggered later (like CURRENT mode needs some time to be active)
rrknight 3:c16d726670b2 298 //pc.printf("setCurrent(%d, %d)\n", motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
rrknight 3:c16d726670b2 299 if(activMode[node_ID-1] != CURRENT) setModeOfOperationPDO(node_ID, VALUE_CURRENT_MODE);
rrknight 3:c16d726670b2 300 else setCurrent(node_ID, value);
rrknight 3:c16d726670b2 301 break;
rrknight 3:c16d726670b2 302 case VELOCITY: //first change modes of motors that will be triggered later
rrknight 3:c16d726670b2 303 if(activMode[node_ID-1] != VELOCITY) setModeOfOperationPDO(node_ID, VALUE_VELOCITY_MODE);
rrknight 3:c16d726670b2 304 else setVelocity(node_ID, value);
rrknight 3:c16d726670b2 305 break;
rrknight 3:c16d726670b2 306 default:
rrknight 3:c16d726670b2 307 break;
rrknight 3:c16d726670b2 308 }
rrknight 2:7ab1d5918efe 309
rrknight 3:c16d726670b2 310 ledchain[3] = 1; //switch on when cmd is applied
rrknight 3:c16d726670b2 311 cmdPlayLED = true;
rrknight 0:18d7499b82f3 312 }
rrknight 3:c16d726670b2 313 else //idle mode 0xFF
rrknight 3:c16d726670b2 314 {
rrknight 3:c16d726670b2 315 if(!cmdPlayLED) ledchain[3] = 0; //switch off when slave is idle, i.e. all cmd in a set are 0xFF
rrknight 3:c16d726670b2 316 }
rrknight 0:18d7499b82f3 317 }
rrknight 2:7ab1d5918efe 318
rrknight 2:7ab1d5918efe 319 wait_us(10);
rrknight 2:7ab1d5918efe 320 }
rrknight 2:7ab1d5918efe 321 }
rrknight 2:7ab1d5918efe 322
rrknight 2:7ab1d5918efe 323 void updateTxSPIBuffer()
rrknight 2:7ab1d5918efe 324 {
rrknight 3:c16d726670b2 325 for(int i=1; i<=numberEpos2Boards; i++)
rrknight 2:7ab1d5918efe 326 {
rrknight 3:c16d726670b2 327 //uint8_t node_id = i;
rrknight 3:c16d726670b2 328
rrknight 3:c16d726670b2 329 getPosition(i);
rrknight 3:c16d726670b2 330 wait_us(200);
rrknight 3:c16d726670b2 331 getCurrent(i);
rrknight 3:c16d726670b2 332 wait_us(200);
rrknight 3:c16d726670b2 333 //getForce(i);
rrknight 3:c16d726670b2 334 wait_us(200);
rrknight 2:7ab1d5918efe 335 }
rrknight 2:7ab1d5918efe 336
rrknight 2:7ab1d5918efe 337 //build the motorDataSet_msg
rrknight 3:c16d726670b2 338 for(int i=0; i<numberEpos2Boards; i++)
rrknight 2:7ab1d5918efe 339 {
rrknight 2:7ab1d5918efe 340 uint8_t node_id = i+1;
rrknight 4:396433e30e64 341
rrknight 4:396433e30e64 342 int32_t position = encPosition[i] - calibOffset[i];
rrknight 4:396433e30e64 343
rrknight 4:396433e30e64 344 //position
rrknight 4:396433e30e64 345 writeBufferSPI[i][0] = position;
rrknight 4:396433e30e64 346 writeBufferSPI[i][1] = position>>8;
rrknight 4:396433e30e64 347 writeBufferSPI[i][2] = position>>16;
rrknight 4:396433e30e64 348 writeBufferSPI[i][3] = position>>24;
rrknight 4:396433e30e64 349
rrknight 4:396433e30e64 350 /*
rrknight 2:7ab1d5918efe 351 //position
rrknight 2:7ab1d5918efe 352 writeBufferSPI[i][0] = encPosition[node_id-1];
rrknight 2:7ab1d5918efe 353 writeBufferSPI[i][1] = encPosition[node_id-1]>>8;
rrknight 2:7ab1d5918efe 354 writeBufferSPI[i][2] = encPosition[node_id-1]>>16;
rrknight 2:7ab1d5918efe 355 writeBufferSPI[i][3] = encPosition[node_id-1]>>24;
rrknight 4:396433e30e64 356 */
rrknight 2:7ab1d5918efe 357 //current
rrknight 2:7ab1d5918efe 358 writeBufferSPI[i][4] = avgCurrent[node_id-1];
rrknight 2:7ab1d5918efe 359 writeBufferSPI[i][5] = avgCurrent[node_id-1]>>8;
rrknight 2:7ab1d5918efe 360
rrknight 2:7ab1d5918efe 361 //force
rrknight 2:7ab1d5918efe 362 writeBufferSPI[i][6] = 0;
rrknight 2:7ab1d5918efe 363 writeBufferSPI[i][7] = 0;
rrknight 2:7ab1d5918efe 364
rrknight 2:7ab1d5918efe 365 //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);
rrknight 2:7ab1d5918efe 366 //force = getMedianForceVal(node_id); //medForce[node_id-1];
rrknight 2:7ab1d5918efe 367 }
rrknight 0:18d7499b82f3 368 }
rrknight 0:18d7499b82f3 369
rrknight 0:18d7499b82f3 370 void initBufferSPI()
rrknight 0:18d7499b82f3 371 {
rrknight 0:18d7499b82f3 372 //init the SPI arrays
rrknight 1:b430b4401fc4 373 for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
rrknight 0:18d7499b82f3 374 {
rrknight 0:18d7499b82f3 375 for(int j=0; j<NUMBER_BYTES_PER_MSG; j++)
rrknight 0:18d7499b82f3 376 {
rrknight 2:7ab1d5918efe 377 writeBufferSPI[i][j] = 0x00;
rrknight 0:18d7499b82f3 378 readBufferSPI[i][j] = 0x00;
rrknight 0:18d7499b82f3 379 }
rrknight 0:18d7499b82f3 380 }
rrknight 4:396433e30e64 381
rrknight 3:c16d726670b2 382 /*
rrknight 1:b430b4401fc4 383 for(int n=0; n<NUMBER_MAX_EPOS2_PER_SLAVE; n++)
rrknight 0:18d7499b82f3 384 {
rrknight 1:b430b4401fc4 385 //position
rrknight 2:7ab1d5918efe 386 writeBufferSPI[n][0] = 0xA0+n;
rrknight 2:7ab1d5918efe 387 writeBufferSPI[n][1] = 0xB0+n;
rrknight 2:7ab1d5918efe 388 writeBufferSPI[n][2] = 0x00;
rrknight 2:7ab1d5918efe 389 writeBufferSPI[n][3] = 0x00;
rrknight 0:18d7499b82f3 390
rrknight 1:b430b4401fc4 391 //current
rrknight 0:18d7499b82f3 392 writeBufferSPI[n][4] = 0xC0+n;
rrknight 2:7ab1d5918efe 393 writeBufferSPI[n][5] = 0x00;
rrknight 2:7ab1d5918efe 394
rrknight 2:7ab1d5918efe 395 //force
rrknight 2:7ab1d5918efe 396 writeBufferSPI[n][6] = 0xD0+n;
rrknight 2:7ab1d5918efe 397 writeBufferSPI[n][7] = 0x00;
rrknight 3:c16d726670b2 398 }*/
rrknight 0:18d7499b82f3 399 }
rrknight 0:18d7499b82f3 400
rrknight 4:396433e30e64 401 void calibrate()
rrknight 4:396433e30e64 402 {
rrknight 4:396433e30e64 403 pc.printf("- Start Calibration\n\r");
rrknight 4:396433e30e64 404
rrknight 4:396433e30e64 405 //set all boards to current mode
rrknight 4:396433e30e64 406 for(int i=1; i<=numberEpos2Boards; i++)
rrknight 4:396433e30e64 407 {
rrknight 4:396433e30e64 408 setModeOfOperationPDO(i, VALUE_CURRENT_MODE);
rrknight 4:396433e30e64 409 }
rrknight 4:396433e30e64 410
rrknight 4:396433e30e64 411 for(int j=0; j<NUMBER_OF_CALIB; j++)
rrknight 4:396433e30e64 412 {
rrknight 4:396433e30e64 413 pc.printf("- Calibration number %d\n\r", j);
rrknight 4:396433e30e64 414 //trigger current in certain order
rrknight 4:396433e30e64 415 setCurrent(7, 70);
rrknight 4:396433e30e64 416 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 417 setCurrent(6, 100);
rrknight 4:396433e30e64 418 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 419 setCurrent(11, 100);
rrknight 4:396433e30e64 420 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 421 setCurrent(1, 100);
rrknight 4:396433e30e64 422 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 423 setCurrent(3, 100);
rrknight 4:396433e30e64 424 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 425 setCurrent(4, 100);
rrknight 4:396433e30e64 426 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 427 setCurrent(5, 70);
rrknight 4:396433e30e64 428 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 429 setCurrent(2, 70);
rrknight 4:396433e30e64 430 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 431 setCurrent(8, 70);
rrknight 4:396433e30e64 432 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 433 setCurrent(9, 70);
rrknight 4:396433e30e64 434 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 435 setCurrent(10, 100);
rrknight 4:396433e30e64 436 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 437 setCurrent(12, 70);
rrknight 4:396433e30e64 438 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 439 setCurrent(13, 70);
rrknight 4:396433e30e64 440 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 441 setCurrent(15, -100);
rrknight 4:396433e30e64 442 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 443 setCurrent(14, 100);
rrknight 4:396433e30e64 444 wait_ms(CALIB_DELAY);
rrknight 4:396433e30e64 445
rrknight 4:396433e30e64 446 pc.printf("- Offsets will be recorded in 5 sec...");
rrknight 4:396433e30e64 447 wait_ms(1000);
rrknight 4:396433e30e64 448 pc.printf("4...");
rrknight 4:396433e30e64 449 wait_ms(1000);
rrknight 4:396433e30e64 450 pc.printf("3...");
rrknight 4:396433e30e64 451 wait_ms(1000);
rrknight 4:396433e30e64 452 pc.printf("2...");
rrknight 4:396433e30e64 453 wait_ms(1000);
rrknight 4:396433e30e64 454 pc.printf("1...");
rrknight 4:396433e30e64 455 wait_ms(1000);
rrknight 4:396433e30e64 456 pc.printf("0...");
rrknight 4:396433e30e64 457
rrknight 4:396433e30e64 458 //save current encoder position as calib offset
rrknight 4:396433e30e64 459 for(int i=0; i<numberEpos2Boards; i++)
rrknight 4:396433e30e64 460 {
rrknight 4:396433e30e64 461 int nodeid = i+1;
rrknight 4:396433e30e64 462 getPosition(nodeid);
rrknight 4:396433e30e64 463 wait_us(500);
rrknight 4:396433e30e64 464 tempCalibEnc[j][i] = encPosition[i];
rrknight 4:396433e30e64 465
rrknight 4:396433e30e64 466 //pc.printf("%d\n\r", calibOffset[i]);
rrknight 4:396433e30e64 467 }
rrknight 4:396433e30e64 468 pc.printf("OK\n\r");
rrknight 4:396433e30e64 469
rrknight 4:396433e30e64 470 pc.printf("current to zero\n\r");
rrknight 4:396433e30e64 471 for(int i=0; i<numberEpos2Boards; i++)
rrknight 4:396433e30e64 472 {
rrknight 4:396433e30e64 473 int nodeid = i+1;
rrknight 4:396433e30e64 474 setCurrent(nodeid, 0);
rrknight 4:396433e30e64 475 wait_us(500);
rrknight 4:396433e30e64 476 }
rrknight 4:396433e30e64 477
rrknight 4:396433e30e64 478 pc.printf("wait 5 sec now\n\r");
rrknight 4:396433e30e64 479 wait_ms(5000);
rrknight 4:396433e30e64 480 pc.printf("will restart now\n\r");
rrknight 4:396433e30e64 481 wait_ms(1000);
rrknight 4:396433e30e64 482 }//end for NUMBER_OF_CALIB times
rrknight 4:396433e30e64 483
rrknight 4:396433e30e64 484 pc.printf("\n\rCompute mean, min and max offsets\n\r");
rrknight 4:396433e30e64 485 for(int i=0; i<numberEpos2Boards; i++)
rrknight 4:396433e30e64 486 {
rrknight 4:396433e30e64 487 meanCalibEnc[i] = 0;
rrknight 4:396433e30e64 488 minDistFromMean[i] = 100000;
rrknight 4:396433e30e64 489 maxDistFromMean[i] = 0;
rrknight 4:396433e30e64 490
rrknight 4:396433e30e64 491 for(int j=0; j<NUMBER_OF_CALIB; j++)
rrknight 4:396433e30e64 492 {
rrknight 4:396433e30e64 493 meanCalibEnc[i] += tempCalibEnc[j][i];
rrknight 4:396433e30e64 494 }
rrknight 4:396433e30e64 495 meanCalibEnc[i] /=NUMBER_OF_CALIB;
rrknight 4:396433e30e64 496 }
rrknight 4:396433e30e64 497
rrknight 4:396433e30e64 498 for(int j=0; j<NUMBER_OF_CALIB; j++)
rrknight 4:396433e30e64 499 {
rrknight 4:396433e30e64 500 for(int i=0; i<numberEpos2Boards; i++)
rrknight 4:396433e30e64 501 {
rrknight 4:396433e30e64 502 distanceFromMean[j][i] = tempCalibEnc[j][i] - meanCalibEnc[i];
rrknight 4:396433e30e64 503
rrknight 4:396433e30e64 504 if(distanceFromMean[j][i] < minDistFromMean[i]) minDistFromMean[i] = distanceFromMean[j][i];
rrknight 4:396433e30e64 505 if(distanceFromMean[j][i] > maxDistFromMean[i]) maxDistFromMean[i] = distanceFromMean[j][i];
rrknight 4:396433e30e64 506
rrknight 4:396433e30e64 507
rrknight 4:396433e30e64 508 }
rrknight 4:396433e30e64 509 }
rrknight 4:396433e30e64 510
rrknight 4:396433e30e64 511 for(int i=0; i<numberEpos2Boards; i++)
rrknight 4:396433e30e64 512 {
rrknight 4:396433e30e64 513 pc.printf("nodeID %d : min[%d] max[%d]\n\r", i+1, minDistFromMean[i], maxDistFromMean[i]);
rrknight 4:396433e30e64 514
rrknight 4:396433e30e64 515 //update the offsets
rrknight 4:396433e30e64 516 calibOffset[i] = meanCalibEnc[i];
rrknight 4:396433e30e64 517 }
rrknight 4:396433e30e64 518 }
rrknight 4:396433e30e64 519
rrknight 0:18d7499b82f3 520 int main()
rrknight 0:18d7499b82f3 521 {
rrknight 2:7ab1d5918efe 522 pc.baud(115200);
rrknight 3:c16d726670b2 523 pc.printf("*** Start Slave Main - DCX Left Arm ***\n\r");
rrknight 0:18d7499b82f3 524
rrknight 0:18d7499b82f3 525 logicPin = 0;
rrknight 2:7ab1d5918efe 526
rrknight 1:b430b4401fc4 527 uint8_t my_val = 0x00; //to read and empty the SPI FIFO buffer
rrknight 0:18d7499b82f3 528
rrknight 0:18d7499b82f3 529 initBufferSPI();
rrknight 0:18d7499b82f3 530 //sync_master = 0;
rrknight 0:18d7499b82f3 531 char rByte = 0x00;
rrknight 0:18d7499b82f3 532
rrknight 0:18d7499b82f3 533 char threeArrows = 0;
rrknight 0:18d7499b82f3 534 bool threeArrowsFound = false;
rrknight 0:18d7499b82f3 535 bool slaveSelected = false;
rrknight 1:b430b4401fc4 536 bool checksumReceived = false;
rrknight 1:b430b4401fc4 537 bool cmdValid = false;
rrknight 0:18d7499b82f3 538
rrknight 0:18d7499b82f3 539 int i = 0; //msg
rrknight 0:18d7499b82f3 540 int j = 0; //byte number
rrknight 0:18d7499b82f3 541
rrknight 0:18d7499b82f3 542 pc.printf("--- Initialise EPOS2 boards ---\n\r");
rrknight 2:7ab1d5918efe 543
rrknight 3:c16d726670b2 544 /*
rrknight 0:18d7499b82f3 545 for(int i=1; i<=NUMBER_EPOS2_BOARDS; i++)
rrknight 0:18d7499b82f3 546 {
rrknight 3:c16d726670b2 547 if(initEposBoard(i) = EPOS2_OK) numberEpos2Boards++;
rrknight 0:18d7499b82f3 548 }
rrknight 2:7ab1d5918efe 549 */
rrknight 2:7ab1d5918efe 550
rrknight 3:c16d726670b2 551 //int i=1;
rrknight 3:c16d726670b2 552 while(initEposBoard(numberEpos2Boards) == EPOS2_OK)
rrknight 3:c16d726670b2 553 {
rrknight 3:c16d726670b2 554 numberEpos2Boards++;
rrknight 3:c16d726670b2 555 //i++;
rrknight 3:c16d726670b2 556 }
rrknight 3:c16d726670b2 557
rrknight 3:c16d726670b2 558 numberEpos2Boards--; //because it has been increment one time too much at the end of the while loop
rrknight 3:c16d726670b2 559
rrknight 2:7ab1d5918efe 560 //initEposBoard(0);
rrknight 0:18d7499b82f3 561
rrknight 2:7ab1d5918efe 562 //ledchain[0] = 1;
rrknight 0:18d7499b82f3 563
rrknight 0:18d7499b82f3 564 pc.printf("--- Enable Interrupts ---\n\r");
rrknight 0:18d7499b82f3 565 //attach the interrupt function
rrknight 0:18d7499b82f3 566 cantoepos.attach(&interrupt);
rrknight 0:18d7499b82f3 567 __enable_irq();
rrknight 0:18d7499b82f3 568
rrknight 4:396433e30e64 569 pc.printf("--- Calibrate Arm ---\n\r");
rrknight 4:396433e30e64 570 /*
rrknight 4:396433e30e64 571 //test
rrknight 4:396433e30e64 572 pc.printf("test home mode\n\r");
rrknight 4:396433e30e64 573 setModeOfOperationPDO(1, VALUE_CURRENT_MODE);
rrknight 4:396433e30e64 574 wait_ms(100);
rrknight 4:396433e30e64 575 pc.printf("set current\n\r");
rrknight 4:396433e30e64 576 setCurrent(1, 100);
rrknight 4:396433e30e64 577 wait_ms(2000);
rrknight 4:396433e30e64 578 getPosition(1);
rrknight 4:396433e30e64 579 wait_ms(100);
rrknight 4:396433e30e64 580 pc.printf("getPosition before home %d\n\r", encPosition[0]);
rrknight 4:396433e30e64 581 pc.printf("set OBJECT_HOME_POSITION\n\r");
rrknight 4:396433e30e64 582 setModeOfOperationPDO(1, 6);
rrknight 4:396433e30e64 583 setObjectSDO(1, OBJECT_HOME_POSITION, encPosition[0]);
rrknight 4:396433e30e64 584 wait_ms(100);
rrknight 4:396433e30e64 585 getPosition(1);
rrknight 4:396433e30e64 586 wait_ms(100);
rrknight 4:396433e30e64 587 pc.printf("getPosition after home %d\n\r", encPosition[0]);
rrknight 4:396433e30e64 588 //test
rrknight 4:396433e30e64 589 */
rrknight 4:396433e30e64 590 //if(calibDonePin)
rrknight 4:396433e30e64 591 calibrate();
rrknight 4:396433e30e64 592
rrknight 0:18d7499b82f3 593 device.reply(0x62); //Prime SPI with first reply
rrknight 0:18d7499b82f3 594
rrknight 0:18d7499b82f3 595 //gather first pack of data
rrknight 2:7ab1d5918efe 596 //get the sensor values
rrknight 3:c16d726670b2 597 updateTxSPIBuffer();
rrknight 0:18d7499b82f3 598
rrknight 2:7ab1d5918efe 599 //update checksum
rrknight 2:7ab1d5918efe 600 calculateSPIChecksum();
rrknight 0:18d7499b82f3 601
rrknight 0:18d7499b82f3 602 //then start the main loop
rrknight 0:18d7499b82f3 603 pc.printf("--- Start main loop ---\n\r");
rrknight 1:b430b4401fc4 604
rrknight 0:18d7499b82f3 605 while(1)
rrknight 2:7ab1d5918efe 606 {
rrknight 2:7ab1d5918efe 607 ledchain[0] = 0; //not selected by master
rrknight 2:7ab1d5918efe 608 ledchain[3] = 0; //no commands played
rrknight 2:7ab1d5918efe 609
rrknight 0:18d7499b82f3 610 //wait, the master will put the pin high at some point, for 10us
rrknight 0:18d7499b82f3 611 while(sync_master == 0)
rrknight 0:18d7499b82f3 612 {
rrknight 2:7ab1d5918efe 613 // logicPin = 1;
rrknight 0:18d7499b82f3 614 wait_us(1);
rrknight 2:7ab1d5918efe 615 // logicPin = 0;
rrknight 0:18d7499b82f3 616 }
rrknight 0:18d7499b82f3 617
rrknight 0:18d7499b82f3 618 slaveSelected = true;
rrknight 2:7ab1d5918efe 619 ledchain[0] = 1;
rrknight 0:18d7499b82f3 620
rrknight 2:7ab1d5918efe 621 while(LPC_SSP1->SR & RNE) // While RNE-Bit = 1 (FIFO receive buffer not empty)...
rrknight 0:18d7499b82f3 622 my_val = LPC_SSP1->DR; // Read the byte in the buffer
rrknight 2:7ab1d5918efe 623 /*
rrknight 2:7ab1d5918efe 624 {
rrknight 2:7ab1d5918efe 625 my_val = LPC_SSP1->DR; // Read the byte in the buffer
rrknight 2:7ab1d5918efe 626
rrknight 2:7ab1d5918efe 627 logicPin = 1;
rrknight 2:7ab1d5918efe 628 wait_us(1);
rrknight 2:7ab1d5918efe 629 logicPin = 0;
rrknight 2:7ab1d5918efe 630 }
rrknight 2:7ab1d5918efe 631 */
rrknight 0:18d7499b82f3 632 //reset for a new message
rrknight 0:18d7499b82f3 633 i = 0;
rrknight 0:18d7499b82f3 634 j = 0;
rrknight 0:18d7499b82f3 635 threeArrows = 0;
rrknight 0:18d7499b82f3 636 threeArrowsFound = false;
rrknight 2:7ab1d5918efe 637 checksumReceived = false;
rrknight 0:18d7499b82f3 638
rrknight 1:b430b4401fc4 639 logicPin = 1;
rrknight 0:18d7499b82f3 640
rrknight 0:18d7499b82f3 641 __disable_irq();
rrknight 0:18d7499b82f3 642
rrknight 0:18d7499b82f3 643 while(slaveSelected)
rrknight 2:7ab1d5918efe 644 {
rrknight 0:18d7499b82f3 645 //SPI polling
rrknight 0:18d7499b82f3 646 if(device.receive())
rrknight 1:b430b4401fc4 647 {
rrknight 1:b430b4401fc4 648 rByte = device.read(); // Read byte from master
rrknight 0:18d7499b82f3 649 //pc.printf("0x%02X ", rByte);
rrknight 0:18d7499b82f3 650
rrknight 1:b430b4401fc4 651 if(threeArrows < 3)
rrknight 0:18d7499b82f3 652 {
rrknight 1:b430b4401fc4 653 if(rByte == OPEN_ARROW)
rrknight 1:b430b4401fc4 654 {
rrknight 1:b430b4401fc4 655 threeArrows++;
rrknight 1:b430b4401fc4 656 //pc.printf("3A++\n\r");
rrknight 1:b430b4401fc4 657 }
rrknight 1:b430b4401fc4 658 else
rrknight 1:b430b4401fc4 659 {
rrknight 1:b430b4401fc4 660 //threeArrows = 0;
rrknight 1:b430b4401fc4 661 //startReceiving = false;
rrknight 1:b430b4401fc4 662 //pc.printf("error3A\n");
rrknight 1:b430b4401fc4 663 //slaveSelected = false;
rrknight 1:b430b4401fc4 664 }
rrknight 1:b430b4401fc4 665
rrknight 1:b430b4401fc4 666 if(threeArrows == 3)
rrknight 1:b430b4401fc4 667 {
rrknight 1:b430b4401fc4 668 device.reply(writeBufferSPI[i][j]);
rrknight 1:b430b4401fc4 669 threeArrowsFound = true;
rrknight 1:b430b4401fc4 670 }
rrknight 1:b430b4401fc4 671 else
rrknight 1:b430b4401fc4 672 {
rrknight 1:b430b4401fc4 673 device.reply(0x62); //close arrow : >
rrknight 1:b430b4401fc4 674 }
rrknight 1:b430b4401fc4 675 }
rrknight 1:b430b4401fc4 676 else
rrknight 1:b430b4401fc4 677 {
rrknight 1:b430b4401fc4 678 readBufferSPI[i][j] = rByte;
rrknight 1:b430b4401fc4 679
rrknight 1:b430b4401fc4 680 j++; //write next byte next time
rrknight 1:b430b4401fc4 681
rrknight 1:b430b4401fc4 682 if(j >= NUMBER_BYTES_PER_MSG)
rrknight 1:b430b4401fc4 683 {
rrknight 1:b430b4401fc4 684 j = 0;
rrknight 1:b430b4401fc4 685 i++; //next node
rrknight 0:18d7499b82f3 686
rrknight 1:b430b4401fc4 687 if(i >= NUMBER_MAX_EPOS2_PER_SLAVE)
rrknight 1:b430b4401fc4 688 {
rrknight 1:b430b4401fc4 689 //finished reading the array
rrknight 1:b430b4401fc4 690 /*
rrknight 1:b430b4401fc4 691 for(int n=0; n<1; n++)
rrknight 0:18d7499b82f3 692 {
rrknight 1:b430b4401fc4 693 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 694 }
rrknight 1:b430b4401fc4 695 */
rrknight 1:b430b4401fc4 696
rrknight 1:b430b4401fc4 697 //reset
rrknight 1:b430b4401fc4 698 threeArrows = 0;
rrknight 1:b430b4401fc4 699 i = 0;
rrknight 1:b430b4401fc4 700 j = 0;
rrknight 1:b430b4401fc4 701 slaveSelected = false; //to end the while loop
rrknight 2:7ab1d5918efe 702 /*
rrknight 2:7ab1d5918efe 703 logicPin = 1;
rrknight 2:7ab1d5918efe 704 wait_us(1);
rrknight 2:7ab1d5918efe 705 logicPin = 0;*/
rrknight 1:b430b4401fc4 706 }
rrknight 1:b430b4401fc4 707 }
rrknight 1:b430b4401fc4 708
rrknight 1:b430b4401fc4 709 if(slaveSelected) device.reply(writeBufferSPI[i][j]);
rrknight 1:b430b4401fc4 710 else device.reply(dataChecksum); //checksum
rrknight 1:b430b4401fc4 711 }
rrknight 0:18d7499b82f3 712 }//if
rrknight 0:18d7499b82f3 713
rrknight 0:18d7499b82f3 714 wait_us(1);
rrknight 0:18d7499b82f3 715 }//while slaveSelected
rrknight 2:7ab1d5918efe 716
rrknight 2:7ab1d5918efe 717 //wait_us(30);
rrknight 2:7ab1d5918efe 718 //logicPin = 1;
rrknight 1:b430b4401fc4 719 //read the checksum
rrknight 1:b430b4401fc4 720 while(!checksumReceived)
rrknight 1:b430b4401fc4 721 {
rrknight 2:7ab1d5918efe 722 //pc.printf("w");
rrknight 1:b430b4401fc4 723 if(device.receive())
rrknight 1:b430b4401fc4 724 {
rrknight 1:b430b4401fc4 725 cmdChecksum = device.read();
rrknight 3:c16d726670b2 726 //pc.printf("%02X\n", cmdChecksum);
rrknight 1:b430b4401fc4 727 cmdValid = verifyChecksum();
rrknight 1:b430b4401fc4 728 checksumReceived = true; //exit while loop
rrknight 1:b430b4401fc4 729 }
rrknight 1:b430b4401fc4 730
rrknight 1:b430b4401fc4 731 wait_us(1);
rrknight 2:7ab1d5918efe 732 }
rrknight 0:18d7499b82f3 733
rrknight 2:7ab1d5918efe 734
rrknight 2:7ab1d5918efe 735 __enable_irq();
rrknight 2:7ab1d5918efe 736
rrknight 2:7ab1d5918efe 737 logicPin = 0;
rrknight 3:c16d726670b2 738 /*
rrknight 3:c16d726670b2 739 //print some data:
rrknight 3:c16d726670b2 740 for(int n=0; n<1; n++)
rrknight 3:c16d726670b2 741 {
rrknight 3:c16d726670b2 742 pc.printf("%02X %02X %02X %02X %02X %02X %02X %02X\n", readBufferSPI[n][0], readBufferSPI[n][1], readBufferSPI[n][2], readBufferSPI[n][3], readBufferSPI[n][4], readBufferSPI[n][5], readBufferSPI[n][6], readBufferSPI[n][7]);
rrknight 3:c16d726670b2 743 }
rrknight 3:c16d726670b2 744 */
rrknight 3:c16d726670b2 745 //pc.printf("%02X\n", readBufferSPI[0][4]);
rrknight 3:c16d726670b2 746
rrknight 2:7ab1d5918efe 747 wait_us(30);
rrknight 3:c16d726670b2 748 /* logicPin = 1;
rrknight 0:18d7499b82f3 749 wait_us(10);
rrknight 2:7ab1d5918efe 750 logicPin = 0;
rrknight 2:7ab1d5918efe 751 wait_us(20);
rrknight 2:7ab1d5918efe 752 */
rrknight 1:b430b4401fc4 753 //if checksum is correct, then play the cmds
rrknight 1:b430b4401fc4 754 if(cmdValid)
rrknight 1:b430b4401fc4 755 {
rrknight 2:7ab1d5918efe 756 logicPin = 1;
rrknight 2:7ab1d5918efe 757 //play the commands
rrknight 2:7ab1d5918efe 758 commandPlayer();
rrknight 1:b430b4401fc4 759 cmdValid = false; //reset for next packet
rrknight 2:7ab1d5918efe 760 logicPin = 0;
rrknight 1:b430b4401fc4 761 }
rrknight 2:7ab1d5918efe 762 /*
rrknight 2:7ab1d5918efe 763 wait_us(20);
rrknight 2:7ab1d5918efe 764 logicPin = 1;
rrknight 1:b430b4401fc4 765 wait_us(10);
rrknight 2:7ab1d5918efe 766 logicPin = 0;
rrknight 2:7ab1d5918efe 767 */
rrknight 2:7ab1d5918efe 768 //get the sensor values ready to be sent for the next loop
rrknight 2:7ab1d5918efe 769 //update the writeBufferSPI
rrknight 3:c16d726670b2 770 updateTxSPIBuffer(); //test with known data first
rrknight 1:b430b4401fc4 771
rrknight 2:7ab1d5918efe 772 //compute checksum
rrknight 2:7ab1d5918efe 773 calculateSPIChecksum();
rrknight 0:18d7499b82f3 774
rrknight 2:7ab1d5918efe 775 //logicPin = 0;
rrknight 2:7ab1d5918efe 776 //ledchain[0] = 0;
rrknight 2:7ab1d5918efe 777 wait_us(10);
rrknight 0:18d7499b82f3 778 }// main while end
rrknight 0:18d7499b82f3 779 }// main end
rrknight 0:18d7499b82f3 780
rrknight 2:7ab1d5918efe 781 #endif //COMPILE_MAIN_CODE_TRS_SLAVE