Arduino Catapult - NeoPixel

#include <Adafruit_NeoPixel.h>
#include <SoftwareServo.h> 

SoftwareServo  lockServo ; 
SoftwareServo  armServo ; 
SoftwareServo  trayServo ; 
SoftwareServo  baseServo ; 

#define lockOpen    110 
#define lockClosed  20 

#define armOpen     140 
#define armArmed    45

#define trayOpened  164
#define trayClosed  167


#define ringPin     10

#define irPinRight  12
#define irPinLeft   4

int basePos = 90 ; 


unsigned long lastTimer = millis();


unsigned long pixelTimer = millis();
int currentPixelLed = 0 ; 


Adafruit_NeoPixel strip = Adafruit_NeoPixel(12, ringPin, NEO_GRB + NEO_KHZ800);

void rest() {
  
  sweep(lockServo,lockServo.read(),lockOpen,15) ; 
  sweep(armServo,armServo.read(),armOpen,15) ; 
 
}

void prepareToShoot() {

  sweep(lockServo,lockServo.read(),lockClosed,15) ; 
  sweep(armServo,armServo.read(),armArmed,15) ; 
  
}

void shoot() {

  sweep(lockServo,lockServo.read(),lockOpen,15) ; 
  
}


void sweep ( SoftwareServo  &servo , int from , int to , int speed  ) {

  int pos = 0 ;

  if (from < to) {

    for (pos = from; pos <= to; pos += 1) { 
      servo.write(pos);              
      delay(speed);     
      SoftwareServo::refresh();                  
    }
    
  } else {

    for (pos = from; pos >= to; pos -= 1) { 
      servo.write(pos);              
      delay(speed);     
      SoftwareServo::refresh();                  
    }

    
  }


}


void trayClose(){
  sweep(trayServo,trayServo.read(),trayClosed,22) ; 
}

void trayOpen() {
  sweep(trayServo,trayServo.read(),trayOpened,22) ; 
}

void trayRelease() {

    trayOpen() ; 
    trayClose() ;     
    delay(2000);

}


void ringAllOff() {

    for (int pixel = 0 ; pixel<12 ; pixel++ ) {
      strip.setPixelColor(pixel, 0, 0, 0);      
    }

    strip.show();

}




#define animIdle        0  
#define animTurnRight   1  
#define animTurnLeft    2  
#define animTurnShoot   3 

int animMode = animIdle ; 



void animCircle() {

    
  unsigned long currentTimer = millis();

  if ( (currentTimer - pixelTimer) > 80  ) {

    pixelTimer = currentTimer ; 

    ringAllOff() ;      
    strip.setPixelColor(currentPixelLed, 50, 250, 50);      
    strip.show();      

    currentPixelLed++ ; 
    if (currentPixelLed==12) currentPixelLed = 0 ;    
    
  }  
  
}


int currentColorRed = 0 ; 
int currentColorGreen = 0 ; 
int currentColorBlue = 0 ; 

void animFade() {

    
  unsigned long currentTimer = millis();

  if ( (currentTimer - pixelTimer) > 30  ) {

    pixelTimer = currentTimer ; 


    for (int i = 0 ; i < 12 ; i++) {
      strip.setPixelColor(i, currentColorRed/4, currentColorGreen, currentColorBlue/4);      
    }
    
    
    strip.show();      
    
    currentColorRed+=2 ; 
    currentColorGreen+=2 ; 
    currentColorBlue+=2 ; 
    
    if (currentColorRed>=255) currentColorRed = 0 ;    
    if (currentColorGreen>=200) currentColorGreen = 0 ;    
    if (currentColorBlue>=255) currentColorBlue = 0 ;    
    
  }  
  
}




void animRight() {


  unsigned long currentTimer = millis();

  if ( (currentTimer - pixelTimer) > 10  ) {

    pixelTimer = currentTimer ; 

    ringAllOff() ;         
    
    strip.setPixelColor(2, currentColorRed, currentColorRed/2, currentColorRed/3);      
    strip.setPixelColor(3, currentColorRed, currentColorRed/3, currentColorRed/2);      
    strip.setPixelColor(4, currentColorRed, currentColorRed/2, currentColorRed/3);      
    strip.setPixelColor(5, currentColorRed, currentColorRed/3, currentColorRed/2);      
    strip.setPixelColor(6, currentColorRed, currentColorRed/2, currentColorRed/3);    

    currentColorRed+=2 ; 
    if (currentColorRed>=155) currentColorRed = 0 ;    
      
    strip.show();      
    
  }  


}



void animLeft() {


  unsigned long currentTimer = millis();

  if ( (currentTimer - pixelTimer) > 10  ) {

    pixelTimer = currentTimer ; 

    ringAllOff() ;         
    
    strip.setPixelColor(8, currentColorRed, currentColorRed/2, currentColorRed/3);      
    strip.setPixelColor(9, currentColorRed, currentColorRed/3, currentColorRed/2);      
    strip.setPixelColor(10, currentColorRed, currentColorRed/2, currentColorRed/3);      
    strip.setPixelColor(11, currentColorRed, currentColorRed/3, currentColorRed/2);      
    strip.setPixelColor(0, currentColorRed, currentColorRed/2, currentColorRed/3);    

    currentColorRed+=2 ; 
    if (currentColorRed>=155) currentColorRed = 0 ;    
      
    strip.show();      
    
  }  


}

void animShoot() {
  

    for (int i = 0 ; i < 12 ; i++) {
      strip.setPixelColor(i, 250, 10, 10);      
    }    
    
    strip.show();     
}

void animateRing() {


    if (animMode == animIdle) animCircle(); 
    if (animMode == animTurnRight) animRight(); 
    if (animMode == animTurnLeft) animLeft(); 
    

    
}



void setup() {

  

  /// prepare led ring

  strip.begin();
  strip.show(); 
  strip.setBrightness(128);

  animateRing() ; 

  /// prepare ir sensors

  pinMode(irPinRight, INPUT);      
  pinMode(irPinLeft, INPUT);      


  

  /// setup servo motors

  lockServo.attach(3); 
  armServo.attach(5); 
  trayServo.attach(6);
  baseServo.attach(9) ;  


  /// basic setup 

  armServo.write(armArmed);
  trayServo.write(trayClosed);
  lockServo.write(lockOpen);
  delay(100);
  SoftwareServo::refresh();
  armServo.write(armOpen);
  delay(100); 
  SoftwareServo::refresh();


  /// test tray

  //trayRelease(); trayRelease(); trayRelease();trayRelease(); trayRelease(); trayRelease();

  

}

void loop() {

  animateRing() ; 


  unsigned long currentTimer = millis();


  int l ; int r ; 

  l = digitalRead(irPinLeft);   
  r = digitalRead(irPinRight);  

  if (r == 0 && l == 0) {

    if ( (currentTimer - lastTimer) > 250  ) {

      lastTimer = currentTimer ; 

      /// shoot!!!!
      animShoot(); 
    
      trayRelease(); 

      rest() ; 
      prepareToShoot() ;  
      shoot(); 
      rest(); 
      
    }

    
  } else if (r == 0 || l == 0 ) {


    if ( (currentTimer - lastTimer) > 25  ) { 

      lastTimer = currentTimer ; 

      if (r==0) {

        basePos-- ; 
        animMode = animTurnLeft ; 
        
      } else {

        basePos++ ; 
        animMode = animTurnRight ; 
        
      }
      
      
      baseServo.write(basePos); 
      
    }

    
  } else {

    animMode = animIdle ; 
    
  }

  
  
  SoftwareServo::refresh();
 

}