Mid level control code

Dependencies:   ros_lib_kinetic

LLComms.cpp

Committer:
WD40andTape
Date:
2018-08-07
Revision:
9:cd3607ba5643
Parent:
8:d6657767a182
Child:
10:1b6daba32452

File content as of revision 9:cd3607ba5643:

// LLComms.cpp

#include "LLComms.h"

LLComms::LLComms(int spi_frequency) : 
    pinGate6(PE_11),
    spi(PC_12, PC_11, PC_10),
    pinCheck(PE_5),
    // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
    pinGate0(PF_11),
    pinGate1(PG_14),
    pinGate2(PF_15),
    pinGate3(PF_12),
    pinGate4(PF_3),
    pinGate5(PF_13),
    //pinGate6(PE_11), // See above nonsense
    pinGate7(PE_13),
    pinReset(PD_2) 
{ // Constructor
    _spi_frequency = spi_frequency;

    PinName LLPins[8] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};
    PinName ADCPins[8] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1};
    for (short int i = 0; i < 8; i++) {
        cs_LL[i] = new DigitalOut(LLPins[i]);
        cs_ADC[i] = new DigitalOut(ADCPins[i]);
    }
}

void LLComms::reset(void) {
    // Initialise relevant variables
    for(int i = 0; i<_n_channels; i++) {
        // All chip selects in off state
        *cs_LL[i] = 1;
        *cs_ADC[i] = 1;
    }
    pinReset = 1; // Initialise reset pin to not reset the controllers.
    wait(0.25);
    pinReset=0; // Reset controllers to be safe
    wait(0.25);
    pinReset = 1; // Ready to go
}

double LLComms::ReadADCPosition_mtrs(int channel) {
    unsigned int outputA;
    unsigned int outputB;
    int output;
    double dblOutput;

    spi.format(8,0);
    spi.frequency(1000000);
    
    *cs_ADC[channel] = 0;
    spi.write(PREAMBLE);
    outputA = spi.write(CHAN_3);
    outputB = spi.write(0xFF);
    *cs_ADC[channel] = 1;
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    output = 4095- output;
    dblOutput = (double) (output);
    dblOutput = dblOutput*0.0229 - 21.582;
    return dblOutput;
}
    
double LLComms::ReadADCPressure_bar(int channel) {
    unsigned int outputA;
    unsigned int outputB;
    int output;
    double dblOutput;

    spi.format(8,0);
    spi.frequency(1000000);
    
    *cs_ADC[channel] = 0;
    spi.write(PREAMBLE);
    outputA = spi.write(CHAN_1);
    outputB = spi.write(0xFF);
    *cs_ADC[channel] = 1;
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    
    dblOutput = (double)(output);
    dblOutput = dblOutput-502.0;
    dblOutput = dblOutput/4095.0*8.0;
    return dblOutput;
}

// Common fall handler functions
/*void LLComms::dcommon_fall_handler(int channel) {
    pinCheck = 0;
    //queue.cancel(ThreadID[channel]); // Cancel relevant queued event
}
void LLComms::dfall0(void) { dcommon_fall_handler(0); }
void LLComms::dSendReceiveData(int channel, int _intDemandPos_Tx[], bool (*_isDataReady)[8]) {
    printf("%i\n\r",channel);
}*/