small adjustments

4DrobotServer/4DrobotServer_config.h

@@ -87,16 +87,15 @@ const int grabber_stepWait     =  50; //[ms]
 // Yeah, a struct would be cleaner, then we could shared the code...
 // {Filter1, filter2}
 
-struct Filter {
-  dconst int filter_servo_pin[] =   {8,??}; 
-  const int filter_in[]        =   {5,165}; //[deg]
-  const int filter_out[]       = {165,5}; //[deg]
-  const int filter_min       =   0; //[deg]
-  const int filter_max       = 180; //[deg]
-  const int filter_stepWait  =  10; //[ms]
-  const uint16_t filter_minpulsewidth  =  544; //[us], default=544
-  const uint16_t filter_maxpulsewidth  =  2400; //[us], default=2400
-};
+const size_t filter_numfilters = 2;
+const int filter_servo_pin[] =   {8,??}; 
+const int filter_in[]        =   {5,165}; //[deg]
+const int filter_out[]       = {165,5}; //[deg]
+const int filter_min       =   0; //[deg]
+const int filter_max       = 180; //[deg]
+const int filter_stepWait  =  10; //[ms]
+const uint16_t filter_minpulsewidth  =  544; //[us], default=544
+const uint16_t filter_maxpulsewidth  =  2400; //[us], default=2400
 
 // ***** STEPPER CONFIGS ***********************
 

4DrobotServer/4DrobotServer_globalVars.h

@@ -52,15 +52,11 @@ int   grabber_pos  = grabber_closed; //Assumed initial position
 
 // Camera filter servo
 #ifdef USE_STDSERVOLIB
-Servo filter_servo[];
+Servo filter_servo[filter_numfilters];
 #else
-Adafruit_TiCoServo filter_servo[];
+Adafruit_TiCoServo filter_servo[filter_numfilters];
 #endif
 
-Servo filter2_servo;
-#else
-Adafruit_TiCoServo filter2_servo;
-#endif
 
  //{FILTER1, FILTER2}
 bool  filter_go[]   = {false, false};

4DrobotServer/ServoControl.ino

@@ -21,30 +21,31 @@ void setup_servo() {
   }
 
   //Sanity check filter config
-  if (not ((filter_in[0] >= filter_min && filter_in[0] <= filter_max) || (filter_in[1] >= filter_min && filter_in[1] <= filter_max)) ) {
-    Serial.print(F("Filter config error; filter_in outside of legal range\n"));
-    while(true) {
-      delay(1);
+  for (int i=0; i< filter_numfilters; i++){
+      if (not ((filter_in[i] >= filter_min && filter_in[i] <= filter_max))) {
+        Serial.print(F("Filter config error; filter_in outside of legal range\n"));
+        while(true) {
+          delay(1);
+        }
+      }
+      if (not ((filter_out[i]  >= filter_min && filter_out[i] <= filter_max))) {
+        Serial.print(F("Filter config error; filter_out outside of legal range\n"));
+        while(true) {
+          delay(1);
+        }
+      }
     }
-  }
-  if (not ((filter_out[0]  >= filter_min && filter_out[0] <= filter_max) || (filter_out[1]  >= filter_min && filter_out[1] <= filter_max))) {
-    Serial.print(F("Filter config error; filter_out outside of legal range\n"));
-    while(true) {
-      delay(1);
-    }
-  }
-
-
 
   //Initialize Servo library
   #ifndef DUMMY_SERVO
   grabber_servo.attach(grabber_servo_pin);
   grabber_servo.write(grabber_pos);
 
-  filter_servo[0].attach(filter_servo_pin[0], filter_minpulsewidth, filter_maxpulsewidth);
-  filter_servo[1].attach(filter_servo_pin[1], filter_minpulsewidth, filter_maxpulsewidth);
-  filter_servo[0].write(filter_pos[0]);
-  filter_servo[1].write(filter_pos[1]);
+  for (int i=0; i< filter_numfilters; i++){
+
+      filter_servo[i].attach(filter_servo_pin[i], filter_minpulsewidth, filter_maxpulsewidth);
+      filter_servo[i].write(filter_pos[i]);      
+  }
   #endif
 }
 
@@ -60,13 +61,11 @@ void servo_control() {
     grabber_control();
   }
 
-  if (filter_go[0]) {
-    filter_control(0);
-  }
-
-  if (filter_go[1]) {
-   filter_control(1);
- }
+  for (int i=0; i< filter_numfilters; i++){
+    if (filter_go[i]) {
+      filter_control(i);
+    }    
+  }  
 }
 
 void grabber_control() {
@@ -78,6 +77,7 @@ void grabber_control() {
       bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER ERROR EMERGENCY STOP\n"));
       break;
     }
+    
     if (grabber_pos < grabber_goto) {
       grabber_pos++;
     }