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@23:61526647cc8a, 2018-12-14 (annotated)

Committer:
dofydoink
Date:
Fri Dec 14 09:58:24 2018 +0000
Revision:
23:61526647cc8a
Parent:
22:82871f00f89d
Child:
24:bc852aa89e7a
Updated communications. Still need to sort out how feedback is handled by ML.

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
dofydoink 12:595ed862e52f 5 LLComms::LLComms() :
WD40andTape 13:a373dfc57b89 6 queue(32 * EVENTS_EVENT_SIZE), //32 //8 * EVENTS_EVENT_SIZE
WD40andTape 8:d6657767a182 7 pinGate6(PE_11),
WD40andTape 14:54c3759e76ed 8 spi_0(PC_12, PC_11, PC_10),
WD40andTape 14:54c3759e76ed 9 spi_1(PF_9, PF_8, PF_7),
WD40andTape 8:d6657767a182 10 pinCheck(PE_5),
WD40andTape 8:d6657767a182 11 // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
WD40andTape 8:d6657767a182 12 pinGate0(PF_11),
WD40andTape 8:d6657767a182 13 pinGate1(PG_14),
WD40andTape 8:d6657767a182 14 pinGate2(PF_15),
WD40andTape 8:d6657767a182 15 pinGate3(PF_12),
WD40andTape 8:d6657767a182 16 pinGate4(PF_3),
WD40andTape 8:d6657767a182 17 pinGate5(PF_13),
WD40andTape 8:d6657767a182 18 //pinGate6(PE_11), // See above nonsense
WD40andTape 8:d6657767a182 19 pinGate7(PE_13),
dofydoink 12:595ed862e52f 20 pinReset(PD_2)
WD40andTape 8:d6657767a182 21 { // Constructor
WD40andTape 8:d6657767a182 22
dofydoink 12:595ed862e52f 23 PinName LLPins[8] = {PD_15, PE_10, PD_11, PD_14, PE_7, PD_12, PF_10, PD_13};
dofydoink 12:595ed862e52f 24 //PinName LLPins[8] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};
WD40andTape 8:d6657767a182 25 PinName ADCPins[8] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1};
WD40andTape 8:d6657767a182 26 for (short int i = 0; i < 8; i++) {
WD40andTape 10:1b6daba32452 27 isDataReady[i] = 0;
WD40andTape 8:d6657767a182 28 cs_LL[i] = new DigitalOut(LLPins[i]);
WD40andTape 8:d6657767a182 29 cs_ADC[i] = new DigitalOut(ADCPins[i]);
WD40andTape 8:d6657767a182 30 }
WD40andTape 10:1b6daba32452 31
WD40andTape 9:cd3607ba5643 32 // Initialise relevant variables
dofydoink 11:7029367a1840 33 for(short int i = 0; i<N_CHANNELS; i++) {
WD40andTape 9:cd3607ba5643 34 // All chip selects in off state
WD40andTape 9:cd3607ba5643 35 *cs_LL[i] = 1;
WD40andTape 9:cd3607ba5643 36 *cs_ADC[i] = 1;
WD40andTape 9:cd3607ba5643 37 }
WD40andTape 9:cd3607ba5643 38 pinReset = 1; // Initialise reset pin to not reset the controllers.
WD40andTape 9:cd3607ba5643 39 wait(0.25);
WD40andTape 9:cd3607ba5643 40 pinReset=0; // Reset controllers to be safe
WD40andTape 9:cd3607ba5643 41 wait(0.25);
WD40andTape 9:cd3607ba5643 42 pinReset = 1; // Ready to go
WD40andTape 10:1b6daba32452 43
WD40andTape 10:1b6daba32452 44 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 45 pinGate0.rise(callback(this,&LLComms::rise0));
dofydoink 12:595ed862e52f 46 pinGate1.rise(callback(this,&LLComms::rise1));
dofydoink 12:595ed862e52f 47 pinGate2.rise(callback(this,&LLComms::rise2));
dofydoink 12:595ed862e52f 48 pinGate3.rise(callback(this,&LLComms::rise3));
dofydoink 12:595ed862e52f 49 pinGate4.rise(callback(this,&LLComms::rise4));
dofydoink 12:595ed862e52f 50 pinGate5.rise(callback(this,&LLComms::rise5));
dofydoink 12:595ed862e52f 51 pinGate6.rise(callback(this,&LLComms::rise6));
dofydoink 12:595ed862e52f 52 pinGate7.rise(callback(this,&LLComms::rise7));
WD40andTape 10:1b6daba32452 53 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 54 pinGate0.fall(callback(this,&LLComms::fall0));
dofydoink 12:595ed862e52f 55 pinGate1.fall(callback(this,&LLComms::fall1));
dofydoink 12:595ed862e52f 56 pinGate2.fall(callback(this,&LLComms::fall2));
dofydoink 12:595ed862e52f 57 pinGate3.fall(callback(this,&LLComms::fall3));
dofydoink 12:595ed862e52f 58 pinGate4.fall(callback(this,&LLComms::fall4));
dofydoink 12:595ed862e52f 59 pinGate5.fall(callback(this,&LLComms::fall5));
dofydoink 12:595ed862e52f 60 pinGate6.fall(callback(this,&LLComms::fall6));
dofydoink 12:595ed862e52f 61 pinGate7.fall(callback(this,&LLComms::fall7));
WD40andTape 9:cd3607ba5643 62 }
WD40andTape 9:cd3607ba5643 63
dofydoink 11:7029367a1840 64 //LLComms::~LLComms(void) { } // Destructor
dofydoink 11:7029367a1840 65
WD40andTape 10:1b6daba32452 66 void LLComms::SendReceiveData(int channel) {
WD40andTape 13:a373dfc57b89 67 //pinTesty = 1;
dofydoink 23:61526647cc8a 68 int intPosSPI_Rx[N_CHANNELS]; // 16bit position value received over SPI from the actuator
dofydoink 23:61526647cc8a 69 int intPresSPI_Rx[N_CHANNELS]; // 16bit pressure value received over SPI from the actuator
dofydoink 23:61526647cc8a 70 int intSumPosition=0;
dofydoink 23:61526647cc8a 71 int intSumVelocity=0;
dofydoink 23:61526647cc8a 72 int intPositionParity, intVelocityParity;
dofydoink 23:61526647cc8a 73 unsigned int intParityPosition, intParityVelocity, intPositionMsg, intVelocityMsg;
dofydoink 23:61526647cc8a 74
dofydoink 23:61526647cc8a 75 unsigned int count = 0, i, b = 1;
dofydoink 23:61526647cc8a 76
dofydoink 23:61526647cc8a 77 int intTempVar;
WD40andTape 10:1b6daba32452 78
WD40andTape 22:82871f00f89d 79 mutChannel[channel].lock(); // Lock mutex for specific Channel
dofydoink 23:61526647cc8a 80 //construct Position message
dofydoink 23:61526647cc8a 81 intPositionMsg = demandPosition[channel];
dofydoink 23:61526647cc8a 82
dofydoink 23:61526647cc8a 83 //calculate the sum of the position data
dofydoink 23:61526647cc8a 84
dofydoink 23:61526647cc8a 85 intTempVar = demandPosition[channel];
dofydoink 23:61526647cc8a 86
dofydoink 23:61526647cc8a 87 while (intTempVar >0)
dofydoink 23:61526647cc8a 88 {
dofydoink 23:61526647cc8a 89 intSumPosition += intTempVar%10;
dofydoink 23:61526647cc8a 90 intTempVar = int(intTempVar/10);
dofydoink 23:61526647cc8a 91 }
dofydoink 23:61526647cc8a 92
dofydoink 23:61526647cc8a 93 //add to message
dofydoink 23:61526647cc8a 94 intPositionMsg = intPositionMsg<<5;
dofydoink 23:61526647cc8a 95 intPositionMsg = intPositionMsg | intSumPosition;
dofydoink 23:61526647cc8a 96
dofydoink 23:61526647cc8a 97 //add type bit (0 == position, 1 == velocity)
dofydoink 23:61526647cc8a 98 intPositionMsg = intPositionMsg<<1;
dofydoink 23:61526647cc8a 99
dofydoink 23:61526647cc8a 100 //calculate decimal parity on whole message
dofydoink 23:61526647cc8a 101
dofydoink 23:61526647cc8a 102 for(i = 0; i < 32; i++){
dofydoink 23:61526647cc8a 103 if( intPositionMsg & (b << i) ){count++;}
dofydoink 23:61526647cc8a 104 }
dofydoink 23:61526647cc8a 105
dofydoink 23:61526647cc8a 106 if( (count % 2) ){ intPositionParity = 0;}
dofydoink 23:61526647cc8a 107 else {intPositionParity = 1;}
dofydoink 23:61526647cc8a 108
dofydoink 23:61526647cc8a 109 //add parity bit to message
dofydoink 23:61526647cc8a 110 intPositionMsg = intPositionMsg<<1;
dofydoink 23:61526647cc8a 111 intPositionMsg = intPositionMsg | intPositionParity;
dofydoink 23:61526647cc8a 112
dofydoink 23:61526647cc8a 113 //
dofydoink 23:61526647cc8a 114 //rinse and repeat for velocity message
dofydoink 23:61526647cc8a 115 intVelocityMsg = demandSpeed[channel];
dofydoink 23:61526647cc8a 116
dofydoink 23:61526647cc8a 117 //calculate the sum of the position data
dofydoink 23:61526647cc8a 118
dofydoink 23:61526647cc8a 119 intTempVar = demandSpeed[channel];
dofydoink 23:61526647cc8a 120
dofydoink 23:61526647cc8a 121 while (intTempVar >0)
dofydoink 23:61526647cc8a 122 {
dofydoink 23:61526647cc8a 123 intSumVelocity += intTempVar%10;
dofydoink 23:61526647cc8a 124 intTempVar = int(intTempVar/10);
dofydoink 23:61526647cc8a 125 }
dofydoink 23:61526647cc8a 126
dofydoink 23:61526647cc8a 127 //add to message
dofydoink 23:61526647cc8a 128 intVelocityMsg = intVelocityMsg<<5;
dofydoink 23:61526647cc8a 129 intVelocityMsg = intVelocityMsg | intSumVelocity;
dofydoink 23:61526647cc8a 130
dofydoink 23:61526647cc8a 131 //add type bit (0 == position, 1 == velocity)
dofydoink 23:61526647cc8a 132 intVelocityMsg = intVelocityMsg<<1;
dofydoink 23:61526647cc8a 133 intVelocityMsg = intVelocityMsg |1;
dofydoink 23:61526647cc8a 134
dofydoink 23:61526647cc8a 135 //calculate decimal parity on whole message
dofydoink 23:61526647cc8a 136
dofydoink 23:61526647cc8a 137 for(i = 0; i < 32; i++){
dofydoink 23:61526647cc8a 138 if( intVelocityMsg & (b << i) ){count++;}
dofydoink 23:61526647cc8a 139 }
dofydoink 23:61526647cc8a 140
dofydoink 23:61526647cc8a 141 if( (count % 2) ){ intVelocityParity = 0;}
dofydoink 23:61526647cc8a 142 else {intVelocityParity = 1;}
dofydoink 23:61526647cc8a 143
dofydoink 23:61526647cc8a 144 //add parity bit to message
dofydoink 23:61526647cc8a 145 intVelocityMsg = intVelocityMsg<<1;
dofydoink 23:61526647cc8a 146 intVelocityMsg = intVelocityMsg | intVelocityParity;
dofydoink 23:61526647cc8a 147
dofydoink 23:61526647cc8a 148 //Combine
dofydoink 23:61526647cc8a 149
dofydoink 23:61526647cc8a 150
WD40andTape 10:1b6daba32452 151 // Get data from controller
WD40andTape 14:54c3759e76ed 152 if( channel < 4 ) {
WD40andTape 14:54c3759e76ed 153 spi_0.format(16,2); // !! Can probably move to constructor
WD40andTape 14:54c3759e76ed 154 spi_0.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 14:54c3759e76ed 155 *cs_LL[channel] = 0; // Select relevant chip
dofydoink 23:61526647cc8a 156 intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
dofydoink 23:61526647cc8a 157
dofydoink 23:61526647cc8a 158 intPresSPI_Rx[channel] = intPosSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
WD40andTape 14:54c3759e76ed 159 *cs_LL[channel] = 1; // Deselect chip
WD40andTape 14:54c3759e76ed 160 } else {
WD40andTape 14:54c3759e76ed 161 spi_1.format(16,2); // !! Can probably move to constructor
WD40andTape 14:54c3759e76ed 162 spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 14:54c3759e76ed 163 *cs_LL[channel] = 0; // Select relevant chip
dofydoink 23:61526647cc8a 164 intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
dofydoink 23:61526647cc8a 165
dofydoink 23:61526647cc8a 166 intPresSPI_Rx[channel] = intPosSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
WD40andTape 14:54c3759e76ed 167 *cs_LL[channel] = 1; // Deselect chip
WD40andTape 14:54c3759e76ed 168 }
WD40andTape 10:1b6daba32452 169 isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
WD40andTape 10:1b6daba32452 170 /*if(channel == 0) {
WD40andTape 10:1b6daba32452 171 intGlobalTest = intPosSPI_Rx[channel];
WD40andTape 10:1b6daba32452 172 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
WD40andTape 10:1b6daba32452 173 }*/
WD40andTape 10:1b6daba32452 174
WD40andTape 10:1b6daba32452 175 // Sort out received data
dofydoink 23:61526647cc8a 176 //STILL TO DO!!!!!!
dofydoink 23:61526647cc8a 177
dofydoink 23:61526647cc8a 178 intPosSPI_Rx[channel] = intPosSPI_Rx[channel]>>7 & 0x1FF;
dofydoink 23:61526647cc8a 179 dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/511*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 10:1b6daba32452 180 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 10:1b6daba32452 181 }
WD40andTape 10:1b6daba32452 182
WD40andTape 10:1b6daba32452 183 // Common rise handler function
WD40andTape 10:1b6daba32452 184 void LLComms::common_rise_handler(int channel) {
WD40andTape 10:1b6daba32452 185 pinCheck = 1;
WD40andTape 10:1b6daba32452 186 if (isDataReady[channel]) { // Check if data is ready for tranmission
WD40andTape 10:1b6daba32452 187 ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
WD40andTape 10:1b6daba32452 188 }
WD40andTape 10:1b6daba32452 189 }
WD40andTape 10:1b6daba32452 190
WD40andTape 10:1b6daba32452 191 // Common fall handler functions
WD40andTape 10:1b6daba32452 192 void LLComms::common_fall_handler(int channel) {
WD40andTape 10:1b6daba32452 193 pinCheck = 0;
WD40andTape 10:1b6daba32452 194 queue.cancel(ThreadID[channel]); // Cancel relevant queued event
WD40andTape 10:1b6daba32452 195 }
WD40andTape 10:1b6daba32452 196
WD40andTape 10:1b6daba32452 197 // Stub rise functions
WD40andTape 10:1b6daba32452 198 void LLComms::rise0(void) { common_rise_handler(0); }
WD40andTape 10:1b6daba32452 199 void LLComms::rise1(void) { common_rise_handler(1); }
WD40andTape 10:1b6daba32452 200 void LLComms::rise2(void) { common_rise_handler(2); }
WD40andTape 10:1b6daba32452 201 void LLComms::rise3(void) { common_rise_handler(3); }
WD40andTape 10:1b6daba32452 202 void LLComms::rise4(void) { common_rise_handler(4); }
WD40andTape 10:1b6daba32452 203 void LLComms::rise5(void) { common_rise_handler(5); }
WD40andTape 10:1b6daba32452 204 void LLComms::rise6(void) { common_rise_handler(6); }
WD40andTape 10:1b6daba32452 205 void LLComms::rise7(void) { common_rise_handler(7); }
WD40andTape 10:1b6daba32452 206 // Stub fall functions
WD40andTape 10:1b6daba32452 207 void LLComms::fall0(void) { common_fall_handler(0); }
WD40andTape 10:1b6daba32452 208 void LLComms::fall1(void) { common_fall_handler(1); }
WD40andTape 10:1b6daba32452 209 void LLComms::fall2(void) { common_fall_handler(2); }
WD40andTape 10:1b6daba32452 210 void LLComms::fall3(void) { common_fall_handler(3); }
WD40andTape 10:1b6daba32452 211 void LLComms::fall4(void) { common_fall_handler(4); }
WD40andTape 10:1b6daba32452 212 void LLComms::fall5(void) { common_fall_handler(5); }
WD40andTape 10:1b6daba32452 213 void LLComms::fall6(void) { common_fall_handler(6); }
WD40andTape 10:1b6daba32452 214 void LLComms::fall7(void) { common_fall_handler(7); }
WD40andTape 10:1b6daba32452 215
dofydoink 12:595ed862e52f 216 // NEEDS CALIBRATING
WD40andTape 8:d6657767a182 217 double LLComms::ReadADCPosition_mtrs(int channel) {
WD40andTape 8:d6657767a182 218 unsigned int outputA;
WD40andTape 8:d6657767a182 219 unsigned int outputB;
WD40andTape 8:d6657767a182 220 int output;
WD40andTape 8:d6657767a182 221 double dblOutput;
WD40andTape 8:d6657767a182 222
WD40andTape 14:54c3759e76ed 223 if( channel < 4 ) {
WD40andTape 14:54c3759e76ed 224 spi_0.format(8,0);
WD40andTape 14:54c3759e76ed 225 spi_0.frequency(1000000);
WD40andTape 14:54c3759e76ed 226
WD40andTape 14:54c3759e76ed 227 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 228 spi_0.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 229 outputA = spi_0.write(CHAN_3);
WD40andTape 14:54c3759e76ed 230 outputB = spi_0.write(0xFF);
WD40andTape 14:54c3759e76ed 231 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 232 } else {
WD40andTape 14:54c3759e76ed 233 spi_1.format(8,0);
WD40andTape 14:54c3759e76ed 234 spi_1.frequency(1000000);
WD40andTape 14:54c3759e76ed 235
WD40andTape 14:54c3759e76ed 236 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 237 spi_1.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 238 outputA = spi_1.write(CHAN_3);
WD40andTape 14:54c3759e76ed 239 outputB = spi_1.write(0xFF);
WD40andTape 14:54c3759e76ed 240 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 241 }
WD40andTape 8:d6657767a182 242
WD40andTape 8:d6657767a182 243 outputA = outputA & DATA_MASK;
WD40andTape 8:d6657767a182 244 outputA = outputA<<8;
WD40andTape 8:d6657767a182 245 output = (outputA | outputB);
WD40andTape 8:d6657767a182 246 output = 4095- output;
WD40andTape 8:d6657767a182 247 dblOutput = (double) (output);
WD40andTape 8:d6657767a182 248 dblOutput = dblOutput*0.0229 - 21.582;
WD40andTape 8:d6657767a182 249 return dblOutput;
WD40andTape 8:d6657767a182 250 }
WD40andTape 8:d6657767a182 251
WD40andTape 8:d6657767a182 252 double LLComms::ReadADCPressure_bar(int channel) {
WD40andTape 8:d6657767a182 253 unsigned int outputA;
WD40andTape 8:d6657767a182 254 unsigned int outputB;
WD40andTape 8:d6657767a182 255 int output;
WD40andTape 8:d6657767a182 256 double dblOutput;
WD40andTape 8:d6657767a182 257
WD40andTape 14:54c3759e76ed 258 if( channel < 4 ) {
WD40andTape 14:54c3759e76ed 259 spi_0.format(8,0);
WD40andTape 14:54c3759e76ed 260 spi_0.frequency(1000000);
WD40andTape 14:54c3759e76ed 261
WD40andTape 14:54c3759e76ed 262 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 263 spi_0.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 264 outputA = spi_0.write(CHAN_1);
WD40andTape 14:54c3759e76ed 265 outputB = spi_0.write(0xFF);
WD40andTape 14:54c3759e76ed 266 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 267 } else {
WD40andTape 14:54c3759e76ed 268 spi_1.format(8,0);
WD40andTape 14:54c3759e76ed 269 spi_1.frequency(1000000);
WD40andTape 14:54c3759e76ed 270
WD40andTape 14:54c3759e76ed 271 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 272 spi_1.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 273 outputA = spi_1.write(CHAN_1);
WD40andTape 14:54c3759e76ed 274 outputB = spi_1.write(0xFF);
WD40andTape 14:54c3759e76ed 275 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 276 }
WD40andTape 8:d6657767a182 277
WD40andTape 8:d6657767a182 278 outputA = outputA & DATA_MASK;
WD40andTape 8:d6657767a182 279 outputA = outputA<<8;
WD40andTape 8:d6657767a182 280 output = (outputA | outputB);
WD40andTape 8:d6657767a182 281
WD40andTape 8:d6657767a182 282 dblOutput = (double)(output);
WD40andTape 8:d6657767a182 283 dblOutput = dblOutput-502.0;
WD40andTape 8:d6657767a182 284 dblOutput = dblOutput/4095.0*8.0;
WD40andTape 8:d6657767a182 285 return dblOutput;
WD40andTape 10:1b6daba32452 286 }