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@36:4459be8296e9, 2019-07-09 (annotated)

Committer:
WD40andTape
Date:
Tue Jul 09 18:46:44 2019 +0000
Revision:
36:4459be8296e9
Parent:
34:54e9ebe9e87f
Child:
37:37da606f4466
tested and working with 9 stages

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 36:4459be8296e9 7 pinGate6(PA_5),
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 36:4459be8296e9 10 spi_2(PB_15, PC_2, PB_13),
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 34:54e9ebe9e87f 17 pinGate5(PC_7),
WD40andTape 8:d6657767a182 18 pinGate7(PE_13),
WD40andTape 36:4459be8296e9 19 pinGate8(PF_4),//PB_3),
WD40andTape 36:4459be8296e9 20 pinReset(PD_2),
WD40andTape 36:4459be8296e9 21 chrp(PB_8),
WD40andTape 36:4459be8296e9 22 chrp1(PC_6)
WD40andTape 8:d6657767a182 23 { // Constructor
WD40andTape 8:d6657767a182 24
WD40andTape 24:bc852aa89e7a 25 spi_0.format(16,2);
WD40andTape 24:bc852aa89e7a 26 spi_0.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 24:bc852aa89e7a 27 spi_1.format(16,2);
WD40andTape 24:bc852aa89e7a 28 spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 36:4459be8296e9 29 spi_2.format(16,2);
WD40andTape 36:4459be8296e9 30 spi_2.frequency(LOW_LEVEL_SPI_FREQUENCY);
WD40andTape 24:bc852aa89e7a 31
WD40andTape 36:4459be8296e9 32 //PinName LLPins[N_CHANNELS] = {PD_15, PE_10, PD_11, PD_14, PE_7, PB_1, PF_10, PD_13};
WD40andTape 36:4459be8296e9 33 PinName LLPins[N_CHANNELS] = {PD_15, PE_10, PD_11, PD_14, PE_7, PB_1, PB_12, PD_13, PB_5};
WD40andTape 36:4459be8296e9 34 for (short int i=0; i<N_CHANNELS; i++) {
WD40andTape 10:1b6daba32452 35 isDataReady[i] = 0;
WD40andTape 8:d6657767a182 36 cs_LL[i] = new DigitalOut(LLPins[i]);
WD40andTape 34:54e9ebe9e87f 37 //cs_ADC[i] = new DigitalOut(ADCPins[i]);
WD40andTape 8:d6657767a182 38 }
WD40andTape 10:1b6daba32452 39
WD40andTape 9:cd3607ba5643 40 // Initialise relevant variables
WD40andTape 36:4459be8296e9 41 for(short int i=0; i<N_CHANNELS; i++) {
WD40andTape 9:cd3607ba5643 42 // All chip selects in off state
WD40andTape 9:cd3607ba5643 43 *cs_LL[i] = 1;
WD40andTape 34:54e9ebe9e87f 44 //*cs_ADC[i] = 1;
WD40andTape 26:7c59002c9cd7 45 // Initialise pressures/positions
WD40andTape 29:10a5cf37a875 46 pressureSensor_uint[i] = 0.0;
WD40andTape 26:7c59002c9cd7 47 pressureSensor_bar[i] = -1.0;
WD40andTape 29:10a5cf37a875 48 positionSensor_uint[i] = 0.0;
WD40andTape 26:7c59002c9cd7 49 positionSensor_mm[i] = -1.0;
WD40andTape 9:cd3607ba5643 50 }
WD40andTape 9:cd3607ba5643 51 pinReset = 1; // Initialise reset pin to not reset the controllers.
WD40andTape 9:cd3607ba5643 52 wait(0.25);
WD40andTape 9:cd3607ba5643 53 pinReset=0; // Reset controllers to be safe
WD40andTape 9:cd3607ba5643 54 wait(0.25);
WD40andTape 9:cd3607ba5643 55 pinReset = 1; // Ready to go
WD40andTape 10:1b6daba32452 56
WD40andTape 10:1b6daba32452 57 // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 58 pinGate0.rise(callback(this,&LLComms::rise0));
dofydoink 12:595ed862e52f 59 pinGate1.rise(callback(this,&LLComms::rise1));
dofydoink 12:595ed862e52f 60 pinGate2.rise(callback(this,&LLComms::rise2));
dofydoink 12:595ed862e52f 61 pinGate3.rise(callback(this,&LLComms::rise3));
dofydoink 12:595ed862e52f 62 pinGate4.rise(callback(this,&LLComms::rise4));
dofydoink 12:595ed862e52f 63 pinGate5.rise(callback(this,&LLComms::rise5));
dofydoink 12:595ed862e52f 64 pinGate6.rise(callback(this,&LLComms::rise6));
dofydoink 12:595ed862e52f 65 pinGate7.rise(callback(this,&LLComms::rise7));
WD40andTape 36:4459be8296e9 66 pinGate8.rise(callback(this,&LLComms::rise8));
WD40andTape 10:1b6daba32452 67 // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
dofydoink 12:595ed862e52f 68 pinGate0.fall(callback(this,&LLComms::fall0));
dofydoink 12:595ed862e52f 69 pinGate1.fall(callback(this,&LLComms::fall1));
dofydoink 12:595ed862e52f 70 pinGate2.fall(callback(this,&LLComms::fall2));
dofydoink 12:595ed862e52f 71 pinGate3.fall(callback(this,&LLComms::fall3));
dofydoink 12:595ed862e52f 72 pinGate4.fall(callback(this,&LLComms::fall4));
dofydoink 12:595ed862e52f 73 pinGate5.fall(callback(this,&LLComms::fall5));
dofydoink 12:595ed862e52f 74 pinGate6.fall(callback(this,&LLComms::fall6));
dofydoink 12:595ed862e52f 75 pinGate7.fall(callback(this,&LLComms::fall7));
WD40andTape 36:4459be8296e9 76 pinGate8.fall(callback(this,&LLComms::fall8));
WD40andTape 9:cd3607ba5643 77 }
WD40andTape 9:cd3607ba5643 78
dofydoink 11:7029367a1840 79 //LLComms::~LLComms(void) { } // Destructor
WD40andTape 26:7c59002c9cd7 80 unsigned int LLComms::formatMessage(short int type, double dblValue, double dblMaxValue) {
WD40andTape 24:bc852aa89e7a 81 // Convert to a 9-bit number
WD40andTape 24:bc852aa89e7a 82 int intValue = (int) ((dblValue/dblMaxValue)*511);
WD40andTape 24:bc852aa89e7a 83 intValue = intValue & 0x1FF; // Ensure number is 9-bit
WD40andTape 24:bc852aa89e7a 84 // Initialize message with value
WD40andTape 24:bc852aa89e7a 85 unsigned int intMsg = intValue;
WD40andTape 24:bc852aa89e7a 86 // Calculate checksum (the decimal sum of the position data)
WD40andTape 24:bc852aa89e7a 87 int intCheckSum = 0, intTempVar = intValue;
WD40andTape 26:7c59002c9cd7 88 while( intTempVar>0 ) {
WD40andTape 24:bc852aa89e7a 89 intCheckSum += intTempVar%10;
WD40andTape 25:88e6cccde856 90 intTempVar = floor(intTempVar/10.0);
WD40andTape 24:bc852aa89e7a 91 }
WD40andTape 24:bc852aa89e7a 92 // Add checksum to message
WD40andTape 24:bc852aa89e7a 93 intMsg = intMsg<<5;
WD40andTape 24:bc852aa89e7a 94 intMsg = intMsg | intCheckSum;
WD40andTape 24:bc852aa89e7a 95 // Add type bit (0 == position, 1 == velocity)
WD40andTape 24:bc852aa89e7a 96 intMsg = intMsg<<1;
WD40andTape 26:7c59002c9cd7 97 intMsg = intMsg | (int)type; // CAST AS BOOL
WD40andTape 24:bc852aa89e7a 98 // Calculate decimal parity check for the whole message
WD40andTape 24:bc852aa89e7a 99 unsigned int count = 0, b = 1;
WD40andTape 24:bc852aa89e7a 100 for(short int i=0; i<32; i++) {
WD40andTape 24:bc852aa89e7a 101 if( intMsg & (b << i) ) count++;
WD40andTape 24:bc852aa89e7a 102 }
WD40andTape 24:bc852aa89e7a 103 // Add parity bit to message (0 == Odd, 1 == Even)
WD40andTape 27:6853ee8ffefd 104 // Parity selected in this way to prevent 0x0000 from passing checks
WD40andTape 24:bc852aa89e7a 105 bool boolParity = !(bool)(count%2);
WD40andTape 24:bc852aa89e7a 106 intMsg = intMsg<<1;
WD40andTape 24:bc852aa89e7a 107 intMsg = intMsg | (int)boolParity;
WD40andTape 26:7c59002c9cd7 108 return intMsg;
WD40andTape 26:7c59002c9cd7 109 }
WD40andTape 26:7c59002c9cd7 110
WD40andTape 26:7c59002c9cd7 111 bool LLComms::CheckMessage(int msg) {
WD40andTape 26:7c59002c9cd7 112 // Find message parity
WD40andTape 26:7c59002c9cd7 113 short int count = 0;
WD40andTape 26:7c59002c9cd7 114 for(short int i=0; i<32; i++) {
WD40andTape 26:7c59002c9cd7 115 if( msg>>1 & (1<<i) ) count++;
WD40andTape 26:7c59002c9cd7 116 }
WD40andTape 26:7c59002c9cd7 117 int intParity = !(count%2);
WD40andTape 26:7c59002c9cd7 118 // Find message CheckSum
WD40andTape 26:7c59002c9cd7 119 int intChkSum = 0;
WD40andTape 26:7c59002c9cd7 120 int intTempVar = msg>>7;
WD40andTape 26:7c59002c9cd7 121 while(intTempVar > 0) {
WD40andTape 26:7c59002c9cd7 122 intChkSum += intTempVar%10;
WD40andTape 26:7c59002c9cd7 123 intTempVar = int(intTempVar/10);
WD40andTape 26:7c59002c9cd7 124 }
WD40andTape 26:7c59002c9cd7 125 // Check if parity and CheckSum match
WD40andTape 26:7c59002c9cd7 126 bool isParityCorrect = (intParity == (msg&0x1));
WD40andTape 26:7c59002c9cd7 127 bool isChkSumCorrect = (intChkSum == ((msg>>2)&0x1F));
WD40andTape 26:7c59002c9cd7 128 bool isCheckPassed = (isParityCorrect && isChkSumCorrect);
WD40andTape 26:7c59002c9cd7 129 return isCheckPassed;
WD40andTape 26:7c59002c9cd7 130 }
WD40andTape 26:7c59002c9cd7 131
WD40andTape 26:7c59002c9cd7 132 bool LLComms::PerformMasterSPI(SPI *spi, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]) {
WD40andTape 26:7c59002c9cd7 133 unsigned int dummyMsg = 0x5555;
WD40andTape 26:7c59002c9cd7 134 bool isSuccess = true;
WD40andTape 26:7c59002c9cd7 135 unsigned int inboundMsg, typeBit;
WD40andTape 27:6853ee8ffefd 136 AnalogIn ain(PA_4); // Random analogue pin
WD40andTape 26:7c59002c9cd7 137 for(short int i=0; i<3; i++) { // Loop 3 times for 3 SPI messages
WD40andTape 27:6853ee8ffefd 138 ain.read_u16(); // Read analogue pin to cause a delay
WD40andTape 27:6853ee8ffefd 139 ain.read_u16();
WD40andTape 26:7c59002c9cd7 140 if(i==0) {
WD40andTape 26:7c59002c9cd7 141 inboundMsg = spi->write(outboundMsgs[0]);
WD40andTape 26:7c59002c9cd7 142 } else if(i==1) {
WD40andTape 26:7c59002c9cd7 143 inboundMsg = spi->write(outboundMsgs[1]);
WD40andTape 26:7c59002c9cd7 144 } else {
WD40andTape 26:7c59002c9cd7 145 inboundMsg = spi->write(dummyMsg);
WD40andTape 26:7c59002c9cd7 146 }
WD40andTape 26:7c59002c9cd7 147 if((unsigned int)inboundMsg != dummyMsg) { // Message is not dummy which is only used for reply
WD40andTape 26:7c59002c9cd7 148 typeBit = inboundMsg>>1 & 0x1;
WD40andTape 26:7c59002c9cd7 149 inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;
WD40andTape 26:7c59002c9cd7 150 if( !CheckMessage(inboundMsg) ) {
WD40andTape 26:7c59002c9cd7 151 isSuccess = false;
WD40andTape 26:7c59002c9cd7 152 }
WD40andTape 26:7c59002c9cd7 153 }
WD40andTape 26:7c59002c9cd7 154 }
WD40andTape 26:7c59002c9cd7 155 return isSuccess;
WD40andTape 24:bc852aa89e7a 156 }
WD40andTape 24:bc852aa89e7a 157
WD40andTape 10:1b6daba32452 158 void LLComms::SendReceiveData(int channel) {
WD40andTape 24:bc852aa89e7a 159 mutChannel[channel].lock(); // Lock mutex for specific Channel
WD40andTape 24:bc852aa89e7a 160
WD40andTape 24:bc852aa89e7a 161 // Construct messages
WD40andTape 26:7c59002c9cd7 162 unsigned int intPositionMsg = formatMessage(0,demandPosition_mm[channel],MAX_ACTUATOR_LIMIT_MM);
WD40andTape 26:7c59002c9cd7 163 unsigned int intVelocityMsg = formatMessage(1,demandSpeed_mmps[channel],MAX_SPEED_MMPS);
WD40andTape 26:7c59002c9cd7 164
WD40andTape 26:7c59002c9cd7 165 *cs_LL[channel] = 0; // Select relevant chip
WD40andTape 26:7c59002c9cd7 166 unsigned int outboundMsgs[2] = { intPositionMsg , intVelocityMsg };
WD40andTape 27:6853ee8ffefd 167 unsigned int inboundMsgsData[2] = { 0 };
WD40andTape 26:7c59002c9cd7 168 bool isSPIsuccess = false;
WD40andTape 26:7c59002c9cd7 169 if( channel < 4 ) {
WD40andTape 26:7c59002c9cd7 170 isSPIsuccess = PerformMasterSPI(&spi_0,outboundMsgs,inboundMsgsData);
WD40andTape 36:4459be8296e9 171 } else if( channel < 7 ) {//was 7, but should have been 8?
WD40andTape 36:4459be8296e9 172 isSPIsuccess = PerformMasterSPI(&spi_1,outboundMsgs,inboundMsgsData);
WD40andTape 26:7c59002c9cd7 173 } else {
WD40andTape 36:4459be8296e9 174 isSPIsuccess = PerformMasterSPI(&spi_2,outboundMsgs,inboundMsgsData);
WD40andTape 26:7c59002c9cd7 175 }
WD40andTape 26:7c59002c9cd7 176 *cs_LL[channel] = 1; // Deselect chip
WD40andTape 26:7c59002c9cd7 177 if( isSPIsuccess ) {
WD40andTape 26:7c59002c9cd7 178 isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
WD40andTape 29:10a5cf37a875 179 positionSensor_uint[channel] = inboundMsgsData[0];
WD40andTape 27:6853ee8ffefd 180 positionSensor_mm[channel] = ((double)inboundMsgsData[0]/511) * (double)MAX_ACTUATOR_LENGTH_MM;
WD40andTape 27:6853ee8ffefd 181 positionSensor_mm[channel] = min( max(positionSensor_mm[channel],0.0) , (double)MAX_ACTUATOR_LENGTH_MM );
WD40andTape 29:10a5cf37a875 182 pressureSensor_uint[channel] = inboundMsgsData[1];
WD40andTape 27:6853ee8ffefd 183 pressureSensor_bar[channel] = ((double)inboundMsgsData[1]/511) * (double)MAX_PRESSURE_LIMIT;
WD40andTape 27:6853ee8ffefd 184 pressureSensor_bar[channel] = min( max(pressureSensor_bar[channel],0.0) , (double)MAX_PRESSURE_LIMIT );
WD40andTape 26:7c59002c9cd7 185 } else { // Data is STILL ready and will be resent at the next pin interrupt
WD40andTape 26:7c59002c9cd7 186 //printf("SPI failed: %d%d. Resending.\n\r",isPositionValid,isPressureValid);
WD40andTape 26:7c59002c9cd7 187 }
WD40andTape 26:7c59002c9cd7 188
WD40andTape 26:7c59002c9cd7 189 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 26:7c59002c9cd7 190 }
WD40andTape 26:7c59002c9cd7 191
WD40andTape 10:1b6daba32452 192 // Common rise handler function
WD40andTape 10:1b6daba32452 193 void LLComms::common_rise_handler(int channel) {
WD40andTape 26:7c59002c9cd7 194 //printf("%d %d common_rise_handler\n\r",channel,isDataReady[channel]);
WD40andTape 36:4459be8296e9 195 if(channel==6) chrp = 1;
WD40andTape 36:4459be8296e9 196 if(channel==0) chrp1 = 1;
WD40andTape 26:7c59002c9cd7 197 if (isDataReady[channel]) { // Check if data is ready for transmission
WD40andTape 10:1b6daba32452 198 ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
WD40andTape 10:1b6daba32452 199 }
WD40andTape 10:1b6daba32452 200 }
WD40andTape 10:1b6daba32452 201
WD40andTape 10:1b6daba32452 202 // Common fall handler functions
WD40andTape 10:1b6daba32452 203 void LLComms::common_fall_handler(int channel) {
WD40andTape 36:4459be8296e9 204 if(channel==6) chrp = 0;
WD40andTape 36:4459be8296e9 205 if(channel==0) chrp1 = 0;
WD40andTape 10:1b6daba32452 206 queue.cancel(ThreadID[channel]); // Cancel relevant queued event
WD40andTape 10:1b6daba32452 207 }
WD40andTape 10:1b6daba32452 208
WD40andTape 10:1b6daba32452 209 // Stub rise functions
WD40andTape 10:1b6daba32452 210 void LLComms::rise0(void) { common_rise_handler(0); }
WD40andTape 10:1b6daba32452 211 void LLComms::rise1(void) { common_rise_handler(1); }
WD40andTape 10:1b6daba32452 212 void LLComms::rise2(void) { common_rise_handler(2); }
WD40andTape 10:1b6daba32452 213 void LLComms::rise3(void) { common_rise_handler(3); }
WD40andTape 10:1b6daba32452 214 void LLComms::rise4(void) { common_rise_handler(4); }
WD40andTape 10:1b6daba32452 215 void LLComms::rise5(void) { common_rise_handler(5); }
WD40andTape 10:1b6daba32452 216 void LLComms::rise6(void) { common_rise_handler(6); }
WD40andTape 10:1b6daba32452 217 void LLComms::rise7(void) { common_rise_handler(7); }
WD40andTape 36:4459be8296e9 218 void LLComms::rise8(void) { common_rise_handler(8); }
WD40andTape 10:1b6daba32452 219 // Stub fall functions
WD40andTape 10:1b6daba32452 220 void LLComms::fall0(void) { common_fall_handler(0); }
WD40andTape 10:1b6daba32452 221 void LLComms::fall1(void) { common_fall_handler(1); }
WD40andTape 10:1b6daba32452 222 void LLComms::fall2(void) { common_fall_handler(2); }
WD40andTape 10:1b6daba32452 223 void LLComms::fall3(void) { common_fall_handler(3); }
WD40andTape 10:1b6daba32452 224 void LLComms::fall4(void) { common_fall_handler(4); }
WD40andTape 10:1b6daba32452 225 void LLComms::fall5(void) { common_fall_handler(5); }
WD40andTape 10:1b6daba32452 226 void LLComms::fall6(void) { common_fall_handler(6); }
WD40andTape 36:4459be8296e9 227 void LLComms::fall7(void) { common_fall_handler(7); }
WD40andTape 36:4459be8296e9 228 void LLComms::fall8(void) { common_fall_handler(8); }