MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ros_lib_kinetic

LLComms.cpp@24:bc852aa89e7a, 2018-12-17 (annotated)

Committer:
WD40andTape
Date:
Mon Dec 17 14:34:37 2018 +0000
Revision:
24:bc852aa89e7a
Parent:
23:61526647cc8a
Child:
25:88e6cccde856
Updating ML/LL communications.

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
WD40andTape 24:bc852aa89e7a 23 spi_0.format(16,2);
WD40andTape 24:bc852aa89e7a 24 spi_0.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 24:bc852aa89e7a 25 spi_1.format(16,2);
WD40andTape 24:bc852aa89e7a 26 spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 24:bc852aa89e7a 27
dofydoink 12:595ed862e52f 28 PinName LLPins[8] = {PD_15, PE_10, PD_11, PD_14, PE_7, PD_12, PF_10, PD_13};
dofydoink 12:595ed862e52f 29 //PinName LLPins[8] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};
WD40andTape 8:d6657767a182 30 PinName ADCPins[8] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1};
WD40andTape 8:d6657767a182 31 for (short int i = 0; i < 8; i++) {
WD40andTape 10:1b6daba32452 32 isDataReady[i] = 0;
WD40andTape 8:d6657767a182 33 cs_LL[i] = new DigitalOut(LLPins[i]);
WD40andTape 8:d6657767a182 34 cs_ADC[i] = new DigitalOut(ADCPins[i]);
WD40andTape 8:d6657767a182 35 }
WD40andTape 10:1b6daba32452 36
WD40andTape 9:cd3607ba5643 37 // Initialise relevant variables
dofydoink 11:7029367a1840 38 for(short int i = 0; i<N_CHANNELS; i++) {
WD40andTape 9:cd3607ba5643 39 // All chip selects in off state
WD40andTape 9:cd3607ba5643 40 *cs_LL[i] = 1;
WD40andTape 9:cd3607ba5643 41 *cs_ADC[i] = 1;
WD40andTape 9:cd3607ba5643 42 }
WD40andTape 9:cd3607ba5643 43 pinReset = 1; // Initialise reset pin to not reset the controllers.
WD40andTape 9:cd3607ba5643 44 wait(0.25);
WD40andTape 9:cd3607ba5643 45 pinReset=0; // Reset controllers to be safe
WD40andTape 9:cd3607ba5643 46 wait(0.25);
WD40andTape 9:cd3607ba5643 47 pinReset = 1; // Ready to go
WD40andTape 10:1b6daba32452 48
WD40andTape 10:1b6daba32452 49 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 50 pinGate0.rise(callback(this,&LLComms::rise0));
dofydoink 12:595ed862e52f 51 pinGate1.rise(callback(this,&LLComms::rise1));
dofydoink 12:595ed862e52f 52 pinGate2.rise(callback(this,&LLComms::rise2));
dofydoink 12:595ed862e52f 53 pinGate3.rise(callback(this,&LLComms::rise3));
dofydoink 12:595ed862e52f 54 pinGate4.rise(callback(this,&LLComms::rise4));
dofydoink 12:595ed862e52f 55 pinGate5.rise(callback(this,&LLComms::rise5));
dofydoink 12:595ed862e52f 56 pinGate6.rise(callback(this,&LLComms::rise6));
dofydoink 12:595ed862e52f 57 pinGate7.rise(callback(this,&LLComms::rise7));
WD40andTape 10:1b6daba32452 58 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 59 pinGate0.fall(callback(this,&LLComms::fall0));
dofydoink 12:595ed862e52f 60 pinGate1.fall(callback(this,&LLComms::fall1));
dofydoink 12:595ed862e52f 61 pinGate2.fall(callback(this,&LLComms::fall2));
dofydoink 12:595ed862e52f 62 pinGate3.fall(callback(this,&LLComms::fall3));
dofydoink 12:595ed862e52f 63 pinGate4.fall(callback(this,&LLComms::fall4));
dofydoink 12:595ed862e52f 64 pinGate5.fall(callback(this,&LLComms::fall5));
dofydoink 12:595ed862e52f 65 pinGate6.fall(callback(this,&LLComms::fall6));
dofydoink 12:595ed862e52f 66 pinGate7.fall(callback(this,&LLComms::fall7));
WD40andTape 9:cd3607ba5643 67 }
WD40andTape 9:cd3607ba5643 68
dofydoink 11:7029367a1840 69 //LLComms::~LLComms(void) { } // Destructor
dofydoink 11:7029367a1840 70
WD40andTape 24:bc852aa89e7a 71 void LLComms::formatMessage(short int type, double dblValue, double dblMaxValue) {
WD40andTape 24:bc852aa89e7a 72 // Convert to a 9-bit number
WD40andTape 24:bc852aa89e7a 73 int intValue = (int) ((dblValue/dblMaxValue)*511);
WD40andTape 24:bc852aa89e7a 74 intValue = intValue & 0x1FF; // Ensure number is 9-bit
WD40andTape 24:bc852aa89e7a 75 // Initialize message with value
WD40andTape 24:bc852aa89e7a 76 unsigned int intMsg = intValue;
WD40andTape 24:bc852aa89e7a 77 // Calculate checksum (the decimal sum of the position data)
WD40andTape 24:bc852aa89e7a 78 int intCheckSum = 0, intTempVar = intValue;
WD40andTape 24:bc852aa89e7a 79 while( intTempVar>0 ) {
WD40andTape 24:bc852aa89e7a 80 intCheckSum += intTempVar%10;
WD40andTape 24:bc852aa89e7a 81 intTempVar = (int)(intTempVar/10);
WD40andTape 24:bc852aa89e7a 82 }
WD40andTape 24:bc852aa89e7a 83 // Add checksum to message
WD40andTape 24:bc852aa89e7a 84 intMsg = intMsg<<5;
WD40andTape 24:bc852aa89e7a 85 intMsg = intMsg | intCheckSum;
WD40andTape 24:bc852aa89e7a 86 // Add type bit (0 == position, 1 == velocity)
WD40andTape 24:bc852aa89e7a 87 intMsg = intMsg<<1;
WD40andTape 24:bc852aa89e7a 88 // Calculate decimal parity check for the whole message
WD40andTape 24:bc852aa89e7a 89 unsigned int count = 0, b = 1;
WD40andTape 24:bc852aa89e7a 90 for(short int i=0; i<32; i++) {
WD40andTape 24:bc852aa89e7a 91 if( intMsg & (b << i) ) count++;
WD40andTape 24:bc852aa89e7a 92 }
WD40andTape 24:bc852aa89e7a 93 // Add parity bit to message (0 == Odd, 1 == Even)
WD40andTape 24:bc852aa89e7a 94 bool boolParity = !(bool)(count%2);
WD40andTape 24:bc852aa89e7a 95 intMsg = intMsg<<1;
WD40andTape 24:bc852aa89e7a 96 intMsg = intMsg | (int)boolParity;
WD40andTape 24:bc852aa89e7a 97 }
WD40andTape 24:bc852aa89e7a 98
WD40andTape 10:1b6daba32452 99 void LLComms::SendReceiveData(int channel) {
WD40andTape 24:bc852aa89e7a 100 mutChannel[channel].lock(); // Lock mutex for specific Channel
WD40andTape 24:bc852aa89e7a 101
WD40andTape 24:bc852aa89e7a 102 // Construct messages
WD40andTape 24:bc852aa89e7a 103 unsigned int intPositionMsg = formatMessage(1,demandPosition[channel],MAX_ACTUATOR_LENGTH);
WD40andTape 24:bc852aa89e7a 104 unsigned int intVelocityMsg = formatMessage(0,demandSpeed[channel],MAX_SPEED_MMPS);
WD40andTape 24:bc852aa89e7a 105
WD40andTape 24:bc852aa89e7a 106 // Get data from controller
dofydoink 23:61526647cc8a 107 int intPosSPI_Rx[N_CHANNELS]; // 16bit position value received over SPI from the actuator
dofydoink 23:61526647cc8a 108 int intPresSPI_Rx[N_CHANNELS]; // 16bit pressure value received over SPI from the actuator
WD40andTape 14:54c3759e76ed 109 if( channel < 4 ) {
WD40andTape 14:54c3759e76ed 110 *cs_LL[channel] = 0; // Select relevant chip
dofydoink 23:61526647cc8a 111 intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
dofydoink 23:61526647cc8a 112 intPresSPI_Rx[channel] = intPosSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
WD40andTape 14:54c3759e76ed 113 *cs_LL[channel] = 1; // Deselect chip
WD40andTape 14:54c3759e76ed 114 } else {
WD40andTape 14:54c3759e76ed 115 *cs_LL[channel] = 0; // Select relevant chip
dofydoink 23:61526647cc8a 116 intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
dofydoink 23:61526647cc8a 117 intPresSPI_Rx[channel] = intPosSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
WD40andTape 14:54c3759e76ed 118 *cs_LL[channel] = 1; // Deselect chip
WD40andTape 14:54c3759e76ed 119 }
WD40andTape 10:1b6daba32452 120 isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
WD40andTape 24:bc852aa89e7a 121
WD40andTape 10:1b6daba32452 122 /*if(channel == 0) {
WD40andTape 10:1b6daba32452 123 intGlobalTest = intPosSPI_Rx[channel];
WD40andTape 10:1b6daba32452 124 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
WD40andTape 10:1b6daba32452 125 }*/
WD40andTape 10:1b6daba32452 126 // Sort out received data
dofydoink 23:61526647cc8a 127 //STILL TO DO!!!!!!
dofydoink 23:61526647cc8a 128 intPosSPI_Rx[channel] = intPosSPI_Rx[channel]>>7 & 0x1FF;
WD40andTape 24:bc852aa89e7a 129 positionSensor_m[channel] = (double)intPosSPI_Rx[channel]/511*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 24:bc852aa89e7a 130 //pressureSensor_bar[channel] = ...
WD40andTape 24:bc852aa89e7a 131
WD40andTape 10:1b6daba32452 132 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 10:1b6daba32452 133 }
WD40andTape 10:1b6daba32452 134
WD40andTape 10:1b6daba32452 135 // Common rise handler function
WD40andTape 10:1b6daba32452 136 void LLComms::common_rise_handler(int channel) {
WD40andTape 10:1b6daba32452 137 pinCheck = 1;
WD40andTape 10:1b6daba32452 138 if (isDataReady[channel]) { // Check if data is ready for tranmission
WD40andTape 10:1b6daba32452 139 ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
WD40andTape 10:1b6daba32452 140 }
WD40andTape 10:1b6daba32452 141 }
WD40andTape 10:1b6daba32452 142
WD40andTape 10:1b6daba32452 143 // Common fall handler functions
WD40andTape 10:1b6daba32452 144 void LLComms::common_fall_handler(int channel) {
WD40andTape 10:1b6daba32452 145 pinCheck = 0;
WD40andTape 10:1b6daba32452 146 queue.cancel(ThreadID[channel]); // Cancel relevant queued event
WD40andTape 10:1b6daba32452 147 }
WD40andTape 10:1b6daba32452 148
WD40andTape 10:1b6daba32452 149 // Stub rise functions
WD40andTape 10:1b6daba32452 150 void LLComms::rise0(void) { common_rise_handler(0); }
WD40andTape 10:1b6daba32452 151 void LLComms::rise1(void) { common_rise_handler(1); }
WD40andTape 10:1b6daba32452 152 void LLComms::rise2(void) { common_rise_handler(2); }
WD40andTape 10:1b6daba32452 153 void LLComms::rise3(void) { common_rise_handler(3); }
WD40andTape 10:1b6daba32452 154 void LLComms::rise4(void) { common_rise_handler(4); }
WD40andTape 10:1b6daba32452 155 void LLComms::rise5(void) { common_rise_handler(5); }
WD40andTape 10:1b6daba32452 156 void LLComms::rise6(void) { common_rise_handler(6); }
WD40andTape 10:1b6daba32452 157 void LLComms::rise7(void) { common_rise_handler(7); }
WD40andTape 10:1b6daba32452 158 // Stub fall functions
WD40andTape 10:1b6daba32452 159 void LLComms::fall0(void) { common_fall_handler(0); }
WD40andTape 10:1b6daba32452 160 void LLComms::fall1(void) { common_fall_handler(1); }
WD40andTape 10:1b6daba32452 161 void LLComms::fall2(void) { common_fall_handler(2); }
WD40andTape 10:1b6daba32452 162 void LLComms::fall3(void) { common_fall_handler(3); }
WD40andTape 10:1b6daba32452 163 void LLComms::fall4(void) { common_fall_handler(4); }
WD40andTape 10:1b6daba32452 164 void LLComms::fall5(void) { common_fall_handler(5); }
WD40andTape 10:1b6daba32452 165 void LLComms::fall6(void) { common_fall_handler(6); }
WD40andTape 10:1b6daba32452 166 void LLComms::fall7(void) { common_fall_handler(7); }
WD40andTape 10:1b6daba32452 167
dofydoink 12:595ed862e52f 168 // NEEDS CALIBRATING
WD40andTape 24:bc852aa89e7a 169 /*double LLComms::ReadADCPosition_mtrs(int channel) {
WD40andTape 8:d6657767a182 170 unsigned int outputA;
WD40andTape 8:d6657767a182 171 unsigned int outputB;
WD40andTape 8:d6657767a182 172 int output;
WD40andTape 8:d6657767a182 173 double dblOutput;
WD40andTape 8:d6657767a182 174
WD40andTape 14:54c3759e76ed 175 if( channel < 4 ) {
WD40andTape 14:54c3759e76ed 176 spi_0.format(8,0);
WD40andTape 14:54c3759e76ed 177 spi_0.frequency(1000000);
WD40andTape 14:54c3759e76ed 178
WD40andTape 14:54c3759e76ed 179 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 180 spi_0.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 181 outputA = spi_0.write(CHAN_3);
WD40andTape 14:54c3759e76ed 182 outputB = spi_0.write(0xFF);
WD40andTape 14:54c3759e76ed 183 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 184 } else {
WD40andTape 14:54c3759e76ed 185 spi_1.format(8,0);
WD40andTape 14:54c3759e76ed 186 spi_1.frequency(1000000);
WD40andTape 14:54c3759e76ed 187
WD40andTape 14:54c3759e76ed 188 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 189 spi_1.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 190 outputA = spi_1.write(CHAN_3);
WD40andTape 14:54c3759e76ed 191 outputB = spi_1.write(0xFF);
WD40andTape 14:54c3759e76ed 192 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 193 }
WD40andTape 8:d6657767a182 194
WD40andTape 8:d6657767a182 195 outputA = outputA & DATA_MASK;
WD40andTape 8:d6657767a182 196 outputA = outputA<<8;
WD40andTape 8:d6657767a182 197 output = (outputA | outputB);
WD40andTape 8:d6657767a182 198 output = 4095- output;
WD40andTape 8:d6657767a182 199 dblOutput = (double) (output);
WD40andTape 8:d6657767a182 200 dblOutput = dblOutput*0.0229 - 21.582;
WD40andTape 8:d6657767a182 201 return dblOutput;
WD40andTape 8:d6657767a182 202 }
WD40andTape 8:d6657767a182 203
WD40andTape 8:d6657767a182 204 double LLComms::ReadADCPressure_bar(int channel) {
WD40andTape 8:d6657767a182 205 unsigned int outputA;
WD40andTape 8:d6657767a182 206 unsigned int outputB;
WD40andTape 8:d6657767a182 207 int output;
WD40andTape 8:d6657767a182 208 double dblOutput;
WD40andTape 8:d6657767a182 209
WD40andTape 14:54c3759e76ed 210 if( channel < 4 ) {
WD40andTape 14:54c3759e76ed 211 spi_0.format(8,0);
WD40andTape 14:54c3759e76ed 212 spi_0.frequency(1000000);
WD40andTape 14:54c3759e76ed 213
WD40andTape 14:54c3759e76ed 214 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 215 spi_0.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 216 outputA = spi_0.write(CHAN_1);
WD40andTape 14:54c3759e76ed 217 outputB = spi_0.write(0xFF);
WD40andTape 14:54c3759e76ed 218 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 219 } else {
WD40andTape 14:54c3759e76ed 220 spi_1.format(8,0);
WD40andTape 14:54c3759e76ed 221 spi_1.frequency(1000000);
WD40andTape 14:54c3759e76ed 222
WD40andTape 14:54c3759e76ed 223 *cs_ADC[channel] = 0;
WD40andTape 14:54c3759e76ed 224 spi_1.write(PREAMBLE);
WD40andTape 14:54c3759e76ed 225 outputA = spi_1.write(CHAN_1);
WD40andTape 14:54c3759e76ed 226 outputB = spi_1.write(0xFF);
WD40andTape 14:54c3759e76ed 227 *cs_ADC[channel] = 1;
WD40andTape 14:54c3759e76ed 228 }
WD40andTape 8:d6657767a182 229
WD40andTape 8:d6657767a182 230 outputA = outputA & DATA_MASK;
WD40andTape 8:d6657767a182 231 outputA = outputA<<8;
WD40andTape 8:d6657767a182 232 output = (outputA | outputB);
WD40andTape 8:d6657767a182 233
WD40andTape 8:d6657767a182 234 dblOutput = (double)(output);
WD40andTape 8:d6657767a182 235 dblOutput = dblOutput-502.0;
WD40andTape 8:d6657767a182 236 dblOutput = dblOutput/4095.0*8.0;
WD40andTape 8:d6657767a182 237 return dblOutput;
WD40andTape 24:bc852aa89e7a 238 }*/