Servo sweep using millis with intermediate stops.

Hi there,

I started with arduino a couple of weeks ago and I think i learned a lot already. Have done a few small projects.
At the moment i would like to build a smarties sorting machine. Using a TCS34725 color sorter, and two servos. One for feeding, and one for the color.
Now I have found a really great code for the TCS sensor which on one side is using millis to control the servo that points out the color and on the other its using an interrupt (dont know if that is relevant here).
No problem detecting colors, and poining on them with the respective servo.

Now for the feeding machanism, I need a servo to have three steps with some waiting time (delay is not an option, because then the color-detecting part of the code is not working anymore) at each position.
eg.

Feedposition at 20° and wait for 0.5second
Scanposition at 80° and wait for 1second
Releaseposition at 140° and wait for 0.5seconds

then repeat this over and over again.

In the CODE the Servo that is supposed to act like this can be found under void rotateTopservo

for reference I attached the code I have at the moment. color detecting is working nicely, TOPSERVO is sweeping between 0° to 180° at the same time.

Now how do I add intermediate positions with waiting time like I described above?

If you need the rest of the code let me know, it was too big for one post…

Thanks a lot for your support.
cyril

// put your main code here, to run repeatedly:

// Tutorial 6a. Colour finder w. ams TCS34725 and HD-1900A

// Main parts: Adafruit Metro Mini, Adafruit TCS34725 sensor breakout-
// board, Pololu HD-1900A servo, ultra-matt black sensor shroud

// Libraries required to use the I2C communication protocol, drive the
// servo and interface with the sensor; use the latest version
#include <Wire.h>
#include "Servo.h"
#include "Adafruit_TCS34725.h"


// Variables that remain constant
const int interruptPin = 3; // Signal input pin from TCS34725 interrupt
// Two-dimensional array containing averaged RGB values from "colour
// training"; matching servo rotation angles for the 10° colour wheel
// segments. No particular order or sorting of samples is necessary. 4th value in array = angle in microseconds
const int SAMPLES[][4] = {
 {1173, 886,  1119,  660}, //   1 violet
  {711, 1059,  1399,  760}, //  2 blau
  {2042,  1076, 915,  850},//    3 orange
  {869,  1229,  907,  950},//    4 grün          kärtli
  {1824, 832, 1082, 1050},//   5 pink
  {1794,721, 769, 1160},//      6 rot
  {2908,2670 ,1540, 1270},//    7 gelb

  /*{742, 653,  679,  660}, //   1 violet
  {762,  819,  825,  760}, //  2 blau
  {998,  655, 589,  850},//    3 orange
  {782,  780,  631,  950},//    4 grün           smarties
  { 924,  618, 646, 1050},//   5 pink
  {951, 546, 540, 1160},//      6 rot
  {1189, 986, 752, 1270},//    7 gelb
  {749, 573, 521, 1570},//    8 braun   */  
};





// Determines the number of samples stored in the array
const byte samplesCount = sizeof(SAMPLES) / sizeof(SAMPLES[0]);
// Instances a Servo object from the library and sets the sensing duration
// (integration time) and sensitivity (gain); see library options. Longer
// integration time plus lower gain = higher accuracy
Adafruit_TCS34725 SENSOR = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_700MS, TCS34725_GAIN_1X);
// Instances a Servo object from the library
Servo SERVOTOP;
Servo SERVOBOTTOM;

// Variables that can change
unsigned long timeNow = 0; // Timestamp that updates each loop() iteration
const byte timeIntervalTOP = 5; // Time to pass until the next servo rotation
const byte timeIntervalBOTTOM = 1;
int angleCurrentTOP;// Angle in microseconds = finer rotation resolution
int angleCurrentBOTTOM; // Angle in microseconds = finer rotation resolution
int angleTargetTOP; // Angle in microseconds = finer rotation resolution
int angleTargetBOTTOM;// Angle in microseconds = finer rotation resolution
uint16_t redSensor, greenSensor, blueSensor, clearSensor; // Raw readings
volatile boolean state = false; // Flag to toggle interrupt on and off



//for TOP Servo
int servoPos = 0;
boolean servoGoingUp = true;
#define SERVO_STEP 1  // Increase to speed up servo movement

long previousMillis = 0;
long interval = 10;





// For the compiler: isr() = function called when an interrupt occurs
void isr()
{
  state = true;
}

void setup()
{
  // TCS34725 interrupt output is active-LOW and open-drain
  pinMode(interruptPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPin), isr, FALLING);

  // Initialises the sensor; the LED is on, but could be switched via pin
  // A0 programmatically, or via connection to GND (LED off)
  SENSOR.begin();
  // Set persistence filter = generates an interrupt for every cycle,
  // regardless of the integration time limits
  SENSOR.write8(TCS34725_PERS, TCS34725_PERS_NONE);
  // Activate the sensor's interrupt function
  SENSOR.setInterrupt(true);

  // Uncomment for "colour training" and checking purposes
  Serial.begin(19200);
  if (SENSOR.begin()) {
    Serial.println("Found sensor");
  }

  // Initialises the servo; connect its signal wire to pin 3
  SERVOTOP.attach(3);
  SERVOBOTTOM.attach(4);
  // Rotates the servo to initial position (angle 0° in microseconds)
 // SERVOTOP.writeMicroseconds(1970);
  SERVOBOTTOM.writeMicroseconds(880);

  // Seed both with an initial value to start with something. All samples
  // are related to an angle expressed in microseconds, stored in the
  // array. At the start, the current angle must be lower than the target
  // angle = the servo starts from the 0° position
  angleCurrentTOP = 0;
  angleTargetTOP = 1800;
  angleCurrentBOTTOM = 0;
  angleTargetBOTTOM =880;
}

void loop()
{

  rotateServoTOP();
  // A call to this function reads from the TCS34725 sensor. Unlike reading
  // from the sensor in the basic way, listening to the sensor's external
  // interrupt, the long integration time of 700ms - necessary to obtain
  // more accurate colour readings - does no longer block loop() from
  // executing other code
  readSensor();
  // A call to this function iterates through the array to retrieve a
  // matching colour sample
  identifySample();
  // A call to this function rotates the servo based on the angle in
  // microseconds, also retrieved from the array
  rotateServoBOTTOM();

 
}






  void rotateServoTOP()

  {
unsigned long currentMillis = millis();
  
  if(currentMillis - previousMillis > interval) {
    previousMillis = currentMillis;   
 
  SERVOTOP.write(servoPos);
if (servoGoingUp)
{
   servoPos += SERVO_STEP;
   if (servoPos >= 180)
   {
       servoPos = 180;
       servoGoingUp = false;
   }
}
else
{
   servoPos -= SERVO_STEP;
   if (servoPos <= 0)
   {
       servoPos = 0;
       servoGoingUp = true;
   }
}

}

}