CS_przemo

#include <avr/io.h>
#include <avr/delay.h>
#include <avr/interrupt.h>
#include "tcs3200.h"

#define CL_PURPLE     0x701
#define CL_YELLOW     0x702
#define CL_GREEN      0x703
#define CL_ORANGE     0x704
#define CL_RED        0x705

#define POS_COLLECT   0x801
#define POS_SENSOR    0x802
#define POS_DROP      0x803

#define SRV_STOP      0x555

#define ERR_NO_CANDY  0x999

#define CL_PURPLE_R   102
#define CL_PURPLE_G   81
#define CL_PURPLE_B   103
#define CL_PURPLE_C   305

#define CL_YELLOW_R   139
#define CL_YELLOW_G   110
#define CL_YELLOW_B   115
#define CL_YELLOW_C   384

#define CL_GREEN_R    108
#define CL_GREEN_G    97
#define CL_GREEN_B    106
#define CL_GREEN_C    331

#define CL_ORANGE_R   130
#define CL_ORANGE_G   91
#define CL_ORANGE_B   108
#define CL_ORANGE_C   348

#define CL_RED_R      113
#define CL_RED_G      82
#define CL_RED_B      103
#define CL_RED_C      317


volatile int measureReady;
volatile int overflowTicks;
volatile int actuallArmPosition;

volatile int CL_PURPLE_VALUE_R;
volatile int CL_PURPLE_VALUE_G;
volatile int CL_PURPLE_VALUE_B;
volatile int CL_PURPLE_VALUE_C;

volatile int CL_YELLOW_VALUE_R;
volatile int CL_YELLOW_VALUE_G;
volatile int CL_YELLOW_VALUE_B;
volatile int CL_YELLOW_VALUE_C;

volatile int CL_GREEN_VALUE_R;
volatile int CL_GREEN_VALUE_G;
volatile int CL_GREEN_VALUE_B;
volatile int CL_GREEN_VALUE_C;

volatile int CL_ORANGE_VALUE_R;
volatile int CL_ORANGE_VALUE_G;
volatile int CL_ORANGE_VALUE_B;
volatile int CL_ORANGE_VALUE_C;

volatile int CL_RED_VALUE_R;
volatile int CL_RED_VALUE_G;
volatile int CL_RED_VALUE_B;
volatile int CL_RED_VALUE_C;

void moveArm (int colorID) {
    DDRB |= (1 << PB1);     // PWM Output
    TCCR1A = 0;
    ICR1 = 19999;           // 20ms pulse width - 50Hz

    TCCR1A = (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);

    TCCR1A |= 2 << 6;       // PB1 as PWM Output

    actuallArmPosition = colorID;

    switch (colorID) {
        case CL_PURPLE:
            OCR1A = 600;        // PURPLE
            break;

        case CL_YELLOW:
            OCR1A = 1050;       // YELLOW
            break;

        case CL_GREEN:
            OCR1A = 1500;       // GREEN
            break;

        case CL_ORANGE:
            OCR1A = 1950;       // ORANGE
            break;

        case CL_RED:
            OCR1A = 2400;       // RED
            break;

        case SRV_STOP:          // SERVO STOP
            OCR1A = 0;
            break;
    }
}

void spinWheel (int moveTo) {
    DDRB |= (1 << PB2);
    TCCR1A = 0;
    ICR1 = 19999;

    TCCR1A = (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);

    TCCR1A |= 2 << 4;

    switch (moveTo) {
        case POS_COLLECT:       // POSITION 1
            OCR1B = 2350;
            break;

        case POS_SENSOR:        // POSITION 2
            OCR1B = 1420;
            break;

        case POS_DROP:          // POSITION 3
            OCR1B = 600;
            break;

        case SRV_STOP:          // SERVO STOP
            OCR1B = 0;
            break;
    }
}

void collectCandy() {
    spinWheel(POS_COLLECT);
    _delay_ms(1500);
    spinWheel(SRV_STOP);
}

uint32_t checkRed() {
    PORTC &= ~(1 << PC4);   // S2 - L
    PORTC &= ~(1 << PC5);   // S3 - L
    _delay_ms(10);
    return TCSMeasure();
}


uint32_t checkBlue() {
    PORTC &= ~(1 << PC4);   // S2 - L
    PORTC |= (1 << PC5);    // S3 - H
    _delay_ms(10);
    return TCSMeasure();
}

uint32_t checkGreen() {
    PORTC |= (1 << PC4);    // S2 - H
    PORTC |= (1 << PC5);    // S3 - H
    _delay_ms(10);
    return TCSMeasure();
}

uint32_t checkClear(){
    PORTC |= (1<<PC4);
    PORTC &= ~(1<<PC5);
    _delay_ms(10);
    return TCSMeasure();
}

int dif(uint32_t a,uint32_t b) {
    if(a>b) {
        return a - b;
    }
    return b-a;
}
int pickColor(uint32_t R, uint32_t G, uint32_t B, uint32_t C) {
    uint32_t Rdif=0;
    uint32_t Gdif=0;
    uint32_t Pdif=0;
    uint32_t Odif=0;
    uint32_t Ydif=0;
    int kt = CL_RED;

    Rdif=dif(CL_RED_VALUE_R,R)+dif(CL_RED_VALUE_G,G)+dif(CL_RED_VALUE_B,B);
    Gdif=dif(CL_GREEN_VALUE_R,R)+dif(CL_GREEN_VALUE_G,G)+dif(CL_GREEN_VALUE_B,B);
    Pdif=dif(CL_PURPLE_VALUE_R,R)+dif(CL_PURPLE_VALUE_G,G)+dif(CL_PURPLE_VALUE_B,B);
    Odif=dif(CL_ORANGE_VALUE_R,R)+dif(CL_ORANGE_VALUE_G,G)+dif(CL_ORANGE_VALUE_B,B);
    Ydif=dif(CL_YELLOW_VALUE_R,R)+dif(CL_YELLOW_VALUE_G,G)+dif(CL_YELLOW_VALUE_B,B);

    //w Rdif jest minimum
    if(Rdif>Gdif) {
        Rdif = Gdif;
        kt =  CL_GREEN;
    }
    if(Rdif>Pdif) {
        Rdif = Pdif;
        kt = CL_PURPLE;
    }
    if(Rdif>Odif) {
        Rdif = Odif;
        kt = CL_ORANGE;
    }
    if(Rdif>Ydif){
        Rdif = Ydif;
        kt = CL_YELLOW;
    }
    return kt;
}
void checkColor() {
    spinWheel(POS_SENSOR);
    _delay_ms(700);

    //spinWheel(SRV_STOP);
    //int nextPosition;
    uint32_t R=0;
    uint32_t G=0;
    uint32_t B=0;
    uint32_t C=0;

    int i;

    for(i = 0 ; i < 5 ; i++) {
        R += checkRed()/100;
        G += checkGreen()/100;
        B += checkBlue()/100;
        C += checkClear()/100;
    }

    R = R/5;
    G = G/5;
    B = B/5;
    C = C/5;

    int nextPosition = pickColor(R, G, B, C);

    if (nextPosition != ERR_NO_CANDY) {
        moveArm(nextPosition);
        _delay_ms(1000);
        moveArm(SRV_STOP);
    }
}


void dropCandy() {
    spinWheel(POS_DROP);
    _delay_ms(700);
}

void prepareColors() {
    int i;

    for(i = 0 ; i < 5 ; i++) {
        collectCandy();
        spinWheel(POS_SENSOR);
        _delay_ms(700);

        uint32_t R = 0;
        uint32_t G = 0;
        uint32_t B = 0;
        uint32_t C = 0;

        int j;

        for(j = 0 ; j < 5 ; j++) {
            R += checkRed()/100;
            G += checkGreen()/100;
            B += checkBlue()/100;
            C += checkClear()/100;
        }

        R = R/5;
        G = G/5;
        B = B/5;
        C = C/5;

        if (i == 0) {
            moveArm(CL_PURPLE);
            _delay_ms(900);
            moveArm(SRV_STOP);

            CL_PURPLE_VALUE_R = R;
            CL_PURPLE_VALUE_G = G;
            CL_PURPLE_VALUE_B = B;
            CL_PURPLE_VALUE_C = C;
        }

        if (i == 1) {
            moveArm(CL_YELLOW);
            _delay_ms(900);
            moveArm(SRV_STOP);

            CL_YELLOW_VALUE_R = R;
            CL_YELLOW_VALUE_G = G;
            CL_YELLOW_VALUE_B = B;
            CL_YELLOW_VALUE_C = C;
        }

        if (i == 2) {
            moveArm(CL_GREEN);
            _delay_ms(900);
            moveArm(SRV_STOP);

            CL_GREEN_VALUE_R = R;
            CL_GREEN_VALUE_G = G;
            CL_GREEN_VALUE_B = B;
            CL_GREEN_VALUE_C = C;
        }

        if (i == 3) {
            moveArm(CL_ORANGE);
            _delay_ms(900);
            moveArm(SRV_STOP);

            CL_ORANGE_VALUE_R = R;
            CL_ORANGE_VALUE_G = G;
            CL_ORANGE_VALUE_B = B;
            CL_ORANGE_VALUE_C = C;
        }

        if (i == 4) {
            moveArm(CL_RED);
            _delay_ms(900);
            moveArm(SRV_STOP);

            CL_RED_VALUE_R = R;
            CL_RED_VALUE_G = G;
            CL_RED_VALUE_B = B;
            CL_RED_VALUE_C = C;
        }
        dropCandy();
    }
}

void moveServo(int pulseWidth) {
    TCCR1A = 0;
    ICR1 = 19999;

    TCCR1A = (1 << WGM11);
    TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11);

    TCCR1A |= 2 << 6;

    OCR1A = pulseWidth;
}

void MAIN_INIT() {
    DDRC |= (1 << PC5) | (1 << PC4) | (1 << PC3) | (1 << PC2) | (1 << PC1);
    PORTC |= (1 << PC3);

    moveArm(CL_GREEN);
    _delay_ms(1000);
    moveArm(SRV_STOP);

    spinWheel(POS_COLLECT);
    _delay_ms(1000);
    spinWheel(SRV_STOP);

    prepareColors();
}

void MAIN_LOOP() {
    while (1) {
        collectCandy();
        checkColor();
        dropCandy();
    }
}

int main() {

    moveArm(CL_RED);
    _delay_ms(1000);
    moveArm(SRV_STOP);

    MAIN_INIT();
    MAIN_LOOP();

    return 0;
}