Diff: main.cpp

Revision:

33:9877ca32e43c

Parent:

32:8c59c536a2a6

Child:

34:54e9ebe9e87f

--- a/main.cpp	Fri Mar 08 12:44:29 2019 +0000
+++ b/main.cpp	Wed Mar 13 11:59:58 2019 +0000
@@ -45,10 +45,20 @@
     SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
     
     struct demands_struct input;
+    DigitalOut SupportPins[4] = {PE_4, PE_2, PE_3, PE_6};
     
     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]));
+            }
+        }
         // PROCESS INPUT
         double maxTimesToTarget_s[3] = { -1.0 };
         //[8,7,6,4,3,-1,-1,0,-1]