Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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;
- 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(100);
- return TCSMeasure();
- }
- uint32_t checkBlue() {
- PORTC &= ~(1 << PC4); // S2 - L
- PORTC |= (1 << PC5); // S3 - H
- _delay_ms(100);
- return TCSMeasure();
- }
- uint32_t checkGreen() {
- PORTC |= (1 << PC4); // S2 - H
- PORTC |= (1 << PC5); // S3 - H
- _delay_ms(100);
- return TCSMeasure();
- }
- uint32_t checkClear(){
- PORTC |= (1<<PC4);
- PORTC &= ~(1<<PC5);
- _delay_ms(100);
- return TCSMeasure();
- }
- int pickColor(uint32_t R, uint32_t G, uint32_t B, uint32_t C) {
- int CL_PURPLE_MISTAKE = 0;
- int CL_YELLOW_MISTAKE = 0;
- int CL_GREEN_MISTAKE = 0;
- int CL_ORANGE_MISTAKE = 0;
- int CL_RED_MISTAKE = 0;
- //---PURPLE---
- if (CL_PURPLE_R > R) CL_PURPLE_MISTAKE += CL_PURPLE_R - R;
- else if (CL_PURPLE_R <= R) CL_PURPLE_MISTAKE += R - CL_PURPLE_R;
- if (CL_PURPLE_G > G) CL_PURPLE_MISTAKE += CL_PURPLE_G - G;
- else if (CL_PURPLE_G <= G) CL_PURPLE_MISTAKE += G - CL_PURPLE_G;
- if (CL_PURPLE_B > B) CL_PURPLE_MISTAKE += CL_PURPLE_B - B;
- else if (CL_PURPLE_B <= B) CL_PURPLE_MISTAKE += B - CL_PURPLE_B;
- if (CL_PURPLE_C > C) CL_PURPLE_MISTAKE += CL_PURPLE_C - C;
- else if (CL_PURPLE_C <= C) CL_PURPLE_MISTAKE += C - CL_PURPLE_C;
- //---YELLOW
- if (CL_YELLOW_R > R) CL_YELLOW_MISTAKE += CL_YELLOW_R - R;
- else if (CL_YELLOW_R <= R) CL_YELLOW_MISTAKE += R - CL_YELLOW_R;
- if (CL_YELLOW_G > G) CL_YELLOW_MISTAKE += CL_YELLOW_G - G;
- else if (CL_YELLOW_G <= G) CL_YELLOW_MISTAKE += G - CL_YELLOW_G;
- if (CL_YELLOW_B > B) CL_YELLOW_MISTAKE += CL_YELLOW_B - B;
- else if (CL_YELLOW_B <= B) CL_YELLOW_MISTAKE += B - CL_YELLOW_B;
- if (CL_YELLOW_C > C) CL_YELLOW_MISTAKE += CL_YELLOW_C - C;
- else if (CL_YELLOW_C <= C) CL_YELLOW_MISTAKE += C - CL_YELLOW_C;
- //---GREEN
- if (CL_GREEN_R > R) CL_GREEN_MISTAKE += CL_GREEN_R - R;
- else if (CL_GREEN_R <= R) CL_GREEN_MISTAKE += R - CL_GREEN_R;
- if (CL_GREEN_G > G) CL_GREEN_MISTAKE += CL_GREEN_G - G;
- else if (CL_GREEN_G <= G) CL_GREEN_MISTAKE += G - CL_GREEN_G;
- if (CL_GREEN_B > B) CL_GREEN_MISTAKE += CL_GREEN_B - B;
- else if (CL_GREEN_B <= B) CL_GREEN_MISTAKE += B - CL_GREEN_B;
- if (CL_GREEN_C > C) CL_GREEN_MISTAKE += CL_GREEN_C - C;
- else if (CL_GREEN_C <= C) CL_GREEN_MISTAKE += C - CL_GREEN_C;
- //---ORANGE
- if (CL_ORANGE_R > R) CL_ORANGE_MISTAKE += CL_ORANGE_R - R;
- else if (CL_ORANGE_R <= R) CL_ORANGE_MISTAKE += R - CL_ORANGE_R;
- if (CL_ORANGE_G > G) CL_ORANGE_MISTAKE += CL_ORANGE_G - G;
- else if (CL_ORANGE_G <= G) CL_ORANGE_MISTAKE += G - CL_ORANGE_G;
- if (CL_ORANGE_B > B) CL_ORANGE_MISTAKE += CL_ORANGE_B - B;
- else if (CL_ORANGE_B <= B) CL_ORANGE_MISTAKE += B - CL_ORANGE_B;
- if (CL_ORANGE_C > C) CL_ORANGE_MISTAKE += CL_ORANGE_C - C;
- else if (CL_ORANGE_C <= C) CL_ORANGE_MISTAKE += C - CL_ORANGE_C;
- //---RED
- if (CL_RED_R > R) CL_RED_MISTAKE += CL_RED_R - R;
- else if (CL_RED_R <= R) CL_RED_MISTAKE += R - CL_RED_R;
- if (CL_RED_G > G) CL_RED_MISTAKE += CL_RED_G - G;
- else if (CL_RED_G <= G) CL_RED_MISTAKE += G - CL_RED_G;
- if (CL_RED_B > B) CL_RED_MISTAKE += CL_RED_B - B;
- else if (CL_RED_B <= B) CL_RED_MISTAKE += B - CL_RED_B;
- if (CL_RED_C > C) CL_RED_MISTAKE += CL_RED_C - C;
- else if (CL_RED_C <= C) CL_RED_MISTAKE += C - CL_RED_C;
- int minimumMistake = 1000;
- int closestColor = 0;
- if (CL_PURPLE_MISTAKE < minimumMistake) {
- minimumMistake = CL_PURPLE_MISTAKE;
- closestColor = CL_PURPLE;
- }
- if (CL_YELLOW_MISTAKE < minimumMistake) {
- minimumMistake = CL_YELLOW_MISTAKE;
- closestColor = CL_YELLOW;
- }
- if (CL_GREEN_MISTAKE < minimumMistake) {
- minimumMistake = CL_GREEN_MISTAKE;
- closestColor = CL_GREEN;
- }
- if (CL_ORANGE_MISTAKE < minimumMistake) {
- minimumMistake = CL_ORANGE_MISTAKE;
- closestColor = CL_ORANGE;
- }
- if (CL_RED_MISTAKE < minimumMistake) {
- minimumMistake = CL_RED_MISTAKE;
- closestColor = CL_RED;
- }
- return closestColor;
- }
- 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;
- for (i = 0 ; i < 5 ; i++) G += checkGreen()/100;
- for (i = 0 ; i < 5 ; i++) B += checkBlue()/100;
- for (i = 0 ; i < 5 ; i++) 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 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);
- }
- 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;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement