Diff: main.cpp

Revision:

8:d6657767a182

Parent:

7:5b6a2cefbf3b

Child:

9:cd3607ba5643

--- a/main.cpp	Fri Aug 03 14:58:36 2018 +0000
+++ b/main.cpp	Tue Aug 07 09:18:15 2018 +0000
@@ -5,14 +5,7 @@
 #include "mbed_events.h"
 // CUSTOM IMPORTS
 #include "HLComms.h"
-
-// ADC SPI DEFINES
-#define PREAMBLE 0x06
-#define CHAN_1 0x30
-#define CHAN_2 0x70
-#define CHAN_3 0xB0
-#define CHAN_4 0xF0
-#define DATA_MASK 0x0F
+#include "LLComms.h"
 
 // SIMPLE CHANNEL SELECTION
 #define ADC_PRESSURE 1
@@ -47,153 +40,25 @@
 double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
 
 int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
-char chrErrorFlag[N_CHANNELS];// 3 error bits from LL
 
-// PIN DECLARATIONS
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
-InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.
-SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
-DigitalOut cs_LL[N_CHANNELS] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};//chip select for low level controller
-DigitalOut cs_ADC[N_CHANNELS] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1}; //chip select for ADC
-DigitalOut pinCheck(PE_5);
-// These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
-InterruptIn pinGate0(PF_11);
-InterruptIn pinGate1(PG_14);
-InterruptIn pinGate2(PF_15);
-InterruptIn pinGate3(PF_12);
-InterruptIn pinGate4(PF_3);
-InterruptIn pinGate5(PF_13);
-//InterruptIn pinGate6(PE_11); // See above nonsense
-InterruptIn pinGate7(PE_13);
-DigitalOut pinReset(PD_2); // Reset pin for all controllers.
+LLComms llcomms(LOW_LEVEL_SPI_FREQUENCY);//(N_CHANNELS);
 
 EventQueue queue(32 * EVENTS_EVENT_SIZE);
 Thread t(osPriorityRealtime);
 Thread threadReceiveAndReplan(osPriorityBelowNormal);
 Thread threadSmoothPathPlan(osPriorityNormal);
-Thread threadSimulateDemand(osPriorityHigh);
 
-Mutex mutChannel[N_CHANNELS];
 Mutex mutPathIn;
 Semaphore semPathPlan(1);
 
 Timer timer;
 Ticker PathCalculationTicker;
 
-int ThreadID[N_CHANNELS];
-
 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
 
-double dblGlobalTest;
-int intGlobalTest;
-
-double ReadADCPosition_mtrs(int channel) {
-    unsigned int outputA;
-    unsigned int outputB;
-    int output;
-    double dblOutput;
-
-    spi.format(8,0);
-    spi.frequency(1000000);
-    
-    cs_ADC[channel] = 0;
-    spi.write(PREAMBLE);
-    outputA = spi.write(CHAN_3);
-    outputB = spi.write(0xFF);
-    cs_ADC[channel]= 1;
-    
-    outputA = outputA & DATA_MASK;
-    outputA = outputA<<8;
-    output = (outputA | outputB);
-    output = 4095- output;
-    dblOutput = (double) (output);
-    dblOutput = dblOutput*0.0229 - 21.582;
-    return dblOutput;
-}
-    
-double ReadADCPressure_bar(int channel) {
-    unsigned int outputA;
-    unsigned int outputB;
-    int output;
-    double dblOutput;
-
-    spi.format(8,0);
-    spi.frequency(1000000);
-    
-    cs_ADC[channel] = 0;
-    spi.write(PREAMBLE);
-    outputA = spi.write(CHAN_1);
-    outputB = spi.write(0xFF);
-    cs_ADC[channel] = 1;
-    
-    outputA = outputA & DATA_MASK;
-    outputA = outputA<<8;
-    output = (outputA | outputB);
-    
-    dblOutput = (double)(output);
-    dblOutput = dblOutput-502.0;
-    dblOutput = dblOutput/4095.0*8.0;
-    return dblOutput;
-}
-
-void SendReceiveData(int channel) {
-    int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
-    
-    // Get data from controller
-    spi.format(16,2);
-    spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
-    mutChannel[channel].lock(); // Lock mutex for specific Channel
-    cs_LL[channel] = 0; // Select relevant chip
-    intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); // Transmit & receive
-    cs_LL[channel] = 1; // Deselect chip
-    isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
-    if(channel == 0) {
-        intGlobalTest = intPosSPI_Rx[channel];
-        dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
-    }
-    
-    // Sort out received data
-    chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
-    
-    intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
-    //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
-    mutChannel[channel].unlock();//unlock mutex for specific channel
-    //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
-}
-
-// Common rise handler function 
-void common_rise_handler(int channel) {
-    pinCheck = 1;
-    if (isDataReady[channel]) { // Check if data is ready for tranmission
-        ThreadID[channel] = queue.call(SendReceiveData,channel); // Schedule transmission
-    }
-}
-
-// Common fall handler functions
-void common_fall_handler(int channel) {
-    pinCheck = 0;
-    queue.cancel(ThreadID[channel]); // Cancel relevant queued event
-}
-
-// Stub rise functions
-void rise0(void) { common_rise_handler(0); }
-void rise1(void) { common_rise_handler(1); }
-void rise2(void) { common_rise_handler(2); }
-void rise3(void) { common_rise_handler(3); }
-void rise4(void) { common_rise_handler(4); }
-void rise5(void) { common_rise_handler(5); }
-void rise6(void) { common_rise_handler(6); }
-void rise7(void) { common_rise_handler(7); }
-
-// Stub fall functions
-void fall0(void) { common_fall_handler(0); }
-void fall1(void) { common_fall_handler(1); }
-void fall2(void) { common_fall_handler(2); }
-void fall3(void) { common_fall_handler(3); }
-void fall4(void) { common_fall_handler(4); }
-void fall5(void) { common_fall_handler(5); }
-void fall6(void) { common_fall_handler(6); }
-void fall7(void) { common_fall_handler(7); }
+/*double dblGlobalTest;
+int intGlobalTest;*/
 
 void startPathPlan() { // Plan a new linear path after receiving new target data
     semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
@@ -368,10 +233,10 @@
             
             // Calculate next step in smooth path
             dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
-            mutChannel[jj].lock(); // MUTEX LOCK
+            llcomms.mutChannel[jj].lock(); // MUTEX LOCK
             intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
             intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; // Ensure number is 13-bit 
-            mutChannel[jj].unlock(); // MUTEX UNLOCK
+            llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
           
             isDataReady[jj] = 1;//signal that data ready
         } // end for
@@ -384,22 +249,84 @@
     } // end while
 }
 
+// NEED TO FIGURE OUT POINTER-TO-MEMBER FUNCTIONS TO MOVE THESE INTO LLComms CLASS
+void SendReceiveData(int channel, int _intDemandPos_Tx[], bool (*_isDataReady)[8]) {
+    int intPosSPI_Rx[N_CHANNELS]; // 13 bit value received over SPI from the actuator
+    
+    // Get data from controller
+    llcomms.spi.format(16,2);
+    llcomms.spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
+    llcomms.mutChannel[channel].lock(); // Lock mutex for specific Channel
+    *llcomms.cs_LL[channel] = 0; // Select relevant chip
+    intPosSPI_Rx[channel] = llcomms.spi.write(_intDemandPos_Tx[channel]); // Transmit & receive
+    *llcomms.cs_LL[channel] = 1; // Deselect chip
+    *_isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
+    /*if(channel == 0) {
+        intGlobalTest = intPosSPI_Rx[channel];
+        dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
+    }*/
+    
+    // Sort out received data
+    llcomms.chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
+    
+    intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
+    //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
+    llcomms.mutChannel[channel].unlock();//unlock mutex for specific channel
+    //printf("%d, %d\r\n",intPosSPI_Rx[0],_intDemandPos_Tx[0]);
+}
+// Common rise handler function 
+void common_rise_handler(int channel) {
+    llcomms.pinCheck = 1;
+    if (isDataReady[channel]) { // Check if data is ready for tranmission
+        //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
+        //llcomms.ThreadID[channel] = queue.call(&llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
+        //llcomms.ThreadID[channel] = queue.call(mbed::Callback(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
+       // llcomms.ThreadID[channel] = queue.call(mbed::Callback<void()>(&llcomms,&LLComms::SendReceiveData),channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
+        //llcomms.ThreadID[channel] = queue.call(&llcomms,*llcomms.SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
+        llcomms.ThreadID[channel] = queue.call(&SendReceiveData,channel,intDemandPos_Tx,&isDataReady); // Schedule transmission
+        //callback(&low_pass1, &LowPass::step)
+    }
+}
+// Common fall handler functions
+void common_fall_handler(int channel) {
+    llcomms.pinCheck = 0;
+    queue.cancel(llcomms.ThreadID[channel]); // Cancel relevant queued event
+}
+// Stub rise functions
+void rise0(void) { common_rise_handler(0); }
+void rise1(void) { common_rise_handler(1); }
+void rise2(void) { common_rise_handler(2); }
+void rise3(void) { common_rise_handler(3); }
+void rise4(void) { common_rise_handler(4); }
+void rise5(void) { common_rise_handler(5); }
+void rise6(void) { common_rise_handler(6); }
+void rise7(void) { common_rise_handler(7); }
+// Stub fall functions
+void fall0(void) { common_fall_handler(0); }
+void fall1(void) { common_fall_handler(1); }
+void fall2(void) { common_fall_handler(2); }
+void fall3(void) { common_fall_handler(3); }
+void fall4(void) { common_fall_handler(4); }
+void fall5(void) { common_fall_handler(5); }
+void fall6(void) { common_fall_handler(6); }
+void fall7(void) { common_fall_handler(7); }
+
 int main() {
     int ii;
     // Initialise relevant variables
     for(ii = 0; ii<N_CHANNELS; ii++) {
         // All chip selects in off state
-        cs_LL[ii] = 1;
-        cs_ADC[ii] = 1;
+        *llcomms.cs_LL[ii] = 1;
+        *llcomms.cs_ADC[ii] = 1;
         // Data ready flags set to not ready
         isDataReady[ii] = 0;
     }    
     
-    pinReset = 1; // Initialise reset pin to not reset the controllers.
+    llcomms.pinReset = 1; // Initialise reset pin to not reset the controllers.
     wait(0.25);
-    pinReset=0; // Reset controllers to be safe
+    llcomms.pinReset=0; // Reset controllers to be safe
     wait(0.25);
-    pinReset = 1; // Ready to go
+    llcomms.pinReset = 1; // Ready to go
     
     pc.baud(BAUD_RATE);
     printf("Hi, there! I'll be your mid-level controller for today.\r\n");
@@ -407,23 +334,23 @@
     t.start(callback(&queue, &EventQueue::dispatch_forever)); // Start the event queue
     
     // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
-    pinGate0.rise(&rise0);
-    pinGate1.rise(&rise1);
-    pinGate2.rise(&rise2);
-    pinGate3.rise(&rise3);
-    pinGate4.rise(&rise4);
-    pinGate5.rise(&rise5);
-    pinGate6.rise(&rise6);
-    pinGate7.rise(&rise7);
+    llcomms.pinGate0.rise(&rise0);
+    llcomms.pinGate1.rise(&rise1);
+    llcomms.pinGate2.rise(&rise2);
+    llcomms.pinGate3.rise(&rise3);
+    llcomms.pinGate4.rise(&rise4);
+    llcomms.pinGate5.rise(&rise5);
+    llcomms.pinGate6.rise(&rise6);
+    llcomms.pinGate7.rise(&rise7);
     // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
-    pinGate0.fall(&fall0);
-    pinGate1.fall(&fall1);
-    pinGate2.fall(&fall2);
-    pinGate3.fall(&fall3);
-    pinGate4.fall(&fall4);
-    pinGate5.fall(&fall5);
-    pinGate6.fall(&fall6);
-    pinGate7.fall(&fall7);
+    llcomms.pinGate0.fall(&fall0);
+    llcomms.pinGate1.fall(&fall1);
+    llcomms.pinGate2.fall(&fall2);
+    llcomms.pinGate3.fall(&fall3);
+    llcomms.pinGate4.fall(&fall4);
+    llcomms.pinGate5.fall(&fall5);
+    llcomms.pinGate6.fall(&fall6);
+    llcomms.pinGate7.fall(&fall7);
     
     timer.start();