Mid level control code

Dependencies:   ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers LLComms.cpp Source File

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); }