Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.

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