Revision 42:5a5ad23a4bb1, committed 2021-06-24

Comitter:

dofydoink

Date:

Thu Jun 24 20:34:47 2021 +0000

Parent:

41:0e3898b29230

Commit message:

first

Changed in this revision

--- a/LLComms.cpp	Thu Sep 03 16:30:23 2020 +0000
+++ b/LLComms.cpp	Thu Jun 24 20:34:47 2021 +0000
@@ -4,30 +4,25 @@
 
 LLComms::LLComms() : 
     queue(32 * EVENTS_EVENT_SIZE), //32 //8 * EVENTS_EVENT_SIZE
-    pinGate6(PA_5),
     spi_0(PC_12, PC_11, PC_10),
-    spi_1(PF_9, PF_8, PF_7),
-    spi_2(PB_15, PC_2, PB_13),
+//    spi_1(PF_9, PF_8, PF_7),
+//    spi_2(PB_15, PC_2, PB_13),
     // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
     pinGate0(PF_11),
     pinGate1(PG_14),
     pinGate2(PF_15),
-    pinGate3(PF_12),
-    pinGate4(PF_3),
-    pinGate5(PC_7),
-    pinGate7(PE_13),
-    pinGate8(PF_4),
+    //pinGate3(PF_12),
     pinReset(PD_2)
 { // Constructor
 
     spi_0.format(16,2);
     spi_0.frequency(LOW_LEVEL_SPI_FREQUENCY);
-    spi_1.format(16,2);
-    spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY);
-    spi_2.format(16,2);
-    spi_2.frequency(LOW_LEVEL_SPI_FREQUENCY);
+//    spi_1.format(16,2);
+//    spi_1.frequency(LOW_LEVEL_SPI_FREQUENCY);
 
-    PinName LLPins[N_CHANNELS] = {PD_15, PE_10, PD_11, PD_14, PE_7, PB_1, PB_12, PD_13, PB_5};
+
+    PinName LLPins[N_CHANNELS] = {PD_15, PE_10, PD_14};//, PD_11};//, PE_7, PB_1, PB_12, PD_13, PB_5};
+    
     for (short int i=0; i<N_CHANNELS; i++) {
         isDataReady[i] = 0;
         cs_LL[i] = new DigitalOut(LLPins[i]);
@@ -50,27 +45,26 @@
     pinReset=0; // Reset controllers to be safe
     wait(0.25);
     pinReset = 1; // Ready to go
+
+//    pinReset.mode(PullUp); // Initialise reset pin to not reset the controllers.
+//    wait(0.25);
+//    pinReset.write(0); // Reset controllers to be safe
+//    wait(0.25);
+//    pinReset.mode(PullUp); // Ready to go
+//    wait(0.25);
     
     // Set up rise interrupts MIGHT NOT NEED TO BE POINTERS
     pinGate0.rise(callback(this,&LLComms::rise0));
     pinGate1.rise(callback(this,&LLComms::rise1));
     pinGate2.rise(callback(this,&LLComms::rise2));
-    pinGate3.rise(callback(this,&LLComms::rise3));
-    pinGate4.rise(callback(this,&LLComms::rise4));
-    pinGate5.rise(callback(this,&LLComms::rise5));
-    pinGate6.rise(callback(this,&LLComms::rise6));
-    pinGate7.rise(callback(this,&LLComms::rise7));
-    pinGate8.rise(callback(this,&LLComms::rise8));
+    //pinGate3.rise(callback(this,&LLComms::rise3));
+
     // Set up fall interrupts MIGHT NOT NEED TO BE POINTERS
     pinGate0.fall(callback(this,&LLComms::fall0));
     pinGate1.fall(callback(this,&LLComms::fall1));
     pinGate2.fall(callback(this,&LLComms::fall2));
-    pinGate3.fall(callback(this,&LLComms::fall3));
-    pinGate4.fall(callback(this,&LLComms::fall4));
-    pinGate5.fall(callback(this,&LLComms::fall5));
-    pinGate6.fall(callback(this,&LLComms::fall6));
-    pinGate7.fall(callback(this,&LLComms::fall7));
-    pinGate8.fall(callback(this,&LLComms::fall8));
+    //pinGate3.fall(callback(this,&LLComms::fall3));
+
 }
 
 //LLComms::~LLComms(void) { } // Destructor
@@ -141,6 +135,7 @@
         } else {
             inboundMsg = spi->write(dummyMsg);
         }
+        //rawMsg[i] = inboundMsg;
         if((unsigned int)inboundMsg != dummyMsg) { // Message is not dummy which is only used for reply
             typeBit = inboundMsg>>1 & 0x1;
             inboundMsgsData[typeBit] = inboundMsg>>7 & 0x1FF;
@@ -158,18 +153,21 @@
     // Construct messages
     unsigned int intPositionMsg = formatMessage(0,demandPosition_mm[channel],MAX_ACTUATOR_LIMIT_MM);
     unsigned int intVelocityMsg = formatMessage(1,demandSpeed_mmps[channel],MAX_SPEED_MMPS);
-    
+    //unsigned int rawMsg[3];
     *cs_LL[channel] = 0; // Select relevant chip
     unsigned int outboundMsgs[2] = { intPositionMsg , intVelocityMsg };
     unsigned int inboundMsgsData[2] = { 0 };
     bool isSPIsuccess = false;
-    if( channel < 4 ) {
+    if( channel < 3 ) {
         isSPIsuccess = PerformMasterSPI(&spi_0,outboundMsgs,inboundMsgsData);
-    } else if( channel < 7 ) {
-        isSPIsuccess = PerformMasterSPI(&spi_1,outboundMsgs,inboundMsgsData);
-    } else {
-        isSPIsuccess = PerformMasterSPI(&spi_2,outboundMsgs,inboundMsgsData);
-    }
+    } //else {
+//        isSPIsuccess = PerformMasterSPI(&spi_1,outboundMsgs,inboundMsgsData,rawMsg);
+//    }
+//    else if( channel < 7 ) {
+//        isSPIsuccess = PerformMasterSPI(&spi_1,outboundMsgs,inboundMsgsData);
+//    } else {
+//        isSPIsuccess = PerformMasterSPI(&spi_2,outboundMsgs,inboundMsgsData);
+//    }
     *cs_LL[channel] = 1; // Deselect chip
     if( isSPIsuccess ) {
         isDataReady[channel] = 0; // Data no longer ready, i.e. we now require new data
@@ -180,15 +178,25 @@
         pressureSensor_bar[channel] = ((double)inboundMsgsData[1]/511) * (double)MAX_PRESSURE_LIMIT;
         pressureSensor_bar[channel] = min( max(pressureSensor_bar[channel],0.0) , (double)MAX_PRESSURE_LIMIT );
     } else { // Data is STILL ready and will be resent at the next pin interrupt
-        //printf("SPI failed: %d%d. Resending.\n\r",isPositionValid,isPressureValid);
+        printf("SPI failed: %d. Resending.\n\r",channel);
     }
     
     mutChannel[channel].unlock();//unlock mutex for specific channel
+    
+ //   if(channel ==3){
+//        printf("%x\t%x\t%x\r\n",rawMsg[0], rawMsg[1], rawMsg[2] );
+//    }
+
+//    printf("%d, ", channel);
+//    if(channel == 3){
+//        printf("\r\n");
+//    }
 }
 
 // Common rise handler function 
 void LLComms::common_rise_handler(int channel) {
     //printf("%d %d common_rise_handler\n\r",channel,isDataReady[channel]);
+    //countervar++;
     if (isDataReady[channel]) { // Check if data is ready for transmission
         ThreadID[channel] = queue.call(this,&LLComms::SendReceiveData,channel); // Schedule transmission
     }
@@ -203,19 +211,10 @@
 void LLComms::rise0(void) { common_rise_handler(0); }
 void LLComms::rise1(void) { common_rise_handler(1); }
 void LLComms::rise2(void) { common_rise_handler(2); }
-void LLComms::rise3(void) { common_rise_handler(3); }
-void LLComms::rise4(void) { common_rise_handler(4); }
-void LLComms::rise5(void) { common_rise_handler(5); }
-void LLComms::rise6(void) { common_rise_handler(6); }
-void LLComms::rise7(void) { common_rise_handler(7); }
-void LLComms::rise8(void) { common_rise_handler(8); }
+//void LLComms::rise3(void) { common_rise_handler(3); }
+
 // Stub fall functions
 void LLComms::fall0(void) { common_fall_handler(0); }
 void LLComms::fall1(void) { common_fall_handler(1); }
 void LLComms::fall2(void) { common_fall_handler(2); }
-void LLComms::fall3(void) { common_fall_handler(3); }
-void LLComms::fall4(void) { common_fall_handler(4); }
-void LLComms::fall5(void) { common_fall_handler(5); }
-void LLComms::fall6(void) { common_fall_handler(6); }
-void LLComms::fall7(void) { common_fall_handler(7); }
-void LLComms::fall8(void) { common_fall_handler(8); }
\ No newline at end of file
+//void LLComms::fall3(void) { common_fall_handler(3); }

--- a/LLComms.h	Thu Sep 03 16:30:23 2020 +0000
+++ b/LLComms.h	Thu Jun 24 20:34:47 2021 +0000
@@ -35,6 +35,12 @@
         double positionSensor_mm[N_CHANNELS]; // The actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
         unsigned int pressureSensor_uint[N_CHANNELS];
         double pressureSensor_bar[N_CHANNELS];  // The pressure in a given chamber in bar (1 bar  = 100,000 Pa)
+        unsigned int formatMessage(short int type, double dblValue, double dblMaxValue);
+        //bool CheckMessage(int msg, short int trueType);
+        bool CheckMessage(int msg);
+        bool PerformMasterSPI(SPI *spi, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]);
+        void SendReceiveData(int channel);
+        int countervar;     
         
         LLComms(); // Constructor
         //~LLComms(); // Destructor
@@ -42,50 +48,30 @@
     private:
     
         // PIN DECLARATIONS     
-        InterruptIn pinGate6; // This pin HAS TO BE defined before SPI set up. No Clue Why.
         SPI spi_0; // mosi, miso, sclk
-        SPI spi_1;
-        SPI spi_2;
+        //SPI spi_1; // mosi, miso, sclk
         DigitalOut* cs_LL[N_CHANNELS]; // Chip select for low level controller
         //DigitalOut* cs_ADC[N_CHANNELS]; // Chip select for ADC
         // These interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
         InterruptIn pinGate0;
         InterruptIn pinGate1;
         InterruptIn pinGate2;
-        InterruptIn pinGate3;
-        InterruptIn pinGate4;
-        InterruptIn pinGate5;
-        InterruptIn pinGate7;
-        InterruptIn pinGate8;
-        DigitalOut pinReset; // Reset pin for all controllers.
+        //InterruptIn pinGate3;
+        DigitalInOut pinReset; // Reset pin for all controllers.
         
         int ThreadID[N_CHANNELS];
         
-        unsigned int formatMessage(short int type, double dblValue, double dblMaxValue);
-        //bool CheckMessage(int msg, short int trueType);
-        bool CheckMessage(int msg);
-        bool PerformMasterSPI(SPI *spi, unsigned int outboundMsgs[], unsigned int inboundMsgsData[]);
-        void SendReceiveData(int channel);
+        
         void common_rise_handler(int channel);
         void common_fall_handler(int channel);
         void rise0(void);
         void rise1(void);
         void rise2(void);
-        void rise3(void);
-        void rise4(void);
-        void rise5(void);
-        void rise6(void);
-        void rise7(void);
-        void rise8(void);
+        //void rise3(void);
         void fall0(void);
         void fall1(void);
         void fall2(void);
-        void fall3(void);
-        void fall4(void);
-        void fall5(void);
-        void fall6(void);
-        void fall7(void);
-        void fall8(void);
+        //void fall3(void);
 
 };
 

--- a/MLSettings.h	Thu Sep 03 16:30:23 2020 +0000
+++ b/MLSettings.h	Thu Jun 24 20:34:47 2021 +0000
@@ -4,18 +4,20 @@
 #define MLSETTINGS_H
 
 // GENERAL SETTINGS
-const unsigned short int N_CHANNELS = 9; // Number of channels to control
+const unsigned short int N_CHANNELS = 3; // Number of channels to control
 // 1-3: front segment; 4-6: rear segment; 7-8: mid segment
 const int BAUD_RATE = 115200; //9600; //115200
+const float PI = 3.14159265359;
 
 // HL COMMUNICATION SETTINGS
 const bool IS_DHCP = false;
 const unsigned short int HL_COMMS_FREQ_HZ = 200; // Frequency at which communications can happen with HL, if messages are waiting
 
 // REAL TIME FREQUENCIES
-const short int SENSOR_FEEDBACK_HZ = 20; // Frequency at which sensor messages are queued / published to HL
-const float LL_DEMANDS_FREQ_HZ = 50;
-const unsigned int LOW_LEVEL_SPI_FREQUENCY = 10000000;
+const short int SENSOR_FEEDBACK_HZ = 5; // Frequency at which sensor messages are queued / published to HL
+const float LL_DEMANDS_FREQ_HZ = 5;
+const unsigned int LOW_LEVEL_SPI_FREQUENCY = 1000000;
+const double HHC_READ_FREQ_HZ = 50;
 
 const float MAX_PRESSURE_LIMIT = 12.0;
 
@@ -23,5 +25,18 @@
 const float MAX_ACTUATOR_LIMIT_MM = 40.0;
 const float MAX_ACTUATOR_LENGTH_MM = 52.2;
 const float MAX_SPEED_MMPS = 24.3457;
+const float MAX_DRIVE_SEGMENT_POSITION = 8.0;//the maximum pressure attainable by LL
+const double MAX_EXTENSION_PRESSURE = 6.0; //the desired maximum operating pressure.
+const double MAX_DRIVE_SEGMENT_SPEED = 4.0;
+const double SAFE_CONTRACT_SPEED = 2.0;
+const double MAX_STEER_SPEED_RADpS = 1.3; //1.212 rad/s was calculated
+const double SAFE_SPEED_MMpS = 8.0;
+const double ANCHOR_POSITION = 25.0;
+const double HOME_DRIVE_POSITION = 0.0;
+
+// KINEMATIC LIMTS & CONSTANTS
+const float MAX_THETA_RAD = 3.665191429; //210 degrees
+const float ACTUATOR_OFFSET = 6.0; //mm
+const double ACT_CONV = 6.055836122; // mm/mL convert injected volume in [mL] to actuator distance in [mm]
 
 #endif
\ No newline at end of file

--- a/main.cpp	Thu Sep 03 16:30:23 2020 +0000
+++ b/main.cpp	Thu Jun 24 20:34:47 2021 +0000
@@ -2,155 +2,841 @@
 #include "math.h"
 // MBED IMPORTS
 #include "mbed.h"
-#include "mbed_events.h"
+
 // CUSTOM IMPORTS
 #include "MLSettings.h"
-#include "HLComms.h"
-#include "LLComms.h"
+//#include "HLComms.h"
+
+#define N_CHANNELS 4
 
 // DEMAND VARIABLES
-double dblDemandSpeed_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
-double dblDemandPosition_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
-LLComms llcomms;
-HLComms hlcomms(HL_COMMS_FREQ_HZ);
+
+//HLComms hlcomms(HL_COMMS_FREQ_HZ);
+
+double limDouble(double var, double min, double max){
+    double output = var;
+    if(var <min){
+        output = min;
+    }
+    if (var>max){
+        output = max;
+    }
+    return output;
+}
+    
+
+//Ticker setDemandsTicker;
+
+
+//joystick input pins
+
+InterruptIn pinZeroIn   (PD_4);
+InterruptIn pinCycleIn  (PD_6);
+InterruptIn pinSucIn    (PD_7);
+InterruptIn pinGasIn    (PF_5);
+InterruptIn pinDebugIn  (PF_9);
+InterruptIn pinFlushIn  (PG_1);
+
+DigitalIn   pinWashIn   (PD_5);
+DigitalIn   pinFwd      (PD_3);
+DigitalIn   pinRev      (PC_3);
+
+
+AnalogIn    pinJSX      (PC_0);
+AnalogIn    pinJSY      (PF_3);
+
+DigitalOut  pinFlushOut (PG_0);
+DigitalOut  pinGasOut   (PD_0);
+DigitalOut  pinWashOut  (PF_1);
+DigitalOut  pinSucOut   (PD_1);
+DigitalOut  pinThirdPump(PF_2);
+DigitalOut  pinJetOut   (PF_0);
+
+//DigitalIn arPins[7] = {pinZeroIn, pinCycleIn, pinSucIn, pinWashIn, pinGasIn, pinFwd, pinRev};
+
+Thread threadReadHHC(osPriorityHigh);
+Thread threadSendFeedback(osPriorityNormal);
+
+Semaphore semReadHHC(1);
+Semaphore semSendFeedback(1);
+Semaphore semReceiveAndReplan(1);
+
+//Ticker SendSensorDataTicker;
+Ticker tickerReadHHC;
+Ticker tickerSendFeedback;
+
+void signalReadHHC(){
+    semReadHHC.release();
+}
+
+void signalSendFeedback() {
+    semSendFeedback.release();
+}
+
+//support function states;
+
+bool isZero = 0;
+bool isCycle = 0;
+bool isSuc = 0;
+bool isWash = 0;
+bool isGas = 0;
+bool isFwd = 0;
+bool isRev = 0;
+bool isBusy = 0;
+bool DemPos=0;
+bool isDebug = 0;
+
+//set up PWM output pins for Demand POSITION AND VELOCITY
+PwmOut pinPosOut[N_CHANNELS] = {PB_15, PB_8, PB_9  ,PC_6};
+PwmOut pinVelOut[N_CHANNELS] = {PB_10, PA_0, PB_11 ,PB_3};
+
 
-Thread threadLowLevelSPI(osPriorityRealtime);
-Thread threadSetDemands(osPriorityNormal);
-Thread threadReceiveAndReplan(osPriorityBelowNormal);
-Thread threadSensorFeedback(osPriorityBelowNormal);
+void FlushActivate(){
+    if(!isBusy){
+        pinFlushOut = 1;
+    }
+}
+
+void FlushStop(){
+    if(!isBusy){
+        pinFlushOut = 0;
+    }
+}
+
+void ZeroActivate(){
+    if(!isBusy){ 
+        isZero = 1; 
+    }
+}
+void ZeroStop(){
+
+}
+
+void DebugActivate(){
+    isDebug = 1;
+}
+
+void DebugStop(){
+    isDebug = 0;
+}    
+
+void CycleActivate(){
+    if(!isBusy){ 
+        isCycle = 1;
+    }
+}
+void CycleStop(){
+
+}
+
+void GasActivate(){
+    if(!isBusy){
+        pinGasOut = 1;
+    }
+}
+void GasStop(){
+    pinGasOut = 0;
+}
+
+void SucActivate(){
+    if(!isBusy){
+        pinSucOut = 1;
+    }
+}
+void SucStop(){
+    pinSucOut = 0;
+}
+
+bool isChange = 1;
+
+Timer timer1;
+
+Ticker DemTicker;
+
+int intAnchorDirection = 0;
+
+
+
+//global variables
+double dblSensitivity = 1.0;
+double dblDriveStep = 0.05;
+bool isSteerLock = 0;
+double dblAlpha;
+double dblBeta;
+double dblTheta;
+double dblPhi;
+double dblS;
+double dblSdot;
+double dblX[3];
+double dblXdot[3];
+double dblAlphaDot;
+double dblBetaDot;
+double dblDeltaAlpha;
+double dblDeltaBeta;
+
+double x_axis_sign = -1.0;
+
+//kinematic constants
+double K_theta = 0.77;// rad/mL
+double d_theta = 0.3;// no unit
 
-Mutex mutPathIn;
-Semaphore semLLcomms(1);
-Semaphore semSensorData(1);
+void SendFeedback(){
+    //support function interrupt states
+    while(1){
+        semSendFeedback.wait();
+        if(!isBusy){
+            if(pinFwd.read()){ 
+                isFwd = 1;
+            } else {
+                isFwd = 0;
+            }
+            
+            if(pinRev.read()){ 
+                isRev = 1;
+            } else {
+                isRev = 0;
+            }
+            
+            if(pinSucIn.read()){ 
+                isSuc = 1;
+            } else {
+                isSuc = 0;
+            }
+            
+            if(pinWashIn.read()){ 
+                isWash = 1;
+            } else {
+                isWash = 0;
+            }
+            
+            if(pinGasIn.read()){ 
+                isGas = 1;
+            } else {
+                isGas = 0;
+            }
+            
+            
+            //
+            //printf("\r\n");
+            //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t", isZero, isGas, isSuc, isRev, isWash, isCycle, isFwd, isDebug);
+    //        
+    //        double dblJSXprint = pinJSX.read();
+    //        double dblJSYprint = pinJSY.read();
+    //        printf("X:%f\tY:%f\t", dblJSXprint, dblJSYprint);
+            
+    //        printf("%f\t",dblAlphaDot);
+    //        printf("%f\t",dblBetaDot);
+    //        printf(":: ");
+    //        
+    //        printf("%f\t",dblAlpha);
+    //        printf("%f\t",dblBeta);
+    //        printf(":: ");
+    //        
+    //        printf("%f\t",dblTheta);
+    //        printf("%f\t",dblPhi);
+    //        printf(":: ");
+            printf("\t");
+            
+            printf("%f\t",dblX[1]);
+            printf("%f\t",dblX[0]);
+            printf("%f\t",dblX[2]);
+            printf(":: ");
+            
+    //        printf("%f\t",dblXdot[0]);
+    //        printf("%f\t",dblXdot[2]);
+    //        printf("%f\t",dblXdot[1]);
+    //        printf(":: ");
+            
+            printf("%f\t",dblS);
+            printf("%f\t",dblSdot);
+            printf("\r\n");
+        }
+    }
+}
 
-Ticker setDemandsTicker;
-Ticker SendSensorDataTicker;
+double DeadZone(double var, double threshold, double max, double min){
+    
+    double output = var;
+    output -= 0.5;
+    min -= 0.5;
+    max -= 0.5; 
+    
+    if(output > threshold){
+        output = (output - threshold);
+        output /= (max - threshold);
+    } else if(output < -1.0*threshold){
+        output = -1.0*(output + threshold);
+        output /= (threshold + min);
+    } else {
+        output = 0.0;
+    }
+    
+    output = limDouble(output, -1.0, 1.0);
+    return output;
+}
+
+double ConvertToPWM(double target, double max){
+    double output;
+    output  = target*0.8/max ;
+    output += 0.1;
+    return output;
+}
 
-void sendSensorData() {
-    while( true ) {
-        semSensorData.wait();
-        hlcomms.send_sensor_message(llcomms.positionSensor_uint,llcomms.pressureSensor_uint);
+void SetFrontDemand(double demandPos[], double demandVel[]){    
+    
+    double demandPos_abs[3];
+    double demandVel_abs[3];
+    double pwmPos_front[3];
+    double pwmVel_front[3];
+    
+    for(int ii = 0; ii<3; ii++){
+        demandPos_abs[ii] = abs(demandPos[ii]);
+        demandVel_abs[ii] = abs(demandVel[ii]);
+        pwmPos_front[ii] = ConvertToPWM(demandPos_abs[ii], (double)MAX_ACTUATOR_LIMIT_MM);
+        pwmVel_front[ii] = ConvertToPWM(demandVel_abs[ii], (double)MAX_SPEED_MMPS);
+        pinPosOut[ii].write(pwmPos_front[ii]);
+        pinVelOut[ii].write(pwmVel_front[ii]);
+        
+    }
+    
+/*    
+    //print for debugging
+    printf("X: ");
+    for(int ii = 0; ii<3; ii++){
+        printf("%f\t",pwmPos_front[ii]);
+    }
+    printf("Xdot: ");
+    for(int ii = 0; ii<3; ii++){
+        printf("%f\t",pwmVel_front[ii]);
+    }
+    */
+}
+
+void SetDriveDemand(double drivePos, double driveVel){    
+    
+    double drivePos_abs;
+    double driveVel_abs;
+    double pwmPos_drive;
+    double pwmVel_drive;
+    
+    drivePos_abs = abs(drivePos);
+    driveVel_abs = abs(driveVel);
+    
+    pwmPos_drive = ConvertToPWM(drivePos_abs, MAX_DRIVE_SEGMENT_POSITION);
+    pwmVel_drive = ConvertToPWM(driveVel_abs, MAX_DRIVE_SEGMENT_SPEED);
+    
+    pinPosOut[3].write(pwmPos_drive);
+    pinVelOut[3].write(pwmVel_drive);
+    
+    /*
+    //print for debugging
+    printf("S:%f\tSdot:%f\t",pwmPos_drive,pwmVel_drive);
+    */
+}
+
+double GetMaxTravelTime(double startPositions[], double endPositions[], double speed){
+    double arT[3];
+    for(int ii = 0; ii< 3; ii++){
+        arT[ii] = endPositions[ii] - startPositions[ii];
+        arT[ii] /= speed;
+        arT[ii] = abs(arT[ii]);
+    }
+    double Tmax = 0.0;
+    for(int ii = 0; ii<3; ii++){
+        if( Tmax < arT[ii] ){
+            Tmax = arT[ii];
+        }
+    }
+    return Tmax;
+}
+
+void UpdateFrontSegPosition(double position[]){
+    for(int ii = 0; ii<3; ii++){
+        dblX[ii] = position[ii];
     }
 }
 
-void signalSendSensorData() {
-    semSensorData.release();
+void GoToNeutralPose(double speed){
+    isBusy = 1;
+    double T_wait;
+    double arSpeed[3] = {speed, speed, speed};  
+    double arPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
+    SetFrontDemand(arPosition, arSpeed);
+    
+    //T_wait = 40.0 / speed;
+    T_wait = GetMaxTravelTime(dblX, arPosition, speed);
+    T_wait += 0.25;//add a little bit of time for security
+    //print for debug    
+    printf("\r\nGoing to neutral. %f\r\n", T_wait);
+    
+    wait(T_wait);
+    UpdateFrontSegPosition(arPosition);
+    //isZero = 0;
+    isBusy = 0;
+}
+
+void GoToAnchorPose(double speed){
+    //reset front segment position
+    //isAnchor = 0;
+    isBusy = 1;
+    double T_wait;
+    double arSpeed[3] = {speed, speed, speed};  
+    double arStartPosition[3];
+    //save starting position
+    for (int ii = 0; ii<3; ii++){
+        arStartPosition[ii] = dblX[ii];
+    }
+    
+    double arAnchorPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
+
+//Go to neutral
+    GoToNeutralPose(speed);
+    
+    //select direction of anchor
+    arAnchorPosition[intAnchorDirection] = ANCHOR_POSITION;
+    intAnchorDirection++; //rotate next anchoring position
+    if(intAnchorDirection >= 3){
+        intAnchorDirection = 0;
+    }
+//go to anchor position    
+    SetFrontDemand(arAnchorPosition, arSpeed);    
+    T_wait = GetMaxTravelTime(dblX, arAnchorPosition, speed);
+    T_wait += 0.25;//add a little bit of time for security
+    printf("Going to anchor. %f s\r\n", T_wait);
+    wait(T_wait);
+    
+    //update front segment position.
+    UpdateFrontSegPosition(arAnchorPosition);
+     
+//contract
+    SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED);
+    T_wait = dblS - HOME_DRIVE_POSITION; 
+    T_wait /= SAFE_CONTRACT_SPEED;
+    printf("Contracting. %f s\r\n", T_wait);
+    wait(T_wait);
+    //update drive segment position
+    dblS = HOME_DRIVE_POSITION;
+    
+//go back to neutral pose
+    GoToNeutralPose(speed);
+    
+//go back to starting position
+    SetFrontDemand(arStartPosition, arSpeed);
+    T_wait = GetMaxTravelTime(dblX, arStartPosition, speed);
+    T_wait += 0.25;//add a little bit of time for security
+    printf("Going to intial position. %f\r\n", T_wait);
+    wait(T_wait);
+    //UpdateFrontSegPosition(arStartPosition);
+    for(int ii = 0; ii<3; ii++){
+        dblX[ii] = arStartPosition[ii];
+    }
+    
+    //print for debug
+    isBusy = 0;
+}
+
+
+void GoToCustomPose(double position[], double speed){
+    //reset front segment position
+    //isBusy = 1;
+    double T_wait;
+    double arSpeed[3] = {speed, speed, speed};  
+    for (int ii = 0; ii<3; ii++){
+       position[ii] = limDouble(position[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM);
+    }
+    SetFrontDemand(position, arSpeed);
+    
+    //T_wait = 40.0 / speed;
+    T_wait = GetMaxTravelTime(dblX, position, speed);
+    T_wait += 0.25;//add a little bit of time for security
+    //print for debug    
+    printf("\r\nGoing to custom pose. T_wait: %f s \r\n", T_wait);
+    wait(T_wait);
+    UpdateFrontSegPosition(position);
+    printf("Done\r\n");
+    //isBusy = 0;
+}
+
+
+void AllGoTo(double position, double speed){
+    //reset front segment position
+    //isBusy = 1;
+    double T_wait;
+    double arSpeed[3] = {speed, speed, speed};  
+    position = limDouble(position, 0.0, (double)MAX_ACTUATOR_LIMIT_MM);
+    
+    double arPosition[3] = {position, position, position};
+    SetFrontDemand(arPosition, arSpeed);
+    
+    //T_wait = 40.0 / speed;
+    T_wait = GetMaxTravelTime(dblX, arPosition, speed);
+    T_wait += 0.25;//add a little bit of time for security
+    //print for debug    
+    printf("\r\nMoving all to %f mm. T_wait: %f\r\n",position, T_wait);
+    
+    wait(T_wait);
+    UpdateFrontSegPosition(arPosition);
+    printf("Done\r\n");
+    //isBusy = 0;
+}
+
+void FlushSyringes(int repetitions){
+    //isBusy = 1;
+    printf("\r\n\r\n:::Flushing Syringes:::\r\n");
+    pinFlushOut = 1;
+    wait(1.0);
+    for(int ii = 0; ii<repetitions; ii++){
+        AllGoTo(40.0,20.0);
+        wait(0.5);
+        AllGoTo(0.0,2.0);
+        wait(0.5);
+    }
+    pinFlushOut = 0;
+    printf("Done\r\n");
+    //isBusy = 0;
+}
+
+void FlushSegment(int repetitions){
+    //isBusy = 1;
+    printf("\r\n\r\n:::Flushing Segment:::\r\n");
+    FlushSyringes(1);//ensure syringes are flushed.
+    double arZero[3] = {0.0, 0.0, 0.0};
+    
+    double flushPosition = 30.0;
+    
+    double arFlushPose1[3] = {flushPosition, 0.0, 0.0};
+    double arFlushPose2[3] = {0.0, flushPosition, 0.0};
+    double arFlushPose3[3] = {0.0, 0.0, flushPosition};
+    for(int ii = 0; ii<repetitions; ii++){
+        //flush first chamber
+        printf("Flushing first chamber\r\n");
+        GoToCustomPose(arFlushPose1, SAFE_SPEED_MMpS);
+        AllGoTo(0.0, SAFE_SPEED_MMpS);
+        
+        //flush second chamber
+        printf("Flushing second chamber\r\n");
+        GoToCustomPose(arFlushPose2, SAFE_SPEED_MMpS);
+        AllGoTo(0.0, SAFE_SPEED_MMpS);
+        
+        //flush third chamber
+        printf("Flushing third chamber\r\n");
+        GoToCustomPose(arFlushPose3, SAFE_SPEED_MMpS);
+        AllGoTo(0.0, SAFE_SPEED_MMpS);
+        
+        FlushSyringes(1);//flush syringes
+    }
+    printf("Done\r\n");
+    //isBusy = 0;
 }
 
-// This function will be called when a new transmission is received from high level
-void ReceiveAndReplan() {
+void FlushTubing(int repetitions){
+    //isBusy = 1;
+    printf("\r\n\r\n:::Flushing Tubing:::\r\n");
+    for (int ii = 0; ii<repetitions; ii++){
+        AllGoTo(40.0,15.0);
+        pinFlushOut = 1;
+        wait(1.0);
+        AllGoTo(0.0, 2.0);
+        pinFlushOut = 0;
+        wait(0.5);
+    }
+}
     
-    SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
+
+void DebugMode(){
+    isBusy = 1;
+    printf("\r\nEntering debug mode. Going to zero.\r\n");
+    double zeroPos[3] = {0.0, 0.0, 0.0};
+    double safeVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS};
     
-    struct demands_struct input;
-    DigitalOut SupportPins[4] = {PE_4, PE_2, PE_3, PE_6};
+   
+    SetFrontDemand(zeroPos, safeVel);
+    SetDriveDemand(0.0, SAFE_CONTRACT_SPEED);
+    double T_wait = GetMaxTravelTime(dblX, zeroPos, SAFE_SPEED_MMpS);
+    T_wait += 0.25;
+    wait(T_wait);
+    UpdateFrontSegPosition(zeroPos);
+    printf("Done\r\nPlease enter command\r\n");
+    
+    int countSyringeFlush = 0;
+    int countSegmentFlush = 0;
+    int countTubingFlush = 0;
     
-    while( true ) {
-        hlcomms.newData.wait();
-        input = hlcomms.get_demands();
-        // SUPPORT FUNCTIONS
-        // [isInsufflate,isSuction,isWashLens,isJet]
-        for(short int i=0; i<4; i++) {
-            if(i<2) { // Active low, i.e. 0 = Off
-                SupportPins[i].write((short int)input.utitilies_bool[i]);
-            } else { // Active high, i.e. 1 = Off
-                SupportPins[i].write((short int)(!input.utitilies_bool[i]));
+    while(isDebug){
+        isBusy = 1;
+        //printf("x\r\n");
+        
+        if( pinZeroIn.read() ){
+            //printf("zero%d\r\n",countSyringeFlush);
+            
+            if(countSyringeFlush == 0){
+                printf("\r\n Syringe Flush Selected\r\n");
+            }
+            countSyringeFlush++;
+            countSegmentFlush = 0;
+            countTubingFlush = 0;
+            if(countSyringeFlush > 50){
+                FlushSyringes(4);
+                countSyringeFlush = 0;
             }
+            //printf("%d\r\n", countSyringeFlush);
+            
+        }else {
+            countSyringeFlush = 0;
+        }
+        
+        if( pinSucIn.read() ){
+            //printf("suc%d\r\n", countSegmentFlush);
+            
+            if(countSegmentFlush == 0){
+                printf("\r\n Segment Flush Selected\r\n");
+            }
+            countSyringeFlush = 0;
+            countSegmentFlush++;
+            countTubingFlush = 0;
+            if(countSegmentFlush > 50){
+                FlushSegment(2);
+                countSegmentFlush = 0;
+            }
+        }else {
+            countSegmentFlush = 0;
+        }
+        
+        if( pinWashIn.read() ){
+            //printf("wash%d\r\n", countTubingFlush);
+            if(countTubingFlush == 0){
+                printf("\r\n Tubing Flush Selected\r\n");
+            }
+            countSyringeFlush = 0;
+            countSegmentFlush = 0;
+            countTubingFlush++;
+            if(countTubingFlush > 50){
+                FlushTubing(2);
+                countTubingFlush = 0;
+            }
+        }else {
+            countTubingFlush = 0;
         }
-        // PROCESS INPUT
-        double maxTimesToTarget_s[3] = { -1.0 };
-        //[8,7,6,4,3,-1,-1,0,-1]
-        //FRONT = channels 0,1,2
-        //MID = channels 3,4(,5)
-        //REAR = channels (6,)7(,8)
-        // Lock mutex, preventing setDemandsForLL from running
-        mutPathIn.lock();
-        for(short int segNum=0; segNum<3; segNum++) {
-            // Limit requested speed
-            if( input.speeds_mmps[segNum] > 0 ) {
-                double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS );
-                // For each actuator, limit the input position, calculate the position change, and select the absolute max
-                double dblDistanceToTarget_mm[3] = { -1.0 };
-                double maxDistanceToTarget_mm = 0.0;
-                for(short int i=0; i<3; i++) {
-                    short int channel = segNum*3+i;
-                    if(channel>=N_CHANNELS) {
-                        continue;
+        
+        wait(0.1);
+    }
+    
+    GoToNeutralPose(SAFE_SPEED_MMpS);
+    //reset front segment position
+    dblAlpha = 0.0;
+    dblBeta  = 0.0;
+    printf("\r\nManual control enabled\r\n");
+    isBusy = 0;
+}
+
+void ReadHHC(){
+    printf("Homing. Please wait...\r\n");
+
+
+    double startingPos[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
+    double startingVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS};
+    SetFrontDemand(startingPos, startingVel);
+    SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED);
+    wait(5);
+    printf("Done.\r\n");
+    threadSendFeedback.start(SendFeedback);
+    tickerSendFeedback.attach(&signalSendFeedback, 1/(float)SENSOR_FEEDBACK_HZ);
+    while(1){
+        semReadHHC.wait();  
+        
+        if(isZero){
+            GoToNeutralPose(SAFE_SPEED_MMpS);
+                //reset front segment position
+            dblAlpha = 0.0;
+            dblBeta  = 0.0;
+            isZero = 0;
+            printf("Done\r\n");
+        }
+        
+        if(isCycle){
+            GoToAnchorPose(SAFE_SPEED_MMpS);
+            isCycle = 0;
+            printf("Done\r\n");
+        }
+        if(isDebug){
+            DebugMode();
+        }
+
+        if(!isSteerLock){   
+            //read Joystick sensors
+            double dblJSX = pinJSX.read();
+            double dblJSY = pinJSY.read();
+            
+            dblJSX = DeadZone(dblJSX, 0.07, 0.85, 0.15);
+            dblJSY = DeadZone(dblJSY, 0.07, 0.85, 0.15);
+            
+            if(pinWashIn.read()){
+                pinWashOut = 0;
+            }else{
+                pinWashOut = 1;
+            }
+                       
+            //convert to angular velocity for front segment
+            
+            //Find changes in angle
+            dblDeltaAlpha = dblJSX * x_axis_sign * dblSensitivity * MAX_STEER_SPEED_RADpS / HHC_READ_FREQ_HZ;
+            dblDeltaBeta  = dblJSY*dblSensitivity*MAX_STEER_SPEED_RADpS/HHC_READ_FREQ_HZ;
+            //convert to speeds in [rad/s]
+            dblAlphaDot = dblDeltaAlpha*HHC_READ_FREQ_HZ;
+            dblBetaDot  = dblDeltaBeta*HHC_READ_FREQ_HZ;
+            
+            //calculate required velocity based on current possition and new inputs.
+            dblPhi = atan2(dblBeta,dblAlpha);
+            double dblPhiDot = dblAlpha * dblBetaDot - dblBeta * dblAlphaDot;
+            dblPhiDot /= (dblAlpha * dblAlpha + dblBeta*dblBeta);
+            if( fabs( cos(dblPhi) ) >= 0.7106781 ){
+                dblTheta = dblAlpha / cos(dblPhi);
+                double dblThetaDot = dblAlphaDot * cos(dblPhi) + dblAlpha * dblPhiDot * sin(dblPhi);
+                dblThetaDot /= cos(dblPhi)*cos(dblPhi);
+                double dblPsi[3];
+                for (int ii =0; ii<3; ii++){
+                    dblPsi[ii] = dblPhi - ii*2*PI/3;
+                    if(dblBeta == 0){
+                        dblXdot[ii] = ACT_CONV * K_theta*( cos(dblPsi[ii]) + d_theta);
+                        dblXdot[ii] *= ( dblAlphaDot - dblBetaDot * sin(dblPsi[ii]) ) / cos(dblPhi);
                     }
-                    double dblCurrentPosition_mm = llcomms.positionSensor_mm[channel];
-                    if(input.psi_mm[channel]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
-                        continue;
+                    else{
+                        dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot);
+                    }   
+                }               
+            } 
+            else {
+                dblTheta = dblBeta / sin(dblPhi);
+                double dblThetaDot = dblBetaDot * sin(dblPhi) + dblBeta * dblPhiDot * cos(dblPhi);
+                dblThetaDot /= sin(dblPhi)*sin(dblPhi);
+                double dblPsi[3];
+                for (int ii =0; ii<3; ii++){
+                    dblPsi[ii] = dblPhi - ii*2*PI/3;
+                    if(dblAlpha == 0){
+                        dblXdot[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta);
+                        dblXdot[ii] *= ( dblBetaDot - dblAlphaDot * sin(dblPsi[ii]) ) / sin(dblPhi);
                     }
-                    // Limit actuator position
-                    input.psi_mm[channel] = min( max( 0.0 , input.psi_mm[channel] ) , (double)MAX_ACTUATOR_LIMIT_MM );
-                    // Calculate actuator position change
-                    dblDistanceToTarget_mm[i] = fabs(input.psi_mm[channel] - dblCurrentPosition_mm);
-                    // Select the max absolute actuator position change
-                    if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
-                        maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
+                    else{
+                        dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta ) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot);
                     }
                 }
-                // For max actuator position change, calculate the time to destination at the limited speed
-                maxTimesToTarget_s[segNum] = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
-                // For each actuator, replan target position and velocity as required
-                for(short int i=0; i<3; i++) {
-                    short int channel = segNum*3+i;
-                    // If requested actuator position change is already within tolerance, do NOT replan that actuator
-                    if( dblDistanceToTarget_mm[i] <= 0.0 ) continue;
-                    // Calculate velocity for each motor to synchronise movements to complete in max time
-                    // Set dblDemandPosition_mm and dblDemandSpeed_mmps
-                    dblDemandPosition_mm[channel] = input.psi_mm[channel];
-                    dblDemandSpeed_mmps[channel] = dblDistanceToTarget_mm[i] / maxTimesToTarget_s[segNum];
+            }
+            //calculate new angles for next front segment position
+            dblAlpha += dblDeltaAlpha; // update new alpha postion
+            dblBeta += dblDeltaBeta; // update new beta position
+            
+            dblAlpha = limDouble(dblAlpha, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD);
+            dblBeta = limDouble(dblBeta, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD);
+            dblPhi = atan2(dblBeta,dblAlpha); // update new phi position
+            if( fabs( cos(dblPhi) ) >= 0.7106781 ){
+                dblTheta = dblAlpha / cos(dblPhi);//calculate new theta value
+            }
+            else { 
+                dblTheta = dblBeta / sin(dblPhi);
+            }
+            dblTheta = limDouble(dblTheta, 0.0, (double)MAX_THETA_RAD ); //limit angle
+            //update alpha and beta if limit has occured
+            dblAlpha = dblTheta *cos(dblPhi);
+            dblBeta = dblTheta *sin(dblPhi);
+            
+            double dblPsi[3];
+            for (int ii = 0; ii <3; ii++) {//calculate new acuator positions    
+                dblPsi[ii] = dblPhi - ii*2*PI/3;
+                dblX[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta) * dblTheta;
+                //add offset
+                dblX[ii] += ACTUATOR_OFFSET;
+                
+                //limit positions
+                dblX[ii] = limDouble( dblX[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM );
+            }
+            //calculate new drive segment position
+            //read drive segment buttons
+            //double dblDriveStep = 0.4;
+            
+            //printf("pin:%d\t", pinFwd.read());
+            
+            if(pinFwd.read()){
+                if(!pinRev.read()){
+                    dblSdot = SAFE_CONTRACT_SPEED;
+                } else{
+                    dblSdot = 0.0;
                 }
             } else {
-                maxTimesToTarget_s[segNum] = -1.0;
+                if(pinRev.read()){
+                    dblSdot = -1.0*SAFE_CONTRACT_SPEED;
+                } else{
+                    dblSdot = 0.0;
+                }
             }
+            
+            dblS += dblSdot/HHC_READ_FREQ_HZ;
+            //limit extension (uses word position, but actually refers to pressure)
+            dblS = limDouble( dblS, 0.0, (double)MAX_EXTENSION_PRESSURE );
+            
+//            printf("S:%f\tSdot:%f\t", dblS, dblSdot);
+            //Send signals to actuators.
+            SetFrontDemand(dblX, dblXdot);//front segment
+            SetDriveDemand(dblS, dblSdot);//drive segment
+
+
         }
-        // Unlock mutex, allowing setDemandsForLL to run again
-        mutPathIn.unlock();
-        
-        // SEND MESSAGE
-        hlcomms.send_durations_message(maxTimesToTarget_s);
     }
-
-}
-
-void startLLcomms() { // Send new demands to LL after receiving new target data
-    semLLcomms.release(); // Uses threadSetDemands which is normal priority
 }
 
-void setDemandsForLL() {
-    
-    while(1) {
-        semLLcomms.wait();
-        mutPathIn.lock(); // Lock relevant mutex
-        for(short int i=0; i<N_CHANNELS; i++) { // For each LL
-            llcomms.mutChannel[i].lock(); // MUTEX LOCK
-            llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
-            llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
-            llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
-            llcomms.isDataReady[i] = 1; // Signal that data ready
-        } // end for
-        mutPathIn.unlock(); // Unlock relevant mutex
-    } // end while(1)
-
+void SetUp(){
+    for(int ii = 0; ii<N_CHANNELS; ii++){
+        pinPosOut[ii].write(0.0);
+        pinVelOut[ii].write(0.0);
+    }
 }
 
 int main() {
+    pinThirdPump = 1;
+    pinJetOut = 1;
+    pinWashOut = 1;
     pc.baud(BAUD_RATE);
-    //printf("ML engage. Compiled at %s\r\n.",__TIME__);
-    wait(3);
+    for(int ii = 0; ii<N_CHANNELS; ii++){
+        pinPosOut[ii].period_ms(2.0);
+        pinVelOut[ii].period_ms(2.0);
+    }
+    wait(1.0);
+    printf("ML engage. Compiled at %s\r\n",__TIME__);
+    
+
+    //set up support function interrupts
+    //Rising
     
-    threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
-    threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
-    threadSetDemands.start(setDemandsForLL); // Start planning thread
-    threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
+    pinZeroIn.mode(PullNone);
+    pinCycleIn.mode(PullNone);
+    pinWashIn.mode(PullNone);
+    pinGasIn.mode(PullNone);
+    pinSucIn.mode(PullNone);
+    pinFwd.mode(PullNone);
+    pinRev.mode(PullNone);
+    pinFlushIn.mode(PullDown);
+    pinDebugIn.mode(PullDown);
+    
+    pinZeroIn.rise(&ZeroActivate);
+    pinCycleIn.rise(&CycleActivate);
+    pinFlushIn.rise(&FlushActivate);
+    pinSucIn.rise(&SucActivate);
+    pinGasIn.rise(&GasActivate);
+    pinDebugIn.rise(&DebugActivate);
+    //falling
+    pinZeroIn.fall(&ZeroStop);
+    pinCycleIn.fall(&CycleStop);
+    pinFlushIn.fall(&FlushStop);
+    pinSucIn.fall(&SucStop);
+    pinGasIn.fall(&GasStop);
+    pinDebugIn.fall(&DebugStop);
+    threadReadHHC.start(ReadHHC); 
 
-    setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
     
-    Thread::wait(1);
+    tickerReadHHC.attach(&signalReadHHC, 1/(float)HHC_READ_FREQ_HZ); // set up  
+    
+
     while(1) {
         Thread::wait(osWaitForever); 
     }