MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

LLComms.cpp@25:88e6cccde856, 2018-12-17 (annotated)

Committer:
WD40andTape
Date:
Mon Dec 17 15:09:44 2018 +0000
Revision:
25:88e6cccde856
Parent:
24:bc852aa89e7a
Child:
26:7c59002c9cd7
Fixed splitting across 2 SPI buses in SendReceiveData. Still need to fix SPI writing and processing received data.

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 25:88e6cccde856 79 while( intTempVar>0 ) { //591
WD40andTape 24:bc852aa89e7a 80 intCheckSum += intTempVar%10;
WD40andTape 25:88e6cccde856 81 intTempVar = floor(intTempVar/10.0);
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 25:88e6cccde856 109 *cs_LL[channel] = 0; // Select relevant chip
WD40andTape 14:54c3759e76ed 110 if( channel < 4 ) {
dofydoink 23:61526647cc8a 111 intPosSPI_Rx[channel] = spi_0.write(intPositionMsg); // Transmit & receive
WD40andTape 25:88e6cccde856 112 intPresSPI_Rx[channel] = intPresSPI_Rx[channel] | spi_0.write(intVelocityMsg); // Transmit & receive
WD40andTape 14:54c3759e76ed 113 } else {
WD40andTape 25:88e6cccde856 114 intPosSPI_Rx[channel] = spi_1.write(intPositionMsg); // Transmit & receive
WD40andTape 25:88e6cccde856 115 intPresSPI_Rx[channel] = intPresSPI_Rx[channel] | spi_1.write(intVelocityMsg); // Transmit & receive
WD40andTape 14:54c3759e76ed 116 }
WD40andTape 25:88e6cccde856 117 *cs_LL[channel] = 1; // Deselect chip
WD40andTape 10:1b6daba32452 118 isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
WD40andTape 24:bc852aa89e7a 119
WD40andTape 10:1b6daba32452 120 /*if(channel == 0) {
WD40andTape 10:1b6daba32452 121 intGlobalTest = intPosSPI_Rx[channel];
WD40andTape 10:1b6daba32452 122 dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
WD40andTape 10:1b6daba32452 123 }*/
WD40andTape 10:1b6daba32452 124 // Sort out received data
dofydoink 23:61526647cc8a 125 //STILL TO DO!!!!!!
dofydoink 23:61526647cc8a 126 intPosSPI_Rx[channel] = intPosSPI_Rx[channel]>>7 & 0x1FF;
WD40andTape 24:bc852aa89e7a 127 positionSensor_m[channel] = (double)intPosSPI_Rx[channel]/511*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
WD40andTape 24:bc852aa89e7a 128 //pressureSensor_bar[channel] = ...
WD40andTape 24:bc852aa89e7a 129
WD40andTape 10:1b6daba32452 130 mutChannel[channel].unlock();//unlock mutex for specific channel
WD40andTape 10:1b6daba32452 131 }
WD40andTape 10:1b6daba32452 132
WD40andTape 10:1b6daba32452 133 // Common rise handler function
WD40andTape 10:1b6daba32452 134 void LLComms::common_rise_handler(int channel) {
WD40andTape 10:1b6daba32452 135 pinCheck = 1;
WD40andTape 10:1b6daba32452 136 if (isDataReady[channel]) { // Check if data is ready for tranmission
WD40andTape 10:1b6daba32452 137 ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
WD40andTape 10:1b6daba32452 138 }
WD40andTape 10:1b6daba32452 139 }
WD40andTape 10:1b6daba32452 140
WD40andTape 10:1b6daba32452 141 // Common fall handler functions
WD40andTape 10:1b6daba32452 142 void LLComms::common_fall_handler(int channel) {
WD40andTape 10:1b6daba32452 143 pinCheck = 0;
WD40andTape 10:1b6daba32452 144 queue.cancel(ThreadID[channel]); // Cancel relevant queued event
WD40andTape 10:1b6daba32452 145 }
WD40andTape 10:1b6daba32452 146
WD40andTape 10:1b6daba32452 147 // Stub rise functions
WD40andTape 10:1b6daba32452 148 void LLComms::rise0(void) { common_rise_handler(0); }
WD40andTape 10:1b6daba32452 149 void LLComms::rise1(void) { common_rise_handler(1); }
WD40andTape 10:1b6daba32452 150 void LLComms::rise2(void) { common_rise_handler(2); }
WD40andTape 10:1b6daba32452 151 void LLComms::rise3(void) { common_rise_handler(3); }
WD40andTape 10:1b6daba32452 152 void LLComms::rise4(void) { common_rise_handler(4); }
WD40andTape 10:1b6daba32452 153 void LLComms::rise5(void) { common_rise_handler(5); }
WD40andTape 10:1b6daba32452 154 void LLComms::rise6(void) { common_rise_handler(6); }
WD40andTape 10:1b6daba32452 155 void LLComms::rise7(void) { common_rise_handler(7); }
WD40andTape 10:1b6daba32452 156 // Stub fall functions
WD40andTape 10:1b6daba32452 157 void LLComms::fall0(void) { common_fall_handler(0); }
WD40andTape 10:1b6daba32452 158 void LLComms::fall1(void) { common_fall_handler(1); }
WD40andTape 10:1b6daba32452 159 void LLComms::fall2(void) { common_fall_handler(2); }
WD40andTape 10:1b6daba32452 160 void LLComms::fall3(void) { common_fall_handler(3); }
WD40andTape 10:1b6daba32452 161 void LLComms::fall4(void) { common_fall_handler(4); }
WD40andTape 10:1b6daba32452 162 void LLComms::fall5(void) { common_fall_handler(5); }
WD40andTape 10:1b6daba32452 163 void LLComms::fall6(void) { common_fall_handler(6); }
WD40andTape 25:88e6cccde856 164 void LLComms::fall7(void) { common_fall_handler(7); }