Add files via upload

Upgraded the servo motor control. Now you can rotate an MG90s servo motor 180 degrees.

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/SDstartup.ino

@@ -0,0 +1,24 @@
+void SDstartup(){ 
+  // This program checks if the card is present and can be initialized:
+  if (!SD.begin(17)) {
+      digitalWrite(statusLED, HIGH); //The blue LED turns on if the card cannot be initialized
+    while(1);
+  }
+
+  char filename[] = "00.CSV"; //File name
+  for (uint8_t i = 0; i < 100; i++) { //The SD card can store up to 100 files
+    filename[0] = i/10 + '0';
+    filename[1] = i%10 + '0';
+    if (! SD.exists(filename)) {
+      dataFile = SD.open(filename, O_CREAT | O_WRITE); //Only open a new file if it doesn't exist
+      break;
+    }
+    else if (SD.exists(F("99.CSV"))){
+      while(1){
+        digitalWrite(statusLED, HIGH); //If there are 100 files, the blue LED turns on
+      }
+    }
+  }
+  dataFile.println(F("Time (ms), Altitude (m), Filtered altitude (m), Acceleration (g), Perpendicular acceleration (g), Temperature (C)")); //File header
+  dataFile.flush(); //Writes data to the SD card
+}

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/batteryStatus.ino

@@ -0,0 +1,8 @@
+void batteryStatus(){
+  if ((2*analogRead(29)*3.3/(pow(2,12)-1)) < 3.8){
+    digitalWrite(batLED, HIGH);
+  }
+  else if ((2*analogRead(29)*3.3/(pow(2,12)-1)) > 3.8){
+    digitalWrite(batLED, LOW);
+  }
+}

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/beepnblink.ino

@@ -0,0 +1,76 @@
+void beepnblink(){
+  if (initVar == true && overtime == false){
+
+    if (p<30){
+      digitalWrite(statusLED, HIGH);
+      if (piezoEnable == true){
+        analogWrite(piezo, 50); //Turn the piezo on for 300ms
+      }
+    }
+  
+    else if (p == 30){
+      digitalWrite(statusLED, LOW);
+      if (piezoEnable == true){
+        analogWrite(piezo, 0); //Turn the piezo off
+      }
+    }
+
+    else if (p == 400){
+      p = 0;
+    }
+  p++;
+  }
+
+  else if (initVar == false && overtime == false){ //This is necesary because if p<30 after launch is detected the LED and the piezo will be stuck in ON mode.
+    digitalWrite(statusLED, LOW);
+    if (piezoEnable == false){
+      analogWrite(piezo, 0);
+    }
+  }
+
+  else if (initVar == false && overtime == true){ //After timeout blink and beep the maximum altitude in meters
+    altToDigits(); //Convert the maximum altitude to digits
+    while(1){
+      for (int i=3; i>-1; i--){
+        blinknbeep(altMaxDig[i]); //Beep and blink the altitude digits
+        delay(700); //0.5s pause in between the beeps and blinks
+        if (i == 0){
+          sleep_ms(10000); //Sleep for 10 seconds to save battery
+        }
+      }
+    }
+  }
+}
+
+void altToDigits(){ //Convert altitude to digits
+  for (int i=0; i<4; i++){
+    rmnd = altMax % 10; //This is the remainder
+    altMaxDig[i] = rmnd;
+    altMax = altMax / 10;
+  }
+}
+
+void blinkLED(int n){ //Blinks the blue LED every 200 ms
+    for (int i=0; i<=n; i++){
+      digitalWrite(statusLED, HIGH);
+      delay(200);
+      digitalWrite(statusLED, LOW);
+      delay(200);
+    }
+}
+
+void blinknbeep(int n){ //Blinks the blue LED every 300 ms and makes the buzzer beep
+    for (int i=0; i<n; i++){
+      digitalWrite(statusLED, HIGH);
+      if (piezoEnable == true){
+        analogWrite(piezo, 50);
+      }
+      delay(250);
+      
+      digitalWrite(statusLED, LOW);
+      if (piezoEnable == true){
+        analogWrite(piezo, 0);
+      }
+      delay(250);
+    }
+}

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/control.ino

@@ -0,0 +1,258 @@
+// Firmware version 2.1. Release date: 15.03.2024 //This version of the firmware features a complementary-coded rotary switch.
+
+// In no respect shall DanInvents be accountable for any liabilities, claims, demands, damages or suits resulting from the use of
+// the flight controller and/or this software. By using this software, you assume all risks associated with this product and
+// its associated features. While the circuitry and software have been tested, they should be considered experimental and handled
+// with caution.
+
+// Before uploading this code make sure that you have downloaded the latest ADXL343 (Adafruit) and MS5637 (Sparkfun) libraries.
+// You will also need the Circular Buffer library by Roberto Lo Giacco.
+// Thanks to Adafruit, Sparkfun and Roberto for the open source libraries and also to Homemade Multibody Dynamics for a guide into how to log data fast.
+// Thanks to MartinMcC for showing how to use a rotary encoder with a microcontroller.
+// Special thanks to Barun Basnet for the exceptional work on Kalman filters.
+// Special thanks to Earle Philhower for providing the support that allows using the Arduino libraries and IDE with the RP2040.
+
+//Firmware improvements of version 2.2 over version 2.1:
+
+// Created two variables zeroPos, and extendedPos that allow the user to adjust the zero and extended positions for the servo motor. The value of these integers in microseconds has been tested
+// with a MG90S servo motor.
+
+//Firmware improvements of version 2.1 over version 2:
+
+// Fixed a bug that made the piezo buzzer and the blue LED stay on once launch was detected.
+// Selecting the position 3 of the rotary switch allows you to test the servo motors. Servo motor 1 moves from its initial to its final position followed by servo 2 3 seconds later.
+// By using position 3 to test the servos you avoid creating unnecessary files in your microSD card.
+// Wait for 10 seconds to allow the rocketeer to prepare for launch before the flight computer is armed this avoids creating unnecesary files in your microSD card.
+
+
+// Firmware improvements of version 2 over version 1.2:
+
+// Changed the sign of the longitudinal acceleration. Now positive acceleration is pointing downwards and negative upwards.
+// Changed the way that launch is detected. Now the altitude must be greater than 10 m and the acceleration higher than 2 gs for over 100 ms.
+// Modified the Kalman filter parameters. Now the filtered data closely follows the measured values but featuring lower noise. This guarantees accurate apogee detection.
+// Modified the frequency at which the flight computer beeps, now it beeps less frequently before launch.
+// Now the flight computer goes silent once launch is detected. After 5 minutes, the flight computer beeps and flashes the altitude.
+// For example, 5 beeps/flashes followed by 7 beeps/flashes means 57 meters.
+// Now the flight computer can rotate a servo 180 degrees (not yet tested).
+
+#include <Wire.h>
+#include "SparkFun_MS5637_Arduino_Library.h"
+#include <Adafruit_Sensor.h>
+#include <Adafruit_ADXL343.h>
+#include <SPI.h>
+#include <SD.h>
+#include <Servo.h>
+#include <EEPROM.h>
+#include <CircularBuffer.h>
+#include "pico/stdlib.h"
+
+CircularBuffer <float,100> FilteredAltitudes;
+CircularBuffer <float,100> altitudes;
+CircularBuffer <float,100> accelerations;
+CircularBuffer <long,100> times;
+
+//Initialization of Kalman Variables
+float R = 0.3; //R = measurement noise covariance. Larger R means large measurement uncertainty
+float Q = 0.3*1e-2;  //Q = process noise covariance. Larger Q means larger estimation uncertainty. Thus increasing Q corrects more
+double Xpe0;  // Xpe0 = prior estimation of signal X at time t=0 (current state)
+double Xe1;  //Xe1 = estimation of X at time t=1 (previous state)
+double Ppe0;  //Ppe0 = prior estimation of "error covariance" at t=0
+double P1,P0; //P1 = error covariance at t=1, P0 = error covariance at t=0
+double K, Xe0, Z; //K = Kalman gain, Xe0 = estimation of signal at t=0, Z = measured signal at t=0
+
+//Physical magnitudes
+float altold; //Baseline pressure
+int altMax; //Rounded maximum altitude
+int altMaxDig[4] = {}; //Max altitude digits
+int rmnd; //Dummy variable remainder
+int dvsr; //Dummy variable for beeping/flasing the altitude
+float temp;
+float currentPressure;
+float altitudeDelta;
+float altThreshold = 10; //Altitude threshold for launchd detection in meters
+float accelThreshold = 2.0; //Acceleration threshold for launch detection in gs.
+float filteredAltitudeDelta;
+float rocketAccel;
+float startingPressure = 0.0;
+
+//Definition of time and auxiliary integers
+int tconfig, n, q, p = 0, r = 0;
+int deltat; //Time step of every loop iteration
+long int t1; //Time variables
+long int t4, tout = 300000; //Here tout is the timeout variable tout = 300000 equals 5 min of data logging time
+
+/* Assign a unique ID to this sensor at the same time */
+Adafruit_ADXL343 accel = Adafruit_ADXL343(12345, &Wire1);
+
+char filename[] = "00.CSV"; //Dummy file name to store flight data.
+
+//Config. rotary switch. This configuration is for the real-coded rotary switch
+byte switchPins[4] = {15, 13, 14, 16}; //Digital pins assigned to the rotary switch
+byte rotValue = B0000; // Variable for printing value over serial debug
+byte switchPos; // Variable for storing the current switch possition
+byte previousValue; //Variable for storing the previous switch possition
+
+//Boolean variables defining the state of the program
+bool initVar = true;
+bool launchCondition1 = false;
+bool launchCondition2 = false;
+bool deploy = false;
+bool automatic = false;
+bool timer = false;
+bool overtime = false;
+bool piezoEnable = true;
+
+//LEDs
+int batLED = 2; //Battery indicator LED
+int statusLED = 26; //Status LED
+
+//Servos
+int servo1pin = 28;
+int servo2pin = 27;
+int zeroPos = 540; //These are the pulse widths for an MG90S servo motor
+int extendedPos = 2580;
+
+//Piezo
+int piezo = 12;
+
+MS5637 barometricSensor; //Creates a barometricSensor object
+File dataFile; //Creates a dataFile object
+Servo servo1; //Creates a servo1 object
+Servo servo2; //Creates a servo2 object
+
+void setup() {
+  //Serial.begin(9600); //For debugging purposes only
+  EEPROM.begin(512); //Emulates EEPROM by allocating 512 kB from the flash memory
+
+  //Declaration of the I2C pins
+  Wire1.setSDA(10);
+  Wire1.setSCL(11);
+
+  //Declaration of the SPI pins
+  SPI.setRX(20);
+  SPI.setTX(19);
+  SPI.setSCK(18);
+  SPI.setCS(17);
+
+  //Declaration of the pins for the battery indicator, and status LED as well as the pin for the buzzer
+  pinMode(batLED, OUTPUT); //Low battery LED
+  pinMode(statusLED, OUTPUT); //Status LED
+  pinMode(piezo, OUTPUT); //Piezo buzzer
+
+  //Piezo buzzer PWM settings
+  analogWriteFreq(4000); //Set the piezo frequency to 4kHz
+  analogWriteRange(100); //Set the dynamic range of the piezo
+   
+  for (int i = 0; i < 4; i = i + 1){
+     pinMode(switchPins[i], INPUT_PULLUP);
+  }
+
+  barometricSensor.begin(Wire1);
+  barometricSensor.setResolution(ms5637_resolution_osr_1024);
+  
+  //Take 16 readings and average them
+  startingPressure = 0.0;
+  for (int x = 0 ; x < 16 ; x++)
+    startingPressure += barometricSensor.getPressure();
+  startingPressure /= (float)16;
+
+  accel.begin();
+  accel.setRange(ADXL343_RANGE_16_G);
+  accel.setDataRate(ADXL343_DATARATE_400_HZ);
+  switchStartup();
+  delay(10000); //Wait for 10 seconds to allow the rocketeer to prepare for launch before the flight computer is armed.
+  preLaunch(); //Here I store the first second of data into the circular buffers
+  SDstartup();
+}
+
+void loop() {
+  batteryStatus(); //Check the battery level
+  
+  if (overtime == false){
+    currentPressure = barometricSensor.getPressure();
+    temp = barometricSensor.getTemperature();
+    sensors_event_t event;
+    accel.getEvent(&event);
+    rocketAccel = -((event.acceleration.y/9.81)-(event.acceleration.x/9.81))/sqrt(2);
+    altitudeDelta = barometricSensor.altitudeChange(currentPressure, startingPressure);
+    filteredAltitudeDelta = kalmanFilter(altitudeDelta);
+    
+
+    if (altitudeDelta > altThreshold && launchCondition1 == false){ //Threshold condition set to 10 m
+      launchCondition1 = true;
+    }
+    
+    if (rocketAccel > accelThreshold && launchCondition2 == false){
+      q++;
+      if (q > 10){ //launcCondition2 stablishes the requirement that to detect launch there should be at least an acceleration of 2g for 100 ms
+        launchCondition2 = true;
+      }
+    }
+
+    else if (rocketAccel < accelThreshold && launchCondition2 == false){
+      q = 0;
+    }
+
+    if (initVar == true){ //Store data to the circular buffer
+      accelerations.push(rocketAccel);
+      altitudes.push(altitudeDelta);
+      FilteredAltitudes.push(filteredAltitudeDelta);
+      times.push(millis()-t4); //Circular buffer for time
+
+      if (launchCondition1 == true && launchCondition2 == true){
+        initVar = false;
+          
+        for (int i = 0; i<=99; i++){ //Saving the buffer allows me to store the data measured before launch.
+         dataFile.print(times[i]-times[0]); //Here times[0] sets the time zero for the time variable
+         dataFile.print(',');
+         dataFile.print(altitudes.shift());
+         dataFile.print(',');
+         dataFile.print(FilteredAltitudes.shift());
+         dataFile.print(',');
+         dataFile.print(accelerations.shift());
+         dataFile.print(',');
+         dataFile.print(event.acceleration.z/9.81);
+         dataFile.print(',');
+         dataFile.println(temp, 1);
+        }
+          
+         dataFile.flush(); //Store data of the 908 ms before launch
+       }
+     }      
+
+    else if (initVar == false){
+      t1 = millis() - t4 - times[0];
+      recovery();  
+      dataFile.print(t1);
+      dataFile.print(',');
+      dataFile.print(altitudeDelta);
+      dataFile.print(',');
+      dataFile.print(filteredAltitudeDelta);
+      dataFile.print(',');
+      dataFile.print(rocketAccel);
+      dataFile.print(',');
+      dataFile.print(event.acceleration.z/9.81);
+      dataFile.print(',');
+      dataFile.println(temp, 1);
+
+      if (altitudeDelta > altold){ //Here is where I store the maximum altitude value
+        altMax = round(altitudeDelta);
+        altold = altMax;
+      }
+  
+      if (r == 200 && overtime == false){ //Here I set the rate at which I send data to the uSD card
+        r = 0;
+        dataFile.flush();
+       }
+       r++;
+       
+    
+      if (t1 >= tout){
+        overtime = true;
+        dataFile.flush();
+        dataFile.close(); //After timeout flush the data to the microSD card and close the file
+      }
+    }
+ }
+ beepnblink();
+}

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/kalmanFilter.ino

@@ -0,0 +1,20 @@
+// This program performs a Kalman filter of the flight data. It smoothens the data and ignores transitory events.
+
+// Q = process noise covariance
+// R = measurement noise covariance. Larger R means large measurement uncertainty. Larger Q means larger estimation uncertainty. Thus increasing Q corrects more.
+// Xpe0 = prior estimation of signal X at time t=0 (current state)
+// Xe1 = estimation of X at time t=1 (previous state)
+// Ppe0 = prior estimation of "error covariance" at t=0,  
+// P1 = error covariance at t=1, P0 = error covariance at t=0
+// K = Kalman gain, Xe0 = estimation of signal at t=0, Z = measured signal at t=0;
+
+float kalmanFilter(float Z){
+    Xpe0 = Xe1; // Assumption of prediction 1
+    Ppe0 = P1 + Q; // Update of prior estimation of "error covariance"
+    K = Ppe0/(Ppe0 + R); // Measurement update or correction of "Kalman gain"
+    Xe0 = Xpe0 + K * (Z - Xpe0); // Measurement update or correction of "estimated signal"
+    P0 = (1 - K) * Ppe0; // Measurement update or correction of "error covariance";
+    Xe1 = Xe0;
+    P1 = P0;
+  return Xe0;
+}

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/preLaunch.ino

@@ -0,0 +1,15 @@
+void preLaunch(){ //This code works great
+  t4 = millis();
+    
+  for (int i = 0; i<=99; i++){
+    currentPressure = barometricSensor.getPressure();
+    sensors_event_t event;
+    accel.getEvent(&event);
+    rocketAccel = ((event.acceleration.y/9.81+0.01)-(event.acceleration.x/9.81-0.04))/sqrt(2);
+    accelerations.push(rocketAccel);
+    altitudeDelta = barometricSensor.altitudeChange(currentPressure, startingPressure)+0.6;
+    altitudes.push(altitudeDelta);
+    FilteredAltitudes.push(kalmanFilter(altitudeDelta));
+    times.push(millis()-t4);
+  }  
+}

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/recovery.ino

@@ -0,0 +1,52 @@
+void recovery(){
+
+  if (timer == true && t1 >= (1000*EEPROM.read(1)+908)){ //Here the 908 ms correspond to the time covered by the circular buffer
+    servo1.write(EEPROM.read(3)); //Move servo1 to the final position EEPROM.read(3);
+    servo1.attach(servo1pin, zeroPos, extendedPos);
+    if (timer == true && t1 >= (1000*EEPROM.read(1)+908+2000)){ //We disable the servo after 2 seconds to save power
+      servo1.detach();
+    }
+
+    if (t1 >= (1000*EEPROM.read(1) + 500*EEPROM.read(6) + 908 + 100)){ //The additional 100 ms is to prevent both servos from moving simultaneously.
+      servo2.write(EEPROM.read(5));
+      servo2.attach(servo2pin, zeroPos, extendedPos);
+      timer = false;
+    }
+
+    if (t1 >= (1000*EEPROM.read(1) + 500*EEPROM.read(6) + 908 + 100 + 2000)){ //We disable the servo after 2 seconds to save power
+      servo2.detach();
+      timer = false;
+    }
+  }
+  
+  else if (automatic == true){
+    if ((filteredAltitudeDelta - altold) < -0.01){
+      n++;      
+      if (n == 4 && deploy == false){
+        deploy = true;
+        tconfig = t1;
+      }
+    }
+      
+    else if ((filteredAltitudeDelta - altold) >= 0 && deploy == false){
+      n = 0;
+    }
+    
+    if (deploy == true && (t1-tconfig) >= 500*EEPROM.read(0)){
+      servo1.write(EEPROM.read(3));
+      servo1.attach(servo1pin, zeroPos, extendedPos);
+        if (deploy == true && (t1-tconfig) >= (500*EEPROM.read(0) + 2000)){ //We disable the servo after 2 seconds to save power
+          servo1.detach();
+        }
+    }
+        
+    if (deploy == true && (t1-tconfig) >= (500*(EEPROM.read(0) + EEPROM.read(6)))){
+     servo2.write(EEPROM.read(5));
+     servo2.attach(servo2pin, zeroPos, extendedPos);
+     deploy = false;
+       if (deploy == true && (t1-tconfig) >= (500*(EEPROM.read(0) + EEPROM.read(6)) + 2000)){ //We disable the servo after 2 seconds to save power
+         servo2.detach();
+        }
+    }
+  }
+}

Firmware/complementary_coded_rotary_switch/Rev 2.2/control/rotSwitch.ino

@@ -0,0 +1,179 @@
+// This program reads the rotary switch.
+
+void readRotSwitch(){
+ for (int k = 0; k < 4; k++){
+  if (digitalRead(switchPins[k]) == LOW) {
+    bitClear(rotValue, k); //sets bit k to 1
+  }
+  else {
+     bitSet(rotValue, k); //sets bit k to 0
+  }
+ }
+}
+
+void switchStartup(){
+    readRotSwitch();
+
+  if (rotValue == 10){ //A Automatic mode
+    servo1.write(EEPROM.read(2)); //EEPROM.read(2)
+    servo1.attach(servo1pin, zeroPos, extendedPos);
+
+    delay(100); //It is important to have a delay to reduce the current spike drawn by the motors
+    servo2.write(EEPROM.read(4)); //EEPROM.read(4)
+    servo2.attach(servo2pin, zeroPos, extendedPos);
+
+    automatic = true;
+    delay(300);
+    blinkLED(EEPROM.read(0));
+    delay(500);
+    blinkLED(EEPROM.read(6));
+    servo1.detach(); //I detach the servos to save power
+    servo2.detach();
+    return;
+  }
+
+  else if (rotValue == 11){ //B Timer mode
+   servo1.write(EEPROM.read(2)); //EEPROM.read(2)
+   servo1.attach(servo1pin, zeroPos, extendedPos);
+
+   delay(100); //It is important to have a delay to reduce the current spike drawn by the motors
+   servo2.write(EEPROM.read(4)); //EEPROM.read(4)
+   servo2.attach(servo2pin, zeroPos, extendedPos);
+
+   timer = true;
+   delay(300);
+   blinkLED(EEPROM.read(1));
+   delay(500);
+   blinkLED(EEPROM.read(6));
+   servo1.detach(); //I detach the servos to save power
+   servo2.detach();
+   return;
+  }
+
+  else if (rotValue == 12){ //C, Configure the time for parachute deployment on automatic mode
+    while(1){
+      readRotSwitch();
+      blinkLED(1);
+      if (previousValue != rotValue){
+        EEPROM.write(0, rotValue);
+        EEPROM.commit();
+        previousValue = rotValue;
+          }
+        }
+      } 
+
+   else if (rotValue == 13){ //D, Configure the time for parachute deployment on timer mode.
+    while(1){
+      readRotSwitch();
+      blinkLED(1);
+      if (previousValue != rotValue){
+        EEPROM.write(1, rotValue);
+        EEPROM.commit();
+        previousValue = rotValue;
+        }
+      }
+    }   
+    
+
+  else if (rotValue == 14){ //E, Adjust servo's 1 initial possition
+    while(1){
+      readRotSwitch();
+      servo1.write(180*rotValue/15);
+      servo1.attach(servo1pin, zeroPos, extendedPos);
+      blinkLED(1);
+      if (previousValue != rotValue){
+        EEPROM.write(2, 180*rotValue/15);
+        EEPROM.commit();
+      }
+      previousValue == rotValue;
+    }
+  }
+
+  else if (rotValue == 15){ //F, Adjust servo's 1 final possition
+      while(1){
+        readRotSwitch();        
+        servo1.write(180*rotValue/15);
+        servo1.attach(servo1pin, zeroPos, extendedPos);
+
+        blinkLED(1);
+        if (previousValue != rotValue){
+          EEPROM.write(3, 180*rotValue/15);
+          EEPROM.commit();
+        }
+        previousValue == rotValue;
+      }
+  }
+
+  else if (rotValue == 0){ //0, Adjust the servo's 2 initial possition
+    while(1){
+      readRotSwitch();
+      servo2.write(180*rotValue/15);
+      servo2.attach(servo2pin, zeroPos, extendedPos);
+
+      blinkLED(1);
+      if (previousValue != rotValue){
+        EEPROM.write(4, 180*rotValue/15);
+        EEPROM.commit();
+      }
+      previousValue == rotValue;
+    }
+  }
+
+  else if (rotValue == 1){ //1, Adjust the servo's 2 final possition
+      while(1){
+        readRotSwitch();        
+        servo2.write(180*rotValue/15);
+        servo2.attach(servo2pin, zeroPos, extendedPos);
+
+        blinkLED(1);
+        if (previousValue != rotValue){
+          EEPROM.write(5, 180*rotValue/15);
+          EEPROM.commit();
+        }
+        previousValue == rotValue;
+      }
+  }
+
+    else if (rotValue == 2){ //2, Adjust the deploy time for servo 2 after servo 1
+    while(1){
+      readRotSwitch();
+      blinkLED(1);
+      if (previousValue != rotValue){
+        EEPROM.write(6, rotValue);
+        EEPROM.commit();
+        previousValue = rotValue;
+      }
+    }
+  }
+
+    else if (rotValue == 3){ //3, Test the servo motors
+     servo1.write(EEPROM.read(2)); //EEPROM.read(2)
+     servo1.attach(servo1pin, zeroPos, extendedPos);
+     delay(500); //It is important to have a delay to reduce the current spike drawn by the motors
+     servo1.detach();
+     servo2.write(EEPROM.read(4)); //EEPROM.read(4)
+     servo2.attach(servo2pin, zeroPos, extendedPos);
+     delay(500);
+     servo2.detach();
+     blinkLED(2);
+     delay(2000);
+     servo1.write(EEPROM.read(3)); //Move servo1 to the final position EEPROM.read(3);
+     servo1.attach(servo1pin, zeroPos, extendedPos);
+     blinkLED(2);
+     delay(2000);
+     servo1.detach();
+     servo2.write(EEPROM.read(5));
+     servo2.attach(servo2pin, zeroPos, extendedPos);
+     delay(500);
+     servo2.detach();
+     while(1){
+        sleep_ms(100000);
+     }
+  }
+  
+    else {
+      while (true){
+        sleep_ms(10000);
+      }
+  }
+}