Function as background and interrupt other

Projects Programming
1

First of all sorry for the comments in spanish, but I think it is easy to understand. In the code below I try to set a color detector as a background function a function that works at every time that doesn't depend on anything, but something is wrong I have made a lot of try but doesn't seem to work, and the other thing is interrupt the movements of ardumoto when fsrlectura detects a force I use ifs but it doesn't interrupt the ardumoto movements, I will be very thankful If you help me.
The code:

//Declaracion variables//
#include <Servo.h>
#include <ADJDS311.h>
#include <Wire.h>

int pwm_a = 3; //pin ardumoto//
int pwm_b = 11;//pin ardumoto//
int dir_a = 12;//pin ardumoto//
int dir_b = 13;//pin ardumoto//
int fsrPin = 0;//pin analogico para sensor de fuerza//fsr pin
int fsrlectura;//variable en de lectura de sensor de fuerza//
int sensorLed_pin = 2; //pin de led de Color Light Sensor Evaluation Board//
ADJDS311 colorSensor(sensorLed_pin);
Servo myservo; //declaracion de servos//
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
Servo myservo6;
//------------------------------------------------//
//--------Declaracion de pines y posiciones-----//
void setup(){
  Serial.begin(9600);
  colorSensor.init();
  colorSensor.ledOn(); //LED de Color Light Sensor Evaluation Board
  myservo.attach(10);
  myservo2.attach(8);
  myservo3.attach(9);
  myservo4.attach(7);
  myservo5.attach(5);
  myservo6.attach(6);
  myservo4.write(90);//posicion inicial//initial pos
  myservo5.write(0);//posicion inicial//initial pos
  myservo6.write(180);//poscion inicial//initial pos
  colorSensor.calibrate(); 
  
  pinMode(pwm_a, OUTPUT);//se declaran los pines de ardumoto como salida//ardumoto pins as output
  pinMode(pwm_b, OUTPUT);//se declaran los pines de ardumoto como salida//
  pinMode(dir_a, OUTPUT);//se declaran los pines de ardumoto como salida//
  pinMode(dir_b, OUTPUT);//se declaran los pines de ardumoto como salida//
  
 analogWrite(pwm_a, 60); 
  analogWrite(pwm_b, 60);
  //------------------------------------------------//
}
// Lo que TOB debe de hacer//
void loop(){
  noInterrupts();
  RGBC color = colorSensor.read(); //leen los colores  read colors
  Serial.print(color.red);//se imprime color rojo//
  Serial.print(" | ");
  Serial.print(color.green);//se imprime color verde//
  Serial.print(" | ");
  Serial.print(color.blue);//se imprime color azul//
  Serial.print(" | ");
  Serial.println(color.clear);
  fsrlectura = analogRead(fsrPin);
  Serial.print(" | ");
  Serial.print ("Lectura FSR = ");
  Serial.print(fsrlectura);//se imprime lectura de sensor de fuerza//
  delay(1100);
  
 digitalWrite(dir_a, LOW); // movimients ardumoto.////movements of ardumoto)
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(3000);
  analogWrite(pwm_a, 60);// movimients ardumoto.//
  analogWrite(pwm_b, 60);// movimients ardumoto.//
  delay(3000);                                                              
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, HIGH);// movimients ardumoto.//
  delay(3000);
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(2000);

  //------------------------------------------------------------------this is the background function-------------------------------------------------//
  
    if(color.red > 980 && color.green < 1000) {  //----------------rojo-----// red
      myservo.write(180);//si se detecta color rojo, abrir depsito AA//
    }
    else{
      myservo.write(0);//si no no abrir//
    }
    if ( color.red < 500 && color.blue < 380 && color.clear < 330 && color.green > 350 && color.green <450){    //---------------verde----------// green
      myservo2.write(90);//si se detecta color verde, abrir deposito de Cuadradas//
    }
    else{
      myservo2.write(0);//si no no abrir//
    }
    if (color.red > 400 && color.red < 600 && color.blue > 260 && color.green < 1000){//----------azul------// blue
      myservo3.write(90);//si se detecta color azul abrir deposito AAA//
    }
    else{
      myservo3.write(0);//si no no abrir//
    }
  //------------------------------------------------------------------------------------------------------------------------//
  
  if (fsrlectura < 10){//si no hay fuerza en fsr mantener pinza en posicion inicial//if no force keep intial position 
    myservo4.write(90);
    myservo5.write(0);
    myservo6.write(180);
  }
  
  

//---------------------------------Here I want to interrupt the ardumoto movemets when fsrlectura detects a force------------//
  
  if (fsrlectura > 630 && fsrlectura < 690) {// ----------AAA// if detects a force do not move ardumoto 
  analogWrite(pwm_a, 0);
  analogWrite(pwm_b, 0);
  digitalWrite(dir_a, LOW);// Movements of ardumoto.
digitalWrite(dir_b, LOW);                  
    myservo4.write(120);          //--------------this are just a secuence of servo movements, but for any reason I can't interrupt the ardumoto//
    delay(1300);
    myservo5.write(180);
    delay(1300);
    myservo6.write(0);
    delay(500);
    myservo6.write(180);
    delay(500);
    myservo6.write(0);
  }
  else{
    myservo4.write(90);    //--------------------if no force is detected keep initial pos and keep moving ardumoto-------//
    myservo5.write(0);
    myservo6.write(180);
    digitalWrite(dir_a, LOW); // movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(3000);
  analogWrite(pwm_a, 60);// movimients ardumoto.//
  analogWrite(pwm_b, 60);// movimients ardumoto.//
  delay(3000);                                                              
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, HIGH);// movimients ardumoto.//
  delay(3000);
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(2000);

  }

  if (fsrlectura > 400 && fsrlectura < 450){ //---------AA//
  analogWrite(pwm_a, 0);
  analogWrite(pwm_b, 0);
  digitalWrite(dir_a, LOW);// Movements of ardumoto.
digitalWrite(dir_b, LOW);
    myservo5.write(20);
    myservo4.write(0);
    delay(1000);
    myservo6.write(0);
    delay(500);
    myservo6.write(180);
    delay(500);
    myservo6.write(0);
  }
  else{
    myservo4.write(90);
    myservo5.write(0);
    myservo6.write(180);
     digitalWrite(dir_a, LOW); // movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(3000);
  analogWrite(pwm_a, 60);// movimients ardumoto.//
  analogWrite(pwm_b, 60);// movimients ardumoto.//
  delay(3000);                                                              
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, HIGH);// movimients ardumoto.//
  delay(3000);
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(2000);

   
  }

  if (fsrlectura > 300 && fsrlectura < 420) { //-------------9V//
  analogWrite(pwm_a, 0);
  analogWrite(pwm_b, 0);
  digitalWrite(dir_a, LOW);// Movements of ardumoto.
digitalWrite(dir_b, LOW);
    myservo4.write(50);
    delay(1000);
    myservo5.write(180);
    delay(1000);
    myservo6.write(0);
    delay(1000);
    myservo6.write(180);
    delay(500);
    myservo6.write(0);
  }
  else{
    myservo4.write(90);
    myservo5.write(0);
    myservo6.write(180);
     digitalWrite(dir_a, LOW); // movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(3000);
  analogWrite(pwm_a, 60);// movimients ardumoto.//
  analogWrite(pwm_b, 60);// movimients ardumoto.//
  delay(3000);                                                              
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, HIGH);// movimients ardumoto.//
  delay(3000);
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(2000);

  }

  if (fsrlectura > 576) { //-------------D//
  analogWrite(pwm_a, 0);
  analogWrite(pwm_b, 0);
  digitalWrite(dir_a, LOW);// Movements of ardumoto.
digitalWrite(dir_b, LOW);
    myservo4.write(145);
    delay(1000);
    myservo5.write(30);
    delay(1000);
    myservo6.write(0);
    delay(1000);
    myservo6.write(180);
    delay(500);
    myservo6.write(0);
  }
  else{
    myservo4.write(90);
    myservo5.write(0);
    myservo6.write(180);
     digitalWrite(dir_a, LOW); // movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(3000);
  analogWrite(pwm_a, 60);// movimients ardumoto.//
  analogWrite(pwm_b, 60);// movimients ardumoto.//
  delay(3000);                                                              
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, HIGH);// movimients ardumoto.//
  delay(3000);
  digitalWrite(dir_a, HIGH);// movimients ardumoto.//
  digitalWrite(dir_b, LOW);// movimients ardumoto.//
  delay(2000);

  }
}
2

You have one process space on the Arduino - no multithreading, no background tasks.

  noInterrupts();

This will ensure that you don't get interrupted, which is contrary to your stated (unachievable) goal.

  delay(1100);
  delay(3000);
  delay(3000);                                                              
  delay(3000);
  delay(2000);
    delay(1300);
    delay(1300);
    delay(500);
    delay(500);

You'd discover changes in the force sensor much more quickly without this crap.

3

where I´m supposed to put all that delays, I was thinking to use while for stop ardumoto, ith this I will only stop it when condition is true.

4

where I´m supposed to put all that delays

Oh, man, I'd really like to respond to that, but good taste dictates that I not.

You need to highlight each one of them, and press delete. Get rid of them. You need to master the blink without delay example, to learn how to write code that does not need delay().

Think about how YOU would turn the LEDs on and off at the right times, with a watch, a pad of paper, and a series of switches.

5

Delay should have been left out of Arduino IMO. Then we wouldn't all this nonsense about Foreground, Background, Multitasking, Multithreading etc etc. I suppose we'd still be stuck with ill conceived loops........

6

I suppose we'd still be stuck with ill conceived loops........

Yeah, but they'd be easier to unravel than having to massively restructure a house of cards built using delay() everywhere.