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@0:18d7499b82f3, 2013-02-27 (annotated)

Committer:
rrknight
Date:
Wed Feb 27 14:53:45 2013 +0000
Revision:
0:18d7499b82f3
Child:
1:b430b4401fc4
empty SPI Buffer works !

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