Mid level control code
Dependencies: ros_lib_kinetic
LLComms.cpp
00001 // LLComms.cpp 00002 00003 #include "LLComms.h" 00004 00005 LLComms::LLComms() : 00006 queue(32 * EVENTS_EVENT_SIZE), //32 //8 * EVENTS_EVENT_SIZE 00007 pinGate6(PA_5), 00008 spi_0(PC_12, PC_11, PC_10), 00009 spi_1(PF_9, PF_8, PF_7), 00010 spi_2(PB_15, PC_2, PB_13), 00011 // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why. 00012 pinGate0(PF_11), 00013 pinGate1(PG_14), 00014 pinGate2(PF_15), 00015 pinGate3(PF_12), 00016 pinGate4(PF_3), 00017 pinGate5(PC_7), 00018 pinGate7(PE_13), 00019 pinGate8(PF_4), 00020 pinReset(PD_2) 00021 { // Constructor 00022 00023 spi_0.format(16,2); 00024 spi_0.frequency(LOW_LEVEL_SPI_FREQUENCY); 00025 spi_1.format(16,2); 00026 spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY); 00027 spi_2.format(16,2); 00028 spi_2.frequency(LOW_LEVEL_SPI_FREQUENCY); 00029 00030 PinName LLPins[N_CHANNELS] = {PD_15, PE_10, PD_11, PD_14, PE_7, PB_1, PB_12, PD_13, PB_5}; 00031 for (short int i=0; i<N_CHANNELS; i++) { 00032 isDataReady[i] = 0; 00033 cs_LL[i] = new DigitalOut(LLPins[i]); 00034 //cs_ADC[i] = new DigitalOut(ADCPins[i]); 00035 } 00036 00037 // Initialise relevant variables 00038 for(short int i=0; i<N_CHANNELS; i++) { 00039 // All chip selects in off state 00040 *cs_LL[i] = 1; 00041 //*cs_ADC[i] = 1; 00042 // Initialise pressures/positions 00043 pressureSensor_uint[i] = 0.0; 00044 pressureSensor_bar[i] = -1.0; 00045 positionSensor_uint[i] = 0.0; 00046 positionSensor_mm[i] = -1.0; 00047 } 00048 pinReset = 1; // Initialise reset pin to not reset the controllers. 00049 wait(0.25); 00050 pinReset=0; // Reset controllers to be safe 00051 wait(0.25); 00052 pinReset = 1; // Ready to go 00053 00054 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS 00055 pinGate0.rise(callback(this,&LLComms::rise0)); 00056 pinGate1.rise(callback(this,&LLComms::rise1)); 00057 pinGate2.rise(callback(this,&LLComms::rise2)); 00058 pinGate3.rise(callback(this,&LLComms::rise3)); 00059 pinGate4.rise(callback(this,&LLComms::rise4)); 00060 pinGate5.rise(callback(this,&LLComms::rise5)); 00061 pinGate6.rise(callback(this,&LLComms::rise6)); 00062 pinGate7.rise(callback(this,&LLComms::rise7)); 00063 pinGate8.rise(callback(this,&LLComms::rise8)); 00064 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS 00065 pinGate0.fall(callback(this,&LLComms::fall0)); 00066 pinGate1.fall(callback(this,&LLComms::fall1)); 00067 pinGate2.fall(callback(this,&LLComms::fall2)); 00068 pinGate3.fall(callback(this,&LLComms::fall3)); 00069 pinGate4.fall(callback(this,&LLComms::fall4)); 00070 pinGate5.fall(callback(this,&LLComms::fall5)); 00071 pinGate6.fall(callback(this,&LLComms::fall6)); 00072 pinGate7.fall(callback(this,&LLComms::fall7)); 00073 pinGate8.fall(callback(this,&LLComms::fall8)); 00074 } 00075 00076 //LLComms::~LLComms(void) { } // Destructor 00077 unsigned int LLComms::formatMessage(short int type, double dblValue, double dblMaxValue) { 00078 // Convert to a 9-bit number 00079 int intValue = (int) ((dblValue/dblMaxValue)*511); 00080 intValue = intValue & 0x1FF; // Ensure number is 9-bit 00081 // Initialize message with value 00082 unsigned int intMsg = intValue; 00083 // Calculate checksum (the decimal sum of the position data) 00084 int intCheckSum = 0, intTempVar = intValue; 00085 while( intTempVar>0 ) { 00086 intCheckSum += intTempVar%10; 00087 intTempVar = floor(intTempVar/10.0); 00088 } 00089 // Add checksum to message 00090 intMsg = intMsg<<5; 00091 intMsg = intMsg | intCheckSum; 00092 // Add type bit (0 == position, 1 == velocity) 00093 intMsg = intMsg<<1; 00094 intMsg = intMsg | (int)type; // CAST AS BOOL 00095 // Calculate decimal parity check for the whole message 00096 unsigned int count = 0, b = 1; 00097 for(short int i=0; i<32; i++) { 00098 if( intMsg & (b << i) ) count++; 00099 } 00100 // Add parity bit to message (0 == Odd, 1 == Even) 00101 // Parity selected in this way to prevent 0x0000 from passing checks 00102 bool boolParity = !(bool)(count%2); 00103 intMsg = intMsg<<1; 00104 intMsg = intMsg | (int)boolParity; 00105 return intMsg; 00106 } 00107 00108 bool LLComms::CheckMessage(int msg) { 00109 // Find message parity 00110 short int count = 0; 00111 for(short int i=0; i<32; i++) { 00112 if( msg>>1 & (1<<i) ) count++; 00113 } 00114 int intParity = !(count%2); 00115 // Find message CheckSum 00116 int intChkSum = 0; 00117 int intTempVar = msg>>7; 00118 while(intTempVar > 0) { 00119 intChkSum += intTempVar%10; 00120 intTempVar = int(intTempVar/10); 00121 } 00122 // Check if parity and CheckSum match 00123 bool isParityCorrect = (intParity == (msg&0x1)); 00124 bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F)); 00125 bool isCheckPassed = (isParityCorrect && isChkSumCorrect); 00126 return isCheckPassed; 00127 } 00128 00129 bool LLComms::PerformMasterSPI(SPI *spi, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) { 00130 unsigned int dummyMsg = 0x5555; 00131 bool isSuccess = true; 00132 unsigned int inboundMsg, typeBit; 00133 AnalogIn ain(PA_4); // Random analogue pin 00134 for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages 00135 ain.read_u16(); // Read analogue pin to cause a delay 00136 ain.read_u16(); 00137 if(i==0) { 00138 inboundMsg = spi->write(outboundMsgs[0]); 00139 } else if(i==1) { 00140 inboundMsg = spi->write(outboundMsgs[1]); 00141 } else { 00142 inboundMsg = spi->write(dummyMsg); 00143 } 00144 if((unsigned int)inboundMsg != dummyMsg) { // Message is not dummy which is only used for reply 00145 typeBit = inboundMsg>>1 & 0x1; 00146 inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF; 00147 if( !CheckMessage(inboundMsg) ) { 00148 isSuccess = false; 00149 } 00150 } 00151 } 00152 return isSuccess; 00153 } 00154 00155 void LLComms::SendReceiveData(int channel) { 00156 mutChannel[channel].lock(); // Lock mutex for specific Channel 00157 00158 // Construct messages 00159 unsigned int intPositionMsg = formatMessage(0,demandPosition_mm[channel],MAX_ACTUATOR_LIMIT_MM); 00160 unsigned int intVelocityMsg = formatMessage(1,demandSpeed_mmps[channel],MAX_SPEED_MMPS); 00161 00162 *cs_LL[channel] = 0; // Select relevant chip 00163 unsigned int outboundMsgs[2] = { intPositionMsg , intVelocityMsg }; 00164 unsigned int inboundMsgsData[2] = { 0 }; 00165 bool isSPIsuccess = false; 00166 if( channel < 4 ) { 00167 isSPIsuccess = PerformMasterSPI(&spi_0,outboundMsgs,inboundMsgsData); 00168 } else if( channel < 7 ) { 00169 isSPIsuccess = PerformMasterSPI(&spi_1,outboundMsgs,inboundMsgsData); 00170 } else { 00171 isSPIsuccess = PerformMasterSPI(&spi_2,outboundMsgs,inboundMsgsData); 00172 } 00173 *cs_LL[channel] = 1; // Deselect chip 00174 if( isSPIsuccess ) { 00175 isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data 00176 positionSensor_uint[channel] = inboundMsgsData[0]; 00177 positionSensor_mm[channel] = ((double)inboundMsgsData[0]/511) * (double)MAX_ACTUATOR_LENGTH_MM; 00178 positionSensor_mm[channel] = min( max(positionSensor_mm[channel],0.0) , (double)MAX_ACTUATOR_LENGTH_MM ); 00179 pressureSensor_uint[channel] = inboundMsgsData[1]; 00180 pressureSensor_bar[channel] = ((double)inboundMsgsData[1]/511) * (double)MAX_PRESSURE_LIMIT; 00181 pressureSensor_bar[channel] = min( max(pressureSensor_bar[channel],0.0) , (double)MAX_PRESSURE_LIMIT ); 00182 } else { // Data is STILL ready and will be resent at the next pin interrupt 00183 //printf("SPI failed: %d%d. Resending.\n\r",isPositionValid,isPressureValid); 00184 } 00185 00186 mutChannel[channel].unlock();//unlock mutex for specific channel 00187 } 00188 00189 // Common rise handler function 00190 void LLComms::common_rise_handler(int channel) { 00191 //printf("%d %d common_rise_handler\n\r",channel,isDataReady[channel]); 00192 if (isDataReady[channel]) { // Check if data is ready for transmission 00193 ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission 00194 } 00195 } 00196 00197 // Common fall handler functions 00198 void LLComms::common_fall_handler(int channel) { 00199 queue.cancel(ThreadID[channel]); // Cancel relevant queued event 00200 } 00201 00202 // Stub rise functions 00203 void LLComms::rise0(void) { common_rise_handler(0); } 00204 void LLComms::rise1(void) { common_rise_handler(1); } 00205 void LLComms::rise2(void) { common_rise_handler(2); } 00206 void LLComms::rise3(void) { common_rise_handler(3); } 00207 void LLComms::rise4(void) { common_rise_handler(4); } 00208 void LLComms::rise5(void) { common_rise_handler(5); } 00209 void LLComms::rise6(void) { common_rise_handler(6); } 00210 void LLComms::rise7(void) { common_rise_handler(7); } 00211 void LLComms::rise8(void) { common_rise_handler(8); } 00212 // Stub fall functions 00213 void LLComms::fall0(void) { common_fall_handler(0); } 00214 void LLComms::fall1(void) { common_fall_handler(1); } 00215 void LLComms::fall2(void) { common_fall_handler(2); } 00216 void LLComms::fall3(void) { common_fall_handler(3); } 00217 void LLComms::fall4(void) { common_fall_handler(4); } 00218 void LLComms::fall5(void) { common_fall_handler(5); } 00219 void LLComms::fall6(void) { common_fall_handler(6); } 00220 void LLComms::fall7(void) { common_fall_handler(7); } 00221 void LLComms::fall8(void) { common_fall_handler(8); }
Generated on Mon Jul 25 2022 18:11:35 by
1.7.2