Furby from hell

Furby from hell

https://www.youtube.com/watch?v=XM-ecABZVGs

https://www.youtube.com/watch?v=XM-ecABZVGs

https://www.youtube.com/watch?v=ZD_SK4SKr8Y

https://www.youtube.com/watch?v=jXKLZpDmWiY

#include <TimerOne.h>
#define pwmRegister OCR1A    // the logical pin, can be set to OCR1B
const int ledIN = 8; // from furby leds to pin 8
const int outPin = 9; // output for arduino clock
const int B_led = 10; // RGB led B leg...
const int G_led = 11; // RGB led G leg...
const int R_led = 12; // RGB led R leg...
long period = 1000; // the period in microseconds
int duty = 512; // duty as a range from 0 to 1024, 512 is 50% duty cycle
int prescale[] = {0, 1, 8, 64, 256, 1024}; // the range of prescale values
int IRpin = 1; // analog pin for reading the IR sensor
float distances [20]; // array to store values for better sensor reading
float gooddistances = 0; // variable to store good values from sensor 
int gooddistancescount = 0; // variable for count good values

void setup() {
    //  Serial.begin(9600);        // start the serial port
    pinMode(outPin, OUTPUT); // define pin 9 as output
    pinMode(R_led, OUTPUT); // define pin 12 as output
    pinMode(G_led, OUTPUT); // define pin 11 as output
    pinMode(B_led, OUTPUT); // define pin 10 as output
    pinMode(ledIN, INPUT); // define pin 12 as input
    ledoff();
    delay(1000);
    Rled();
    delay(1000);
    Gled();
    delay(1000);
    Bled();
    delay(1000);
    ledoff();
    Timer1.initialize(period); // initialize timer1, 1000 microseconds
    Timer1.pwm(9, duty); // setup pwm on pin 9, 50% duty cycle
}

void loop() {
    for (int i = 0; i < 20; i++) {
        //get value from sharp sensor
        float volts = analogRead(IRpin)*0.0048828125; // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
        float distance = 65 * pow(volts, -1.10); // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
        distances [i] = distance;
        /*
          Serial.print("Distance ");
          Serial.print(i); 
          Serial.print(":");
          Serial.println(distances [i]);
         */
    }
    // To get better readings use only repeating ones
    for (int i = 0; i < 20; i++) {
        for (int j = 0; j < 20; j++)
            if (i != j) {
                if (distances[i] == distances[j]) {
                    gooddistances = gooddistances + distances[i];
                    gooddistancescount++;
                }
                /*
                              Serial.print("Distance i:");
                              Serial.println(distances[i]);
                              Serial.print("Distance j:");
                              Serial.println(distances[j]);    
                              Serial.print("Gooddistance:");
                              Serial.println(gooddistances);
                              Serial.print("Gooddistancecount:");
                              Serial.println(gooddistancescount);               
                 */            
               }
    }
    //if readings are good
    if (gooddistances / gooddistancescount > 10 && gooddistances / gooddistancescount < 90) {
        //  Serial.print("Distance:");  
        //  Serial.println(gooddistances/gooddistancescount);
        period = gooddistances / gooddistancescount - 10; // set furby clock depending on distance from sharp
        if (digitalRead(ledIN) == HIGH) //Turn on leds only if furby leds are on
        {
            if (period > 20 && period < 40) {
                Rled();
            }
            else if (period > 40 && period < 60) {
                Gled();
            }
            else if (period > 60 && period < 80) {
                Bled();
            }
        } else {
            ledoff(); //if furby led if off turn our leds off  
        }
        //  Serial.print("Setting period to ");
        //  Serial.println(period);
        Timer1.setPeriod(period);
        Timer1.setPwmDuty(outPin, duty); // don't change the duty cycle  
    }
    gooddistances = 0; //reset values
    gooddistancescount = 0; //reset values
}

//somefunction from TimerOne library

bool setPulseWidth(long microseconds) {
    bool ret = false;
    int prescaleValue = prescale[Timer1.clockSelectBits];
    // calculate time per tick in ns
    long precision = (F_CPU / 128000) * prescaleValue;
    period = precision * ICR1 / 1000; // period in microseconds
    if (microseconds < period) {
        duty = map(microseconds, 0, period, 0, 1024);
        if (duty < 1)
            duty = 1;
        if (microseconds > 0 && duty < RESOLUTION) {
            Timer1.pwm(outPin, duty);
            ret = true;
        }
    }
    return ret;
}
//Turn LEDs off

void ledoff() {
    digitalWrite(R_led, LOW);
    digitalWrite(G_led, LOW);
    digitalWrite(B_led, LOW);
}
//Turn only R LED  

void Rled() {
    digitalWrite(R_led, HIGH);
    digitalWrite(G_led, LOW);
    digitalWrite(B_led, LOW);
}
//Turn only G LED  

void Gled() {
    digitalWrite(R_led, LOW);
    digitalWrite(G_led, HIGH);
    digitalWrite(B_led, LOW);
}
//Turn only B LED  

void Bled() {
    digitalWrite(R_led, LOW);
    digitalWrite(G_led, LOW);
    digitalWrite(B_led, HIGH);
}