Diff: main.cpp

Revision:

10:1b6daba32452

Parent:

9:cd3607ba5643

Child:

11:7029367a1840

--- a/main.cpp	Tue Aug 07 14:15:37 2018 +0000
+++ b/main.cpp	Tue Aug 07 15:31:59 2018 +0000
@@ -39,13 +39,10 @@
 double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; // The current position of the linear path (not sent to actuator)
 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
-
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 LLComms llcomms(LOW_LEVEL_SPI_FREQUENCY);//(N_CHANNELS);
 
-EventQueue queue(32 * EVENTS_EVENT_SIZE);
-Thread t(osPriorityRealtime);
+Thread threadLowLevelSPI(osPriorityRealtime);
 Thread threadReceiveAndReplan(osPriorityBelowNormal);
 Thread threadSmoothPathPlan(osPriorityNormal);
 
@@ -55,8 +52,6 @@
 Timer timer;
 Ticker PathCalculationTicker;
 
-bool isDataReady[N_CHANNELS] = { 0 }; // flag to indicate path data is ready for transmission to low level.
-
 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
 }
@@ -225,11 +220,11 @@
             // Calculate next step in smooth path
             dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
             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 
+            llcomms.demandPosition[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
+            llcomms.demandPosition[jj] = llcomms.demandPosition[jj] & 0x1FFF; // Ensure number is 13-bit
             llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
           
-            isDataReady[jj] = 1;//signal that data ready
+            llcomms.isDataReady[jj] = 1; // Signal that data ready
         } // end for
         
         //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
@@ -240,99 +235,14 @@
     } // 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
-        //llcomms.ThreadID[channel] = queue.call(&llcomms,&LLComms::dSendReceiveData,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() {    
     pc.baud(BAUD_RATE);
     printf("Hi, there! I'll be your mid-level controller for today.\r\n");
     wait(3);
     
-    llcomms.reset();
-    t.start(callback(&queue, &EventQueue::dispatch_forever)); // Start the event queue
-    
-    // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
-    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
-    llcomms.pinGate0.fall(&fall0);
-    //llcomms.pinGate0.fall(&llcomms,&LLComms::dfall0);
-    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();
 
+    threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
     threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
     threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread