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@6:8a0250a4877a, 2015-02-11 (annotated)

Committer:
therobotstudio
Date:
Wed Feb 11 16:18:15 2015 +0000
Revision:
6:8a0250a4877a
Parent:
5:52e3137c2831 
v1.0.0

Who changed what in which revision?

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