Servo Motor, RGB LED and Proximity Sensor

Hello,
So I’m pretty much new to code and I would greatly appreciate help in this. I’m trying to activate a servo motor to turn to a specific angle slowly and then have LEDs run through a Color blending sequence. When thats done, the servo motor will turn back to its original angle. This is all activated by a infrared proximity sensor (GP2Y0A02YK0F) so that when the user gets close it will activate this program. Generally when there is nothing in front of the sensor, the LEDs are off and the Servo motor is in the original position. I haven’t coded the sensor part yet, as I was just trying to get the LED and Motor code working first, which I’m not sure if I’m completely doing this wrong (because the two programs I am combining -for the most part- run smoothly when independent of each other) or if I should take another approach…

Also would encoding the sensor be by if statements?

/*RGB Color Changing and Motor activated on Proximity Sensor
  Version 1.0
  
  Combination of ColorCrossfader and ServoMotor Arduino Examples
  
  Description: Within a certain range, or proximity the viewer is from the Infrared Proximity Sensor, the Servo motor will turn to 105 
  degrees in a slow speed. Then the RGB LEDs will light in a specific color-blending sequence. Then the motor will turn back to 86
  degrees (starting point). Here it will hold until another object in range activates the motor and LEDs to activate.
  
*/


#include <VarSpeedServo.h>

//Servo Variables
int numberOfServos=1;  //will change to 2 Servos later (use array)
VarSpeedServo servo1;
int servoPin = 7;
int closepos = 105;
int openpos = 86;
int pos = 86;

//Color Variables
int redPin = 9;   // Red LED,   connected to digital pin 9
int grnPin = 10;  // Green LED, connected to digital pin 10
int bluPin = 11;  // Blue LED,  connected to digital pin 11

//Color arrays
int black[3] = { 0, 0, 0};
int white[3] = { 100, 100, 100};
int red[3] = { 100, 0, 0};
int yellow[3] = { 55, 95, 0};
int green[3] = { 0, 100, 0};
int blue[3] = { 0, 0, 100};
int purple[3] = { 70, 0, 95};
int dimWhite[3] = { 30, 30, 30 };

//Set initial color
int redVal = black[0];
int grnVal = black[1];
int bluVal = black[2];

int wait = 5;      // 5ms internal crossFade delay; increase for slower fades
int hold = 0;       // Optional hold when a color is complete, before the next crossFade
int DEBUG = 1;      // DEBUG counter; if set to 1, will write values back via serial
int loopCount = 60; // How often should DEBUG report?
int repeat = 0;     // How many times should we loop before stopping? (0 for no stop)
int j = 0;          // Loop counter for repeat

//Initialize color variables
int prevR = redVal;
int prevG = grnVal;
int prevB = bluVal;

void setup()
{
  for (int i=0; i<numberOfServos; i++)
  {
    pinMode(redPin, OUTPUT);
    pinMode(grnPin, OUTPUT);
    pinMode(bluPin, OUTPUT);
    
    if(DEBUG) {
      Serial.begin(9600);
    }
      
    servo1.attach(servoPin);
    servo1.slowmove(100,2);
  }
}


//Main Program
void loop()
{


  for(pos = 86; pos < 105; pos +=1)
  {
    servo1.slowmove(105,1);
    color();

  }
  for(pos = 105;pos>=86; pos-=1)
  {
    servo1.slowmove(86,1);
  }
} 
 
/*  //test v.2
  for (int i=0;i<numberOfServos;i++)
  {
    servo1.slowmove(openpos,1);
    delay(5000);
    //color();
    servo1.slowmove(closepos,1);
    delay(5000);
  }
  
} */

void color()
{ 
  crossFade(black);
  crossFade(red);
  crossFade(yellow);
  crossFade(green);
  crossFade(blue);
  crossFade(dimWhite);
  crossFade(black);
  
}


//MATH STUFF
/* BELOW THIS LINE IS THE MATH -- YOU SHOULDN'T NEED TO CHANGE THIS FOR THE BASICS
* 
* The program works like this:
* Imagine a crossfade that moves the red LED from 0-10, 
*   the green from 0-5, and the blue from 10 to 7, in
*   ten steps.
*   We'd want to count the 10 steps and increase or 
*   decrease color values in evenly stepped increments.
*   Imagine a + indicates raising a value by 1, and a -
*   equals lowering it. Our 10 step fade would look like:
* 
*   1 2 3 4 5 6 7 8 9 10
* R + + + + + + + + + +
* G   +   +   +   +   +
* B     -     -     -
* 
* The red rises from 0 to 10 in ten steps, the green from 
* 0-5 in 5 steps, and the blue falls from 10 to 7 in three steps.
* 
* In the real program, the color percentages are converted to 
* 0-255 values, and there are 1020 steps (255*4).
* 
* To figure out how big a step there should be between one up- or
* down-tick of one of the LED values, we call calculateStep(), 
* which calculates the absolute gap between the start and end values, 
* and then divides that gap by 1020 to determine the size of the step  
* between adjustments in the value.
*/
int calculateStep(int prevValue, int endValue) {
  int step = endValue - prevValue; // What's the overall gap?
  if (step) {                      // If its non-zero, 
    step = 1020/step;              //   divide by 1020
  } 
  return step;
}

/* The next function is calculateVal. When the loop value, i,
*  reaches the step size appropriate for one of the
*  colors, it increases or decreases the value of that color by 1. 
*  (R, G, and B are each calculated separately.)
*/

int calculateVal(int step, int val, int i) {

  if ((step) && i % step == 0) { // If step is non-zero and its time to change a value,
    if (step > 0) {              //   increment the value if step is positive...
      val += 1;           
    } 
    else if (step < 0) {         //   ...or decrement it if step is negative
      val -= 1;
    } 
  }
  // Defensive driving: make sure val stays in the range 0-255
  if (val > 255) {
    val = 255;
  } 
  else if (val < 0) {
    val = 0;
  }
  return val;
}

/* crossFade() converts the percentage colors to a 
*  0-255 range, then loops 1020 times, checking to see if  
*  the value needs to be updated each time, then writing
*  the color values to the correct pins.
*/

void crossFade(int color[3]) {
  // Convert to 0-255
  int R = (color[0] * 255) / 100;
  int G = (color[1] * 255) / 100;
  int B = (color[2] * 255) / 100;

  int stepR = calculateStep(prevR, R);
  int stepG = calculateStep(prevG, G); 
  int stepB = calculateStep(prevB, B);

  for (int i = 0; i <= 1020; i++) {
    redVal = calculateVal(stepR, redVal, i);
    grnVal = calculateVal(stepG, grnVal, i);
    bluVal = calculateVal(stepB, bluVal, i);

    analogWrite(redPin, redVal);   // Write current values to LED pins
    analogWrite(grnPin, grnVal);      
    analogWrite(bluPin, bluVal); 

    delay(wait); // Pause for 'wait' milliseconds before resuming the loop

    if (DEBUG) { // If we want serial output, print it at the 
      if (i == 0 or i % loopCount == 0) { // beginning, and every loopCount times
        Serial.print("Loop/RGB: #");
        Serial.print(i);
        Serial.print(" | ");
        Serial.print(redVal);
        Serial.print(" / ");
        Serial.print(grnVal);
        Serial.print(" / ");  
        Serial.println(bluVal); 
      } 
      DEBUG += 1;
    }
  }
  // Update current values for next loop
  prevR = redVal; 
  prevG = grnVal; 
  prevB = bluVal;
  delay(hold); // Pause for optional 'wait' milliseconds before resuming the loop
}

If you could upload some photos so I can see what it is you're trying to do?

Here is a schematic of my prototype on the breadboard:

And the video of what I'm trying to do with each program running independently. But basically when the flower opens, it's supposed to stay open and activate the color fade (in the second video) and then close back up. That's just the code I'm working on now since it doesn't work together (or I'm not sure how to integrate it together for it to play in that specific order). Then I will try to add in the proximity sensor, which I need help on too, but first things first.

Servo motor Test: http://www.youtube.com/watch?v=29yuBINsTno&feature=youtu.be Color Fader Test: http://www.youtube.com/watch?v=6bQZNVu-6yE&feature=youtu.be

Need code to do: 1) Within certain distance take from IR sensor, activate code 2) Turn motor to certain degree to open petals 3) Hold angle position and activate CrossColor Fade code (cycle through different color combinations) 4) Color fades off and Turn motor back to original degree to close petals 5) Hold closed position until something comes within certain distance again

Updated code just incase I missed anything when I changed it last time, but should be similar, except for pins.

#include <VarSpeedServo.h>

//Servo Variables
int numberOfServos=2;  //will change to 2 Servos later (use array)
VarSpeedServo servo1;
int servoPin = 4;
int closepos = 1;  //int closepos = 99.5; 
int openpos = 6;  //int openpos = 85.5;
int pos = 86.5;

//Color Variables
int redPin = 6;   // Red LED,   connected to digital pin 9
int grnPin = 5;  // Green LED, connected to digital pin 10
int bluPin = 3;  // Blue LED,  connected to digital pin 11

//Color arrays
int black[3] = { 0, 0, 0};
int white[3] = { 100, 100, 100};
int red[3] = { 100, 0, 0}; 
int yellow[3] = { 40, 95, 0}; //red +50 (55)
int green[3] = { 0, 100, 0};
int blue[3] = { 0, 0, 100};
int purple[3] = { 70, 0, 95}; //red +50 (70)
int dimWhite[3] = { 30, 30, 30 };

//Set initial color
int redVal = black[0];
int grnVal = black[1];
int bluVal = black[2];

int wait = 10;      // 5ms internal crossFade delay; increase for slower fades
int hold = 0;       // Optional hold when a color is complete, before the next crossFade
int DEBUG = 1;      // DEBUG counter; if set to 1, will write values back via serial
int loopCount = 60; // How often should DEBUG report?
int repeat = 0;     // How many times should we loop before stopping? (0 for no stop)
int j = 0;          // Loop counter for repeat

//Initialize color variables
int prevR = redVal;
int prevG = grnVal;
int prevB = bluVal;

void setup()
{
    pinMode(redPin, OUTPUT);
    pinMode(grnPin, OUTPUT);
    pinMode(bluPin, OUTPUT);
    
    if(DEBUG) {
      Serial.begin(9600);
    }    
  for (int s=0; s<numberOfServos; s++)  //this SETUP is making COLOR FADE not work
  {
    servo1.attach(servoPin);
    servo1.slowmove(openpos,1);
   
 
     } 
  
}


//Main Program
void loop()
{
  crossFade(red);
  crossFade(green);
  crossFade(blue);
  crossFade(yellow);
  
    if (repeat) { // Do we loop a finite number of times?
    j += 1;
    if (j >= repeat) { // Are we there yet?
      exit(j);         // If so, stop.
    }
  }
 
/*

 
  //TEST v.2
  for (int i=0; i<10 ;i++)
  {
    servo1.slowmove(openpos,1);
    delay(50);
    color();
    servo1.slowmove(closepos,1);
    delay(50);
  } */
  
 //TEST v.3
    for(int g=0; g < 10; g++)
    {
    if(servo1.read() == openpos)
      servo1.slowmove(closepos, 1);   //move servo to 100 at speed of 300, Middle angle is 90 (a little above or less moves left or right slowly). speed > is also slower
      
      else if(servo1.read() == closepos)
       
          servo1.slowmove(openpos, 1); //counterclock 
          //color();       
    }   
}

/*void color()
{ 
  crossFade(black);
  crossFade(red);
  //crossFade(yellow);
  crossFade(green);
  crossFade(blue);
  crossFade(yellow);
  //crossFade(dimWhite);
  crossFade(black);
  
    if (repeat) { // Do we loop a finite number of times?
    j += 1;
    if (j >= repeat) { // Are we there yet?
      exit(j);         // If so, stop.
    }
  }
} */


//MATH STUFF
/* BELOW THIS LINE IS THE MATH -- YOU SHOULDN'T NEED TO CHANGE THIS FOR THE BASICS
* 
* The program works like this:
* Imagine a crossfade that moves the red LED from 0-10, 
*   the green from 0-5, and the blue from 10 to 7, in
*   ten steps.
*   We'd want to count the 10 steps and increase or 
*   decrease color values in evenly stepped increments.
*   Imagine a + indicates raising a value by 1, and a -
*   equals lowering it. Our 10 step fade would look like:
* 
*   1 2 3 4 5 6 7 8 9 10
* R + + + + + + + + + +
* G   +   +   +   +   +
* B     -     -     -
* 
* The red rises from 0 to 10 in ten steps, the green from 
* 0-5 in 5 steps, and the blue falls from 10 to 7 in three steps.
* 
* In the real program, the color percentages are converted to 
* 0-255 values, and there are 1020 steps (255*4).
* 
* To figure out how big a step there should be between one up- or
* down-tick of one of the LED values, we call calculateStep(), 
* which calculates the absolute gap between the start and end values, 
* and then divides that gap by 1020 to determine the size of the step  
* between adjustments in the value.
*/
int calculateStep(int prevValue, int endValue) {
  int step = endValue - prevValue; // What's the overall gap?
  if (step) {                      // If its non-zero, 
    step = 1020/step;              //   divide by 1020
  } 
  return step;
}

/* The next function is calculateVal. When the loop value, i,
*  reaches the step size appropriate for one of the
*  colors, it increases or decreases the value of that color by 1. 
*  (R, G, and B are each calculated separately.)
*/

int calculateVal(int step, int val, int i) {

  if ((step) && i % step == 0) { // If step is non-zero and its time to change a value,
    if (step > 0) {              //   increment the value if step is positive...
      val += 1;           
    } 
    else if (step < 0) {         //   ...or decrement it if step is negative
      val -= 1;
    } 
  }
  // Defensive driving: make sure val stays in the range 0-255
  if (val > 255) {
    val = 255;
  } 
  else if (val < 0) {
    val = 0;
  }
  return val;
}

/* crossFade() converts the percentage colors to a 
*  0-255 range, then loops 1020 times, checking to see if  
*  the value needs to be updated each time, then writing
*  the color values to the correct pins.
*/

void crossFade(int color[3]) {
  // Convert to 0-255
  int R = (color[0] * 255) / 100;
  int G = (color[1] * 255) / 100;
  int B = (color[2] * 255) / 100;

  int stepR = calculateStep(prevR, R);
  int stepG = calculateStep(prevG, G); 
  int stepB = calculateStep(prevB, B);

  for (int i = 0; i <= 1020; i++) {
    redVal = calculateVal(stepR, redVal, i);
    grnVal = calculateVal(stepG, grnVal, i);
    bluVal = calculateVal(stepB, bluVal, i);

    analogWrite(redPin, redVal);   // Write current values to LED pins
    analogWrite(grnPin, grnVal);      
    analogWrite(bluPin, bluVal); 

    delay(wait); // Pause for 'wait' milliseconds before resuming the loop

    if (DEBUG) { // If we want serial output, print it at the 
      if (i == 0 or i % loopCount == 0) { // beginning, and every loopCount times
        Serial.print("Loop/RGB: #");
        Serial.print(i);
        Serial.print(" | ");
        Serial.print(redVal);
        Serial.print(" / ");
        Serial.print(grnVal);
        Serial.print(" / ");  
        Serial.println(bluVal); 
      } 
      DEBUG += 1;
    }
  }
  // Update current values for next loop
  prevR = redVal; 
  prevG = grnVal; 
  prevB = bluVal;
  delay(hold); // Pause for optional 'wait' milliseconds before resuming the loop
}