added dual filter compatibility

4DrobotServer_config.h

+#ifndef __4DrobotServer_config_h__
+#define __4DrobotServer_config_h__
+
+/* 
+ * This file is part of 4DrobotServer.
+ * 4DrobotServer is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ * 4DrobotServer is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ * You should have received a copy of the GNU General Public License along with 4DrobotServer. If not, see <https://www.gnu.org/licenses/>. 
+ */
+  
+//Note: This isn't a proper header file, as it contains implementations (variable = value).
+//      It can therefore only be included in one compilation unit.
+//      This is OK as long as we stay within Arduino studio and only use .ino files for code,
+//      which are concatonated in alphabetical order.
+
+//Note on Arduino pins used for Ethernet shield,
+// AVOID THESE PINS:
+// 4/10/11/12/13 (UNO)
+// 50/51/52/10/4; 53 unused but it MUST be an output (Mega)
+
+// ***** NETWORK CONFIG ************************
+
+// MAC address for the Arduino on the plasma lens
+//byte mac[] = {0xA8, 0x61, 0x0A, 0xAE, 0x84, 0xED};
+
+// MAC address for the Arduino on the CLIC structure
+//byte mac[] = {0xA8, 0x61, 0x0A, 0xAE, 0x84, 0xEE};
+
+// MAC address for Kyrre's test board
+//byte mac[] = {0xA8, 0x61, 0x0A, 0xAE, 0x72, 0xF7};
+
+// MAC address for the Arduino on the 4DRobot (Office Network)
+//byte mac[] = {0xA8, 0x61, 0x0A, 0xAE, 0x84, 0xEF};
+
+// MAC address for the Arduino on the 4DRobot (Technical Network, CORRECT one)
+byte mac[] = {0xA8, 0x61, 0x0A, 0xAE, 0x71, 0xAC};
+
+
+//To use DHCP or not to use DHCP, that's the question.
+// Comment out to use static IP configuration
+#define USE_DHCP
+
+#ifndef USE_DHCP
+IPAddress ip(172, 26, 91, 239);
+IPAddress myDns(137, 138, 16, 5);
+IPAddress gateway(172, 26, 91, 1);
+IPAddress subnet(255, 255, 255, 0);
+#endif
+
+const int    telnet_port = 23;
+
+//Number of connection 'slots'
+const size_t telnet_numConnections = MAX_SOCK_NUM;
+
+//This quickly takes a LOT of memory!
+//The total for MEGA is 8k, and we also need some stack space!
+//Note: It's possible to reduce the number of connection slots.
+const size_t telnet_bufflen = 256;
+const size_t output_bufflen = 2048;
+
+// ***** LOCAL CONTROL CONFIG ******************
+const int emergency_stop_pin = 9;
+const bool emergency_isEmergency = HIGH;
+
+// ***** TEMPERATURE SENSOR CONFIG *************
+
+const size_t  temp_numSensorPins         = 2;
+const uint8_t temp_oneWire_pins[]        = {48,49};
+const size_t  temp_numSensors_perpin[]   = {1,1};
+const size_t  temp_numSensors            = 2;
+const unsigned long temp_update_interval = 1000; //[ms]
+
+const uint8_t temp_numRetries = 5;
+
+// ***** SAMPLE GRABBER SERVO CONFIG ***********
+
+// TODO: Tune stepWait, positions
+// Note: Adafruit_TiCoServo requires pin 9/10 (Uno) or 2/3/5/6/7/8/11/12/13/44/45/46 (Mega) as it needs hardware PWM capability.
+const int grabber_servo_pin    =   5;
+const int grabber_closed       =  70; //[deg]
+const int grabber_open         = 130; //[deg]
+const int grabber_min          =   0; //[deg]
+const int grabber_max          = 180; //[deg]
+const int grabber_stepWait     =  50; //[ms]
+
+// ***** CAMERA FILTER SERVO CONFIG ************
+// Yeah, a struct would be cleaner, then we could shared the code...
+
+const int filter1_servo_pin =   8;
+const int filter1_in        =   5; //[deg]
+const int filter1_out       = 165; //[deg]
+const int filter1_min       =   0; //[deg]
+const int filter1_max       = 180; //[deg]
+const int filter1_stepWait  =  10; //[ms]
+const uint16_t filter1_minpulsewidth  =  544; //[us], default=544
+const uint16_t filter1_maxpulsewidth  =  2400; //[us], default=2400
+
+const int filter2_servo_pin =   ??;
+const int filter2_in        = 165; //[deg]
+const int filter2_out       = 	5; //[deg]
+const int filter2_min       =   0; //[deg]
+const int filter2_max       = 180; //[deg]
+const int filter2_stepWait  =  10; //[ms]
+const uint16_t filter2_minpulsewidth  =  544; //[us], default=544
+const uint16_t filter2_maxpulsewidth  =  2400; //[us], default=2400
+
+// ***** STEPPER CONFIGS ***********************
+
+const size_t stepper_numAxis                 = 3;
+
+//Array indices of the 3 axis
+#define STEPPER_X 1
+#define STEPPER_Y 0
+#define STEPPER_Z 2
+
+const char    stepper_axnames[]              = {'Y','X','Z'};
+
+const uint8_t stepper_dir_pin[]              = {2,6,11}; //Stepper direction pins (HIGH=positive, LOW=negative)
+const uint8_t stepper_step_pin[]             = {3,7,12}; //Stepper pulse-to-step pins (pulse HIGH, otherwise low)
+
+//Define level for stepper_dir_pin[] to count steps in forward direction
+// Used to define the Z axis as zero when on top, so that we can zero it first on top switch,
+// then leave it on the limit switch while zeroing the others (safe).
+const bool    stepper_forward[]              = {LOW,HIGH,HIGH};
+const bool    stepper_backward[]             = {HIGH,LOW,LOW};
+
+const uint8_t stepper_switchMAX_pin[]         = {33,27,43}; //positive steps limit switch pins
+const uint8_t stepper_switchMIN_pin[]         = {32,26,42}; //negative steps limit switch pins
+//TODO: Verify active/inactive -> HIGH/LOW
+//      Note: Needs pulldown/pullup resistori
+//      Note: Probably safest to define on-limit = open, so if a switch fails the axis is safe (but stopped)
+const bool    stepper_switch_active          = LOW;    //On limit switch when pin active
+const bool    stepper_switch_inactive        = HIGH;   //On limit switch when pin inactive
+
+//If tuning timings, make sure to enable ACCELERATION_DEBUG and check that nothing overflows etc.
+// Note that code execution time of the normal move is about 29us, and 19us for zeroseek (measured with a scope).
+// This is effectively added to the stepper_delay.
+const unsigned int  stepper_onTime           = 25;    // [us] How long to hold the step pin HIGH
+const unsigned int  stepper_delay_fast       = 1000;  // [us] How long to wait between steps at flat-top when moving fast
+                                                      //      Note: stepper_onTime will be subtracted from this
+                                                      //            when setting the waiting from falling to rising edge.
+                                                      //            Computation time is not taken into account.
+const unsigned int  stepper_delay_slow       = 10000; // [us] How long to wait between steps at flat-top when moving slow
+                                                      //      Note: It will just truncate the normal accel profile
+                                                      //            to the longest delay above this value.
+                                                      //            If this value is longer than the longest in the accel profile,
+                                                      //            there will be no acceleration, it will start directly at this speed.
+const size_t        stepper_accelerate_steps = 300;   // When accelerating, how many steps to use for ramp to fast speed
+
+// When at high speed, ramp down to slow speed when
+// pos < accelerate_steps+slowzone or pos > (rail_length-accelerate_steps-slowzone)
+// and when pos approaches the target position
+//TODO: implement
+const unsigned long int  stepper_rail_length[]    = {7229,7798,2802}; // [steps] Total length of the rails
+const unsigned long int  stepper_slowzone_steps   = 50;               // [steps] Take it easy when approaching limit switches
+
+const unsigned long int stepper_maxsteps_move[]   = {7500,8100,3100}; // [steps] Maximum allowed displacement from current zero position (abs mode or not).
+                                                                      // This should limit the damage in case of limit switch failure.
+                                                                      // TODO: In abs mode, we probaby should not allow commands to go to negatice position...
+
+//Interlocks
+const long int           stepper_Zpos_unlockXY         = 1400;   // [steps] Only allow XY movements when
+                                                                 //          Zpos < Zpos_unlockXY and Z axis is in absolute mode
+const unsigned long int  stepper_onlimit_steps_towards = 2;      // [steps] Number of steps to allow when a limit switch is active
+                                                                 //         (moving towards the switch, i.e. allow
+                                                                 //          this many hits before stopping in case of spurious hits)
+                                                                 //         Note: For ZEROSEEK, this limit is always 0,
+                                                                 //               however there is a restart/retry if it is not stable.
+const unsigned long int  stepper_onlimit_steps_away    = 100;    // [steps] Number of steps to allow when a limit switch is active
+                                                                 //         (moving away from the switch, i.e. allow
+                                                                 //          this many hits before stopping in order to
+                                                                 //          allow escape from switch)
+
+//When zeroseeking, confirm the limit switch a few times before calling it good and going in absolute mode.
+const int stepper_zeroseek_confirmHitCounts = 5;
+const unsigned int stepper_zeroseek_confirmHitDelay = 1000; //[us]
+
+// ***** INTERNAL CONFIGS FOR DEV ***************
+
+// Uncomment to include extra debug messages about parser (sent to Serial only)
+//#define PARSER_DEBUG
+
+// Uncomment to include extra debug messages about sensors (sent to Serial only)
+//#define SENSOR_DEBUG
+
+// Uncomment to include extra debug messages about acceleration (sent to Serial only)
+//#define ACCELERATION_DEBUG
+
+// Uncomment to use standard Arduino Servo library instead of TiCoServo from Adafruit.
+// This will cause the grabber to twich, but will work on any pin and any Arduino
+//#define USE_STDSERVOLIB
+
+// ***** HARDWARE SIMULATION (FOR DEV) *********
+
+//Uncomment to simulate hardware
+//#define DUMMY_TEMP
+//#define DUMMY_SERVO
+//#define DUMMY_STEPPER
+
+
+#endif

4DrobotServer_globalVars.h

+#ifndef __4DrobotServer_globalVars_h__
+#define __4DrobotServer_globalVars_h__
+
+/* 
+ * This file is part of 4DrobotServer.
+ * 4DrobotServer is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ * 4DrobotServer is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ * You should have received a copy of the GNU General Public License along with 4DrobotServer. If not, see <https://www.gnu.org/licenses/>. 
+ */
+ 
+//Note: This isn't a proper header file, as it contains implementations (variable = value).
+//      It can therefore only be included in one compilation unit.
+//      This is OK as long as we stay within Arduino studio and only use .ino files for code,
+//      which are concatonated in alphabetical order.
+
+// Sockets for TCP communications
+EthernetServer telnet_socket(telnet_port);
+EthernetClient telnet_connection[telnet_numConnections];
+
+// ASCII TTY IO Buffers
+// buffCount is the number of elements = idx of first free element
+// buffCount_prev is buffCount in the start of the previous parsing round.
+//  It is used to print a "$" when done; if now == 0 and prev !=0
+//  then this cycle cleared the data from the previous round and are ready.
+char           telnet_buff[telnet_numConnections][telnet_bufflen];
+size_t         telnet_buffCount[telnet_numConnections];
+size_t         telnet_buffCount_prev[telnet_numConnections];
+
+char           serial_buff[telnet_bufflen];
+size_t         serial_buffCount;
+size_t         serial_buffCount_prev;
+
+char           output_buff[output_bufflen];
+size_t         output_buffCount;
+
+// Temperature sensor access & buffers
+
+OneWire            temp_oneWire[temp_numSensorPins];
+DallasTemperature  temp_sensors[temp_numSensorPins];
+float              temp_data[temp_numSensors]; //[degC]
+unsigned long      temp_update_prev = 0;       //[ms]
+
+// Sample grabber servo
+#ifdef USE_STDSERVOLIB
+Servo grabber_servo;
+#else
+Adafruit_TiCoServo grabber_servo;
+#endif
+bool  grabber_go   = false;
+int   grabber_goto = 0;
+int   grabber_pos  = grabber_closed; //Assumed initial position
+
+// Camera filter servos
+#ifdef USE_STDSERVOLIB
+Servo filter1_servo;
+Servo filter2_servo;
+#else
+Adafruit_TiCoServo filter1_servo;
+Adafruit_TiCoServo filter2_servo;
+#endif
+bool  filter1_go   = false;
+bool  filter2_go   = false;
+int   filter1_goto = 0;
+int   filter2_goto = 0;
+int   filter1_pos  = filter1_out; //Assumed initial position
+int   filter2_pos  = filter2_out; //Assumed initial position
+// Stepper motors
+
+bool     stepper_go              = false;               //Move a stepper on the next loop?
+bool     stepper_go_zero         = false;               //Move-to-position   (false) or
+                                                        // move-to-switchLOW (true)?
+size_t   stepper_go_axis         = 0;                   //Which stepper to move?
+long int stepper_goto            = 0;                   //[steps] Target position
+                                                        // (used if stepper_go_zero==false)
+bool     stepper_interlock       = true;                //Is the stepper interlock enabled (safe)
+                                                        //                   or disabled (unsafe)?
+long int stepper_pos[]           = {0,0,0};             //Current stepper position
+bool     stepper_mode_absolute[] = {false,false,false}; //Do we have a valid zeroing?
+                                                        // If true, pos represents the absolute
+                                                        // position of that axis
+
+const unsigned int stepper_maxwait_us = 16383;                       // [us] Maximum value allowed by delayMicroseconds
+unsigned int       stepper_accel_profile[stepper_accelerate_steps];  // [us] Stepper acceleration profile
+size_t             stepper_accelerate_steps_slow;                    // Truncate the accel_profile after this many steps
+                                                                     //      when moving slowly
+#endif

ServoControl.ino

+/* 
+ * This file is part of 4DrobotServer.
+ * 4DrobotServer is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+ * 4DrobotServer is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ * You should have received a copy of the GNU General Public License along with 4DrobotServer. If not, see <https://www.gnu.org/licenses/>. 
+ */
+
+void setup_servo() {
+  //Sanity check grabber config
+  if (not (grabber_open >= grabber_min && grabber_open <= grabber_max)) {
+    Serial.print(F("Grabber config error; grabber_open outside of legal range\n"));
+    while(true) {
+      delay(1);
+    }
+  }
+  if (not (grabber_closed >= grabber_min && grabber_closed <= grabber_max)) {
+    Serial.print(F("Grabber config error; grabber_closed outside of legal range\n"));
+    while(true) {
+      delay(1);
+    }
+  }
+
+  //Sanity check filter config
+  if (not (filter1_in >= filter1_min && filter1_in <= filter1_max)) {
+    Serial.print(F("Filter 1 config error; filter1_in outside of legal range\n"));
+    while(true) {
+      delay(1);
+    }
+  }
+  if (not (filter1_out  >= filter1_min && filter1_out <= filter1_max)) {
+    Serial.print(F("Filter 1 config error; filter1_out outside of legal range\n"));
+    while(true) {
+      delay(1);
+    }
+  }
+  
+  if (not (filter2_in >= filter2_min && filter2_in <= filter2_max)) {
+    Serial.print(F("Filter 2 config error; filter2_in outside of legal range\n"));
+    while(true) {
+      delay(1);
+    }
+  }
+  if (not (filter2_out  >= filter2_min && filter2_out <= filter2_max)) {
+    Serial.print(F("Filter 2 config error; filter2_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);
+
+  filter1_servo.attach(filter1_servo_pin, filter1_minpulsewidth, filter1_maxpulsewidth);
+  filter1_servo.write(filter1_pos);
+  filter2_servo.attach(filter2_servo_pin, filter2_minpulsewidth, filter2_maxpulsewidth);
+  filter2_servo.write(filter2_pos);
+  #endif
+}
+
+//Program for controlling the servomotors for the grabber and camera filters
+void servo_control() {
+  if (not (grabber_go || filter1_go || filter2_go)) {
+    return;
+  }
+
+  //Move!
+
+  if (grabber_go) {
+    grabber_control();
+  }
+
+  if (filter1_go) {
+    filter_control("Filter 1");
+  }
+  
+  if (filter2_go) {
+    filter_control("Filter 2");
+  }
+}
+
+void grabber_control() {
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER MOVING...\n"));
+  output_buff_flush();
+  
+  while (grabber_pos != grabber_goto) {
+    if (digitalRead(emergency_stop_pin) == emergency_isEmergency) {
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER ERROR EMERGENCY STOP\n"));
+      break;
+    }
+    if (grabber_pos < grabber_goto) {
+      grabber_pos++;
+    }
+    else {
+      grabber_pos--;
+    }
+
+    #ifndef DUMMY_SERVO
+    grabber_servo.write(grabber_pos);
+    #else
+    Serial.print("Grabber to: ");
+    Serial.print(grabber_pos);
+    Serial.print('\n');
+    #endif
+
+    delay(grabber_stepWait);
+  }
+ 
+  grabber_go   = false;
+  stepper_goto = 0;
+
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< GRABBER GO FINISHED POS = "));
+  char buff[5];
+  snprintf(buff,sizeof(buff), "%03d\n", grabber_pos);
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
+
+  output_buff_flush();
+}
+
+void filter_control(filter) {
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER MOVING...\n"));
+  output_buff_flush();
+  if (filter=="Filter 1"){
+  	filter_pos=filter1_pos
+  	filter_goto=filter1_goto
+  }
+  
+   if (filter=="Filter 2"){
+  	filter_pos=filter2_pos
+  	filter_goto=filter2_goto
+  }
+  
+  while (filter_pos != filter_goto) {
+    if (digitalRead(emergency_stop_pin) == emergency_isEmergency) {
+      bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER ERROR EMERGENCY STOP\n"));
+      break;
+    }
+    if (filter_pos < filter_goto) {
+      filter_pos++;
+    }
+    else {
+      filter_pos--;
+    }
+
+    #ifndef DUMMY_SERVO
+    filter_servo.write(filter_pos);
+    #else
+    Serial.print(filter + " to: ");
+    Serial.print(filter_pos);
+    Serial.print('\n');
+    #endif
+
+    delay(filter_stepWait);
+  }
+ 
+  filter_go   = false;
+  filter_goto = 0;
+  
+  if (filter=="Filter 1"){
+  	filter1_go=filter_go
+  	filter1_goto=filter_goto
+  }
+  
+  if (filter=="Filter 2"){
+  	filter2_go=filter_go
+  	filter2_goto=filter_goto
+  }
+
+
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, F("< FILTER GO FINISHED POS = "));
+  char buff[5];
+  snprintf(buff,sizeof(buff), "%03d\n", filter_pos);
+  bufferWrite(output_buff, sizeof(output_buff), output_buffCount, buff);
+
+  output_buff_flush();
+}