Untitled
unknown
plain_text
2 years ago
9.4 kB
2
Indexable
#include <Process.h> #include <string.h> #define S2 27 #define S3 17 #define sensorOut 22 int frequency=0; int culoare1[]={0,0,0,0,0,0}; int culoare2[]={0,0,0,0,0,0}; int culoare3[]={0,0,0,0,0,0}; int culoare4[]={0,0,0,0,0,0}; int cFinal=0; String pinUp="5 "; String pinDown="6 "; String lastAngleUp="1.85"; String newAngleUp="0 "; String newAngleDown="0 "; String cale="python servoControl.py "; String command=""; Process p; void setup() { pinMode(S2,OUTPUT); pinMode(S3,OUTPUT); pinMode(sensorOut,INPUT); initialdetect(); } void loop() { command=""; newAngleUp="0.65 "; command =cale; command +=pinUp; command +=newAngleUp; command +=lastAngleUp; p.runShellCommand(command); while(p.running()); lastAngleUp=newAngleUp; cFinal = readColor(); delay(10); switch (cFinal) { case 1: command=""; newAngleDown="0.2 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; case 2: command=""; newAngleDown="0.8 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; case 3: command=""; newAngleDown="1.5 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; case 4: command=""; newAngleDown="2 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; case 0: break; } delay(300); //daca readColor a returnat o culoare se va misca servo-ul if(cFinal!=0){ //Mutare bomboana spre fanta coborare command=""; newAngleUp="0.1 "; command =cale; command +=pinUp; command +=newAngleUp; command +=newAngleUp; p.runShellCommand(command); while(p.running()); lastAngleUp=newAngleUp; //Revenire servo in pozitie initiala command=""; newAngleUp="1.85 "; command =cale; command +=pinUp; command +=newAngleUp; command +=lastAngleUp; p.runShellCommand(command); while(p.running()); lastAngleUp=newAngleUp; } delay(300); cFinal=0; } //Functie pentru detectarea initiala a valorilor RGB a bomboanelor //Primele 4 bomboane introduse trebuie sa fie de culori diferite pentru functionarea corespunzatoare void initialdetect(){ for(int i=1;i<=4;i++) { command=""; newAngleUp="0.65 "; command =cale; command +=pinUp; command +=newAngleUp; command +=lastAngleUp; p.runShellCommand(command); while(p.running()); lastAngleUp=newAngleUp; int lowVal[]={0,0,0}; int highVal[]={0,0,0}; for(int j=1;j<=5;j++){ int r=readRed(); int g=readGreen(); int b=readBlue(); if(j==1){ lowVal[0]=r; lowVal[1]=g; lowVal[2]=b; highVal[0]=r; highVal[1]=g; highVal[2]=b; } else{ if(r<lowVal[0]) lowVal[0]=r; if(r>highVal[0]) highVal[0]=r; if(g<lowVal[1]) lowVal[1]=g; if(g>highVal[1]) highVal[1]=g; if(b<lowVal[2]) lowVal[2]=b; if(b>highVal[b]) highVal[b]=b; } delay(500); } switch(i){ case 1: culoare1[0]=lowVal[0]-1; culoare1[1]=lowVal[1]-1; culoare1[2]=lowVal[2]-1; culoare1[3]=highVal[0]+1; culoare1[4]=highVal[1]+1; culoare1[5]=highVal[2]+1; command=""; newAngleDown="0.2 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; case 2: culoare2[0]=lowVal[0]-1; culoare2[1]=lowVal[1]-1; culoare2[2]=lowVal[2]-1; culoare2[3]=highVal[0]+1; culoare2[4]=highVal[1]+1; culoare2[5]=highVal[2]+1; command=""; newAngleDown="0.8 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; case 3: culoare3[0]=lowVal[0]-1; culoare3[1]=lowVal[1]-1; culoare3[2]=lowVal[2]-1; culoare3[3]=highVal[0]+1; culoare3[4]=highVal[1]+1; culoare3[5]=highVal[2]+1; command=""; newAngleDown="1.5 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; case 4: culoare4[0]=lowVal[0]-1; culoare4[1]=lowVal[1]-1; culoare4[2]=lowVal[2]-1; culoare4[3]=highVal[0]+1; culoare4[4]=highVal[1]+1; culoare4[5]=highVal[2]+1; command=""; newAngleDown="2 "; command =cale; command +=pinDown; command +=newAngleDown; command +=newAngleDown; p.runShellCommand(command); while(p.running()); break; } command=""; newAngleUp="0.1 "; command =cale; command +=pinUp; command +=newAngleUp; command +=newAngleUp; p.runShellCommand(command); while(p.running()); lastAngleUp=newAngleUp; //Revenire servo in pozitie initiala command=""; newAngleUp="1.85 "; command =cale; command +=pinUp; command +=newAngleUp; command +=lastAngleUp; p.runShellCommand(command); while(p.running()); lastAngleUp=newAngleUp; delay(300); } } //functie detectie culoare int readColor(){ int r=readRed(); int g=readGreen(); int b=readBlue(); if(r>=culoare1[0] && r<=culoare1[3]) if(g>=culoare1[1] && g<=culoare1[4]) if(b>=culoare1[2] && b<=culoare1[5]) return 1; if(r>=culoare2[0] && r<=culoare2[3]) if(g>=culoare2[1] && g<=culoare2[4]) if(b>=culoare2[2] && b<=culoare2[5]) return 2; if(r>=culoare3[0] && r<=culoare3[3]) if(g>=culoare3[1] && g<=culoare3[4]) if(b>=culoare3[2] && b<=culoare3[5]) return 3; if(r>=culoare4[0] && r<=culoare4[3]) if(g>=culoare4[1] && g<=culoare4[4]) if(b>=culoare4[2] && b<=culoare4[5]) return 4; return 0; } // Citire Culoare Rosu - returneaza valoarea R din RGB int readRed() { //Setare S2 si S3 pt culoarea rosu digitalWrite(S2, LOW); digitalWrite(S3, LOW); //Citire frecventa frequency = pulseIn(sensorOut, LOW); int R = frequency; //Output Consola Console.print("R= "); Console.print(R); Console.print(" "); delay(50); return R; } // Citire Culoare Verde - returneaza valoarea G din RGB int readGreen(){ //Setare S2 si S3 pt culoarea verde digitalWrite(S2, HIGH); digitalWrite(S3, HIGH); //Citire frecventa frequency = pulseIn(sensorOut, LOW); int G = frequency; // Output Consola Console.print("G= "); Console.print(frequency); Console.print(" "); delay(50); return G; } // Citire Culoare Albastru - returneaza valoarea B din RGB int readBlue(){ //Setare S2 si S3 pt culoarea albastru digitalWrite(S2, LOW); digitalWrite(S3, HIGH); //Citire frecventa frequency = pulseIn(sensorOut, LOW); int B = frequency; //Output Consola Console.print("B= "); Console.print(frequency); Console.println(" "); delay(50); return B; }