Diff: main.cpp

Revision:

3:c83291bf9fd2

Parent:

2:eea12b149dba

Child:

4:303584310071

--- a/main.cpp	Tue Jul 31 10:44:10 2018 +0000
+++ b/main.cpp	Tue Jul 31 14:30:40 2018 +0000
@@ -4,21 +4,6 @@
 //#include "mbed.h"
 #include "mbed_events.h"
 
-// COMMS
-/*#include "EthernetInterface.h"
-#include "TCPServer.h"
-#include "TCPSocket.h"
-#include <sstream> // stringstream
-#include <vector> // vector
-#include <string.h> // strtok
-const bool IS_DHCP = false;
-#define PORT 80
-struct msg_format {
-    double psi[3][3];
-    //unsigned short duration;
-    double duration;
-} input;*/
-
 //ADC SPI stuff
 #define PREAMBLE 0x06
 #define CHAN_1 0x30
@@ -44,7 +29,7 @@
 #define PATH_SAMPLE_TIME_S 0.05
 
 #define MAX_LENGTH_MM 100.0
-#define MAX_ACTUATOR_LENGTH 50.0
+#define MAX_ACTUATOR_LENGTH 52.2
 #define MAX_SPEED_MMPS 24.3457
 #define PATH_TOLERANCE_MM 0.2 
 
@@ -96,15 +81,36 @@
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 
-SPI spi(PC_12,PC_11, PC_10); // mosi, miso, sclk
+
+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);
+
+//InterruptIn pinGate[N_CHANNELS] ={PF_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins
 
-DigitalOut cs_LL[N_CHANNELS] = {PF_10, PD_13, PE_7, PD_12, PD_14, PD_11, PD_15, PE_10};//chip select for low level controller
-DigitalOut cs_ADC[N_CHANNELS] = {PF_1, PF_0, PD_1, PD_0, PG_0, PE_1, PG_9, PG_12}; //chip select for ADC
+//WRONG!!!!!!!!!!
+//InterruptIn pinGate[N_CHANNELS] ={PG_11, PG_14, PF_15, PF_12, PF_3, PF_13, PE_11, PE_13};//gate interrupt pins
+//WRONG!!!!!
 
-InterruptIn pinGate[N_CHANNELS] ={PE_11, PE_13, PF_3, PF_13, PF_15, PF_12, PF_11, PG_14};//gate interrupt pins
+//InterruptIn testPinGate(PF_11);
+//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); //
+InterruptIn pinGate7(PE_13);
 
 DigitalOut pinReset(PD_2); //reset pin for all controllers.
 
+
 EventQueue queue(32 * EVENTS_EVENT_SIZE);
 
 Thread t(osPriorityRealtime);
@@ -144,11 +150,12 @@
 
 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
 
-unsigned int ReadADCPosition(int channel)
+double ReadADCPosition(int channel)
 {
     unsigned int outputA;
     unsigned int outputB;
-    unsigned int output;
+    int output;
+    double dblOutput;
 
     spi.format(8,0);
     spi.frequency(1000000);
@@ -162,15 +169,17 @@
     outputA = outputA & DATA_MASK;
     outputA = outputA<<8;
     output = (outputA | outputB);
-    
-    return output;
+    output = 4095- output;
+    dblOutput = (double) (output - 1504)/4095*100.0;
+    return dblOutput;
 }
     
-unsigned int ReadADCPressure(int channel)
+double ReadADCPressure(int channel)
 {
     unsigned int outputA;
     unsigned int outputB;
-    unsigned int output;
+    int output;
+    double dblOutput;
 
     spi.format(8,0);
     spi.frequency(1000000);
@@ -185,11 +194,13 @@
     outputA = outputA<<8;
     output = (outputA | outputB);
     
-    return output;
+    dblOutput = (double) (output-502)/4095*8.0;
+    return dblOutput;
 }
 
 void SendReceiveData(int channel) 
 {
+    
     //get data from controller
     spi.format(16,2);
     spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
@@ -199,32 +210,74 @@
     cs_LL[channel] = 1;//deselect chip
     isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
     
-    dblPressure_bar[channel] = ReadADCPressure(channel);
-    dblPosition_mtrs[channel] = ReadADCPosition(channel);
+
     
     //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/dblActuatorConversion[channel])/1000;
+    //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
+    
+}
+
+void testSendReceiveData() 
+{
+    printf("x\r\n");
+    //get data from controller
+    spi.format(16,2);
+    spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
+    
+    cs_LL[0] = 0;//select relevant chip
+    intPosSPI_Rx[0] = spi.write(intDemandPos_Tx[0]); //transmit & receive
+    cs_LL[0] = 1;//deselect chip
+    isDataReady[0] = 0;//data no longer ready, i.e. we now require new data
+    
+    //sort out received data
+    chrErrorFlag[0] = intPosSPI_Rx[0]>>13;
+    intPosSPI_Rx[0] = intPosSPI_Rx[0] & 0x1FFF;
+    dblPosition_mtrs[0] = (double)intPosSPI_Rx[0]/8191*(MAX_ACTUATOR_LENGTH/dblActuatorConversion[0])/1000;
+    
+    
 }
 
 //common rise handler function 
 
 void common_rise_handler(int channel)
 {
+    pinCheck = 1;
     //check if data is ready for tranmission
     if (isDataReady[channel])
     {
+        
         // Add transmit task to event queue
         ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
     }
 }
 
+void testRiseFunction(void)
+{
+    pinCheck = 1;
+    //check if data is ready for tranmission
+    if (isDataReady[0])
+    {
+        // Add transmit task to event queue
+        ThreadID[0] = queue.call(testSendReceiveData);//schedule transmission
+    }
+}
+
+void testFallFunction(void)
+{
+    pinCheck = 0;
+    //cancel relevant queued event    
+    queue.cancel(ThreadID[0]);
+}
+
 
 
 //common_fall handler functions
 void common_fall_handler(int channel) 
 {
+    pinCheck = 0;
     //cancel relevant queued event    
     queue.cancel(ThreadID[channel]);
 }
@@ -383,19 +436,22 @@
             //convert to actuator distances
             dblPathToActuator[ii] = dblSmoothPathCurrentPos_mm[ii];
             
-            intDemandPos_Tx[ii] = (int) (dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH)*8191;//convert to a 13-bit number;
+            intDemandPos_Tx[ii] = (int) ((dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
+            
             intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
             
+            dblPressure_bar[ii] = ReadADCPressure(ii);
+            dblPosition_mtrs[ii] = ReadADCPosition(ii);
           
             isDataReady[ii] = 1;//signal that data ready
 
         }
-        
-        //printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPathCurrentPos_mm[0], dblSmoothPathCurrentPos_mm[0]);
+        printf("%f, %d\r\n",dblPathToActuator[0], intDemandPos_Tx[0]); 
+        //printf("%f,%f, %f, %f\r\n", dblTargetActLen_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
     }
 }
 
-/*void SimulateDemand() // For testing purposes
+void SimulateDemand() // For testing purposes
 {
     int kk = 0;
     while(1)
@@ -404,7 +460,7 @@
         if(kk == 0)
         {
             dblPSI[0][0] = (double) 10.0;
-            dblTargetTime_s = 1.0;
+            dblTargetTime_s = 1.5;
         }
         else
         { 
@@ -422,7 +478,7 @@
     
         Thread::wait(7000);
     }
-}*/
+}
 
 
 
@@ -430,9 +486,18 @@
 
 int main() 
 {
+    for (ii = 0; ii <250; ii++)
+    {
+        pinCheck = 1;
+        wait(0.001);
+        pinCheck = 0;
+        wait(0.001);
+    }
+    
     //initialise relevant variables
     for(ii = 0; ii<N_CHANNELS; ii++)
     {
+        
         //all chip selects in off state
         cs_LL[ii] = 1;
         cs_ADC[ii] = 1;
@@ -445,6 +510,10 @@
     dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm. 
     
     pinReset = 1; //initialise reset pin to not reset the controllers.
+    wait(0.1);
+    pinReset=0; //reset controllers to be safe
+    wait(0.1);
+    pinReset = 1;//ready to go
     
     //say something nice to the user.
     pc.baud(9600);
@@ -456,24 +525,38 @@
     //set up the interrupts
 
     //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
-    pinGate[0].rise(&rise0);
-    pinGate[1].rise(&rise1);
-    pinGate[2].rise(&rise2);
-    pinGate[3].rise(&rise3);
-    pinGate[4].rise(&rise4);
-    pinGate[5].rise(&rise5);
-    pinGate[6].rise(&rise6);
-    pinGate[7].rise(&rise7);
+    //
+    //pinGate[0].rise(&testRiseFunction);
+    //testPinGate.rise(&testRiseFunction);//<working
+    //testPinGate.rise(&rise0);
+    
+    pinGate0.rise(&rise0);
+    /*
+    pinGate1.rise(&rise1);
+    pinGate2.rise(&rise2);
+    pinGate3.rise(&rise3);
+    pinGate4.rise(&rise4);
+    pinGate5.rise(&rise5);
+    pinGate6.rise(&rise6);
+    pinGate7.rise(&rise7);
+    */
     
     //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
-    pinGate[0].fall(&fall0);
-    pinGate[1].fall(&fall1);
-    pinGate[2].fall(&fall2);
-    pinGate[3].fall(&fall3);
-    pinGate[4].fall(&fall4);
-    pinGate[5].fall(&fall5);
-    pinGate[6].fall(&fall6);
-    pinGate[7].fall(&fall7);
+    //
+    //pinGate[0].fall(&testFallFunction);
+    //testPinGate.fall(&testFallFunction); <working
+    //testPinGate.fall(&fall0);
+    
+    pinGate0.fall(&fall0);
+    /*
+    pinGate1.fall(&fall1);
+    pinGate2.fall(&fall2);
+    pinGate3.fall(&fall3);
+    pinGate4.fall(&fall4);
+    pinGate5.fall(&fall5);
+    pinGate6.fall(&fall6);
+    pinGate7.fall(&fall7);
+    */
     
     timer.start();
     
@@ -484,7 +567,7 @@
     
     threadReplan.start(ReplanPath);//start Replanning thread 
      Thread::wait(1);  
-     
+    
       
     while(1) {
         Thread::wait(osWaitForever);