MorphGI / Mbed OS MGI_Rebuild_MidLevel_9_0-Basic_PWM

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@14:54c3759e76ed, 2018-08-29 (annotated)

Committer:
WD40andTape
Date:
Wed Aug 29 16:31:42 2018 +0000
Revision:
14:54c3759e76ed
Parent:
13:a373dfc57b89
Child:
21:0b10d8e615d1
Temp fix for actuator numbering and added handling to only replan some actuators. Tested and working with 6 actuators.

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