MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

LLComms.cpp@13:a373dfc57b89, 2018-08-29 (annotated)

Committer:
WD40andTape
Date:
Wed Aug 29 10:47:02 2018 +0000
Revision:
13:a373dfc57b89
Parent:
12:595ed862e52f
Child:
14:54c3759e76ed
Added limits to smooth path plan and fixed path replan (isTimeChanged) logic.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 8:d6657767a182 1 // LLComms.cpp
WD40andTape 8:d6657767a182 2
WD40andTape 8:d6657767a182 3 #include "LLComms.h"
WD40andTape 8:d6657767a182 4
WD40andTape 13:a373dfc57b89 5 //DigitalOut pinTesty(PB_8);
WD40andTape 13:a373dfc57b89 6
dofydoink 12:595ed862e52f 7 LLComms::LLComms() :
WD40andTape 13:a373dfc57b89 8 queue(32 * EVENTS_EVENT_SIZE), //32 //8 * EVENTS_EVENT_SIZE
WD40andTape 8:d6657767a182 9 pinGate6(PE_11),
WD40andTape 8:d6657767a182 10 spi(PC_12, PC_11, PC_10),
WD40andTape 8:d6657767a182 11 pinCheck(PE_5),
WD40andTape 8:d6657767a182 12 // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
WD40andTape 8:d6657767a182 13 pinGate0(PF_11),
WD40andTape 8:d6657767a182 14 pinGate1(PG_14),
WD40andTape 8:d6657767a182 15 pinGate2(PF_15),
WD40andTape 8:d6657767a182 16 pinGate3(PF_12),
WD40andTape 8:d6657767a182 17 pinGate4(PF_3),
WD40andTape 8:d6657767a182 18 pinGate5(PF_13),
WD40andTape 8:d6657767a182 19 //pinGate6(PE_11), // See above nonsense
WD40andTape 8:d6657767a182 20 pinGate7(PE_13),
dofydoink 12:595ed862e52f 21 pinReset(PD_2)
WD40andTape 8:d6657767a182 22 { // Constructor
WD40andTape 8:d6657767a182 23
dofydoink 12:595ed862e52f 24 PinName LLPins[8] = {PD_15, PE_10, PD_11, PD_14, PE_7, PD_12, PF_10, PD_13};
dofydoink 12:595ed862e52f 25 //PinName LLPins[8] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};
WD40andTape 8:d6657767a182 26 PinName ADCPins[8] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1};
WD40andTape 8:d6657767a182 27 for (short int i = 0; i < 8; i++) {
WD40andTape 10:1b6daba32452 28 isDataReady[i] = 0;
WD40andTape 8:d6657767a182 29 cs_LL[i] = new DigitalOut(LLPins[i]);
WD40andTape 8:d6657767a182 30 cs_ADC[i] = new DigitalOut(ADCPins[i]);
WD40andTape 8:d6657767a182 31 }
WD40andTape 10:1b6daba32452 32
WD40andTape 9:cd3607ba5643 33 // Initialise relevant variables
dofydoink 11:7029367a1840 34 for(short int i = 0; i<N_CHANNELS; i++) {
WD40andTape 9:cd3607ba5643 35 // All chip selects in off state
WD40andTape 9:cd3607ba5643 36 *cs_LL[i] = 1;
WD40andTape 9:cd3607ba5643 37 *cs_ADC[i] = 1;
WD40andTape 9:cd3607ba5643 38 }
WD40andTape 9:cd3607ba5643 39 pinReset = 1; // Initialise reset pin to not reset the controllers.
WD40andTape 9:cd3607ba5643 40 wait(0.25);
WD40andTape 9:cd3607ba5643 41 pinReset=0; // Reset controllers to be safe
WD40andTape 9:cd3607ba5643 42 wait(0.25);
WD40andTape 9:cd3607ba5643 43 pinReset = 1; // Ready to go
WD40andTape 10:1b6daba32452 44
WD40andTape 10:1b6daba32452 45 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 46 pinGate0.rise(callback(this,&LLComms::rise0));
dofydoink 12:595ed862e52f 47 pinGate1.rise(callback(this,&LLComms::rise1));
dofydoink 12:595ed862e52f 48 pinGate2.rise(callback(this,&LLComms::rise2));
dofydoink 12:595ed862e52f 49 pinGate3.rise(callback(this,&LLComms::rise3));
dofydoink 12:595ed862e52f 50 pinGate4.rise(callback(this,&LLComms::rise4));
dofydoink 12:595ed862e52f 51 pinGate5.rise(callback(this,&LLComms::rise5));
dofydoink 12:595ed862e52f 52 pinGate6.rise(callback(this,&LLComms::rise6));
dofydoink 12:595ed862e52f 53 pinGate7.rise(callback(this,&LLComms::rise7));
WD40andTape 10:1b6daba32452 54 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 55 pinGate0.fall(callback(this,&LLComms::fall0));
dofydoink 12:595ed862e52f 56 pinGate1.fall(callback(this,&LLComms::fall1));
dofydoink 12:595ed862e52f 57 pinGate2.fall(callback(this,&LLComms::fall2));
dofydoink 12:595ed862e52f 58 pinGate3.fall(callback(this,&LLComms::fall3));
dofydoink 12:595ed862e52f 59 pinGate4.fall(callback(this,&LLComms::fall4));
dofydoink 12:595ed862e52f 60 pinGate5.fall(callback(this,&LLComms::fall5));
dofydoink 12:595ed862e52f 61 pinGate6.fall(callback(this,&LLComms::fall6));
dofydoink 12:595ed862e52f 62 pinGate7.fall(callback(this,&LLComms::fall7));
WD40andTape 9:cd3607ba5643 63 }
WD40andTape 9:cd3607ba5643 64
dofydoink 11:7029367a1840 65 //LLComms::~LLComms(void) { } // Destructor
dofydoink 11:7029367a1840 66
WD40andTape 10:1b6daba32452 67 void LLComms::SendReceiveData(int channel) {
WD40andTape 13:a373dfc57b89 68 //pinTesty = 1;
dofydoink 11:7029367a1840 69 int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
WD40andTape 10:1b6daba32452 70
WD40andTape 10:1b6daba32452 71 // Get data from controller
WD40andTape 10:1b6daba32452 72 spi.format(16,2);
dofydoink 12:595ed862e52f 73 spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 10:1b6daba32452 74 mutChannel[channel].lock(); // Lock mutex for specific Channel
WD40andTape 10:1b6daba32452 75 *cs_LL[channel] = 0; // Select relevant chip
WD40andTape 10:1b6daba32452 76 intPosSPI_Rx[channel] = spi.write(demandPosition[channel]); // Transmit & receive
WD40andTape 10:1b6daba32452 77 *cs_LL[channel] = 1; // Deselect chip
WD40andTape 10:1b6daba32452 78 isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
WD40andTape 10:1b6daba32452 79 /*if(channel == 0) {
WD40andTape 10:1b6daba32452 80 intGlobalTest = intPosSPI_Rx[channel];
WD40andTape 10:1b6daba32452 81 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
WD40andTape 10:1b6daba32452 82 }*/
WD40andTape 10:1b6daba32452 83
WD40andTape 10:1b6daba32452 84 // Sort out received data
WD40andTape 10:1b6daba32452 85 chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
WD40andTape 10:1b6daba32452 86
WD40andTape 10:1b6daba32452 87 intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
WD40andTape 10:1b6daba32452 88 //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 10:1b6daba32452 89 mutChannel[channel].unlock();//unlock mutex for specific channel
dofydoink 12:595ed862e52f 90 //if(IS_PRINT_OUTPUT) printf("%d, %d\r\n",intPosSPI_Rx[0],_intDemandPos_Tx[0]);
WD40andTape 13:a373dfc57b89 91 //pinTesty = 0;
WD40andTape 10:1b6daba32452 92 }
WD40andTape 10:1b6daba32452 93
WD40andTape 10:1b6daba32452 94 // Common rise handler function
WD40andTape 10:1b6daba32452 95 void LLComms::common_rise_handler(int channel) {
WD40andTape 10:1b6daba32452 96 pinCheck = 1;
WD40andTape 10:1b6daba32452 97 if (isDataReady[channel]) { // Check if data is ready for tranmission
WD40andTape 10:1b6daba32452 98 ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
WD40andTape 10:1b6daba32452 99 }
WD40andTape 10:1b6daba32452 100 }
WD40andTape 10:1b6daba32452 101
WD40andTape 10:1b6daba32452 102 // Common fall handler functions
WD40andTape 10:1b6daba32452 103 void LLComms::common_fall_handler(int channel) {
WD40andTape 10:1b6daba32452 104 pinCheck = 0;
WD40andTape 10:1b6daba32452 105 queue.cancel(ThreadID[channel]); // Cancel relevant queued event
WD40andTape 10:1b6daba32452 106 }
WD40andTape 10:1b6daba32452 107
WD40andTape 10:1b6daba32452 108 // Stub rise functions
WD40andTape 10:1b6daba32452 109 void LLComms::rise0(void) { common_rise_handler(0); }
WD40andTape 10:1b6daba32452 110 void LLComms::rise1(void) { common_rise_handler(1); }
WD40andTape 10:1b6daba32452 111 void LLComms::rise2(void) { common_rise_handler(2); }
WD40andTape 10:1b6daba32452 112 void LLComms::rise3(void) { common_rise_handler(3); }
WD40andTape 10:1b6daba32452 113 void LLComms::rise4(void) { common_rise_handler(4); }
WD40andTape 10:1b6daba32452 114 void LLComms::rise5(void) { common_rise_handler(5); }
WD40andTape 10:1b6daba32452 115 void LLComms::rise6(void) { common_rise_handler(6); }
WD40andTape 10:1b6daba32452 116 void LLComms::rise7(void) { common_rise_handler(7); }
WD40andTape 10:1b6daba32452 117 // Stub fall functions
WD40andTape 10:1b6daba32452 118 void LLComms::fall0(void) { common_fall_handler(0); }
WD40andTape 10:1b6daba32452 119 void LLComms::fall1(void) { common_fall_handler(1); }
WD40andTape 10:1b6daba32452 120 void LLComms::fall2(void) { common_fall_handler(2); }
WD40andTape 10:1b6daba32452 121 void LLComms::fall3(void) { common_fall_handler(3); }
WD40andTape 10:1b6daba32452 122 void LLComms::fall4(void) { common_fall_handler(4); }
WD40andTape 10:1b6daba32452 123 void LLComms::fall5(void) { common_fall_handler(5); }
WD40andTape 10:1b6daba32452 124 void LLComms::fall6(void) { common_fall_handler(6); }
WD40andTape 10:1b6daba32452 125 void LLComms::fall7(void) { common_fall_handler(7); }
WD40andTape 10:1b6daba32452 126
dofydoink 12:595ed862e52f 127 // NEEDS CALIBRATING
WD40andTape 8:d6657767a182 128 double LLComms::ReadADCPosition_mtrs(int channel) {
WD40andTape 8:d6657767a182 129 unsigned int outputA;
WD40andTape 8:d6657767a182 130 unsigned int outputB;
WD40andTape 8:d6657767a182 131 int output;
WD40andTape 8:d6657767a182 132 double dblOutput;
WD40andTape 8:d6657767a182 133
WD40andTape 8:d6657767a182 134 spi.format(8,0);
WD40andTape 8:d6657767a182 135 spi.frequency(1000000);
WD40andTape 8:d6657767a182 136
WD40andTape 8:d6657767a182 137 *cs_ADC[channel] = 0;
WD40andTape 8:d6657767a182 138 spi.write(PREAMBLE);
WD40andTape 8:d6657767a182 139 outputA = spi.write(CHAN_3);
WD40andTape 8:d6657767a182 140 outputB = spi.write(0xFF);
WD40andTape 8:d6657767a182 141 *cs_ADC[channel] = 1;
WD40andTape 8:d6657767a182 142
WD40andTape 8:d6657767a182 143 outputA = outputA & DATA_MASK;
WD40andTape 8:d6657767a182 144 outputA = outputA<<8;
WD40andTape 8:d6657767a182 145 output = (outputA | outputB);
WD40andTape 8:d6657767a182 146 output = 4095- output;
WD40andTape 8:d6657767a182 147 dblOutput = (double) (output);
WD40andTape 8:d6657767a182 148 dblOutput = dblOutput*0.0229 - 21.582;
WD40andTape 8:d6657767a182 149 return dblOutput;
WD40andTape 8:d6657767a182 150 }
WD40andTape 8:d6657767a182 151
WD40andTape 8:d6657767a182 152 double LLComms::ReadADCPressure_bar(int channel) {
WD40andTape 8:d6657767a182 153 unsigned int outputA;
WD40andTape 8:d6657767a182 154 unsigned int outputB;
WD40andTape 8:d6657767a182 155 int output;
WD40andTape 8:d6657767a182 156 double dblOutput;
WD40andTape 8:d6657767a182 157
WD40andTape 8:d6657767a182 158 spi.format(8,0);
WD40andTape 8:d6657767a182 159 spi.frequency(1000000);
WD40andTape 8:d6657767a182 160
WD40andTape 8:d6657767a182 161 *cs_ADC[channel] = 0;
WD40andTape 8:d6657767a182 162 spi.write(PREAMBLE);
WD40andTape 8:d6657767a182 163 outputA = spi.write(CHAN_1);
WD40andTape 8:d6657767a182 164 outputB = spi.write(0xFF);
WD40andTape 8:d6657767a182 165 *cs_ADC[channel] = 1;
WD40andTape 8:d6657767a182 166
WD40andTape 8:d6657767a182 167 outputA = outputA & DATA_MASK;
WD40andTape 8:d6657767a182 168 outputA = outputA<<8;
WD40andTape 8:d6657767a182 169 output = (outputA | outputB);
WD40andTape 8:d6657767a182 170
WD40andTape 8:d6657767a182 171 dblOutput = (double)(output);
WD40andTape 8:d6657767a182 172 dblOutput = dblOutput-502.0;
WD40andTape 8:d6657767a182 173 dblOutput = dblOutput/4095.0*8.0;
WD40andTape 8:d6657767a182 174 return dblOutput;
WD40andTape 10:1b6daba32452 175 }