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