Ruch serva w trakcie działania programu.

[dusi007]

Witam !

Chce zrobić sortownik, bazujący na czujniku koloru, który polegałby na tym, że gdy kolor przedmiotu jest inny niż biały serwo zrzuca dany przedmiot z taśmociągu.

Objaśnienia:

Jest to czujnik koloru RGB TSC3200, poniższy kod wyświetla 3 składowe RGB, następnie wprowadzam 3 zmienne xx,yy,zz; są ta wartości dla których jest +- kolor biały (powinno być to 255,255,255,ale czujnik nie chce tak odczytać choćby nie wiem co). Pomysł na program generalnie polega na tym, że jeśli otrzyma te wartości(xx,yy,zz) z odchyłem do +-20 to Arduino zmienia stan pinu 3 na HIGH co aktywuje komendę "attachInterrupt" która każe zatrzymać pętle loop, przejść do funkcji "servo" , a ta wykonuje obrót serwa i ustawia wartość na pinie 3 na LOW, co zapętla cały proces.

Liczę na waszą pomoc, gdyż serwo nie wykuje obrotu a się "telepie" obroty prawo lewo o mniej więcej stopień góra dwa.

Myślę że może być to spowodowane tym, iż czujnik TSC3200 dokonuje bardzo dużo pomiarów ok. 6-8 na sek.

Liczę na waszą pomoc !

Pozdrawiam.

Oto kod programu:

#include <Servo.h> 
Servo myservo;  // create servo object to control a servo 
               // twelve servo objects can be created on most boards

int s0 = 7, s1 = 4, s2 = 5, s3 = 6; //piny modułu połączone z Arduino
int zatrzymanie = 3;                // pin służący do zatrzymania pętli loop funkcja attachInterrupt.
int out = 2;                      //pin 2 Arduino i OUT od czujnika
int flag = 0;
byte counter = 0;
byte countR = 0, countG = 0, countB = 0;
void setup()
{
 Serial.begin(9600);             //inicjalizacja monitora szeregowego
 pinMode(s0, OUTPUT);            //ustawienie pinów jako wyjścia
 pinMode(s1, OUTPUT);
 pinMode(s2, OUTPUT);
 pinMode(s3, OUTPUT);
 digitalWrite(zatrzymanie,LOW);
 myservo.attach(9);
attachInterrupt(digitalPinToInterrupt(3), servo, RISING);
}

void TCS()
{
 flag = 0;
 digitalWrite(s1, HIGH);
 digitalWrite(s0, HIGH);
 digitalWrite(s2, LOW);
 digitalWrite(s3, LOW);
 attachInterrupt(0, ISR_INTO, LOW);
 timer0_init();

}
void ISR_INTO()
{
 counter++;
}
void timer0_init(void)
{
 TCCR2A = 0x00;
 TCCR2B = 0x07; //częstotliwość zegara - 1024
 TCNT2 = 100;   //przepełnienie 10 ms
 TIMSK2 = 0x01; //pozwolenie na przerwanie
}
int i = 0;
ISR(TIMER2_OVF_vect)           //timer 2, 10ms przepełnienie 10ms. funkcja przerwanie wewnętrzengo przepełnienia
{
 TCNT2 = 100;
 flag++;
 if (flag == 1)                 //pobranie i wyświetlenie na ekranie składowych RGB
 {
   countR = counter;
   Serial.print("red=");
   Serial.print(countR );
   digitalWrite(s2, HIGH);
   digitalWrite(s3, HIGH);
 }
 else if (flag == 2)
 {
   countG = counter;
   Serial.print("green=");
   Serial.print(countG );
   digitalWrite(s2, LOW);
   digitalWrite(s3, HIGH);
 }
 else if (flag == 3)
 {
   countB = counter;
   Serial.print("blue=");
   Serial.print(countB);
   digitalWrite(s2, LOW);
   digitalWrite(s3, LOW);


int xx =95;
int yy = 180;
int zz = 54;

delay(1000);
if(countR>=(xx-20)&&countR<=(xx+20)&&countG>=(yy-20)&&countG<=(yy+20)&&countB>=(zz-20)&&countB<=(zz+20))
{
 Serial.println("---Bialy \n");
 delay(10);
 digitalWrite(zatrzymanie,LOW);
}
else
{
Serial.println("------INNY \n");
 digitalWrite(zatrzymanie,HIGH);

}

 }
 else if (flag == 4)
 {
   flag = 0;
 }
 counter = 0;
}
void loop()                    //główna pętla programu
{
 TCS();
 delay(500);
 while (1);

}
void servo()
{
 for(byte pos = 5; pos <= 170; pos += 5) // goes from 0 degrees to 170 degrees 
 {                                  // in steps of 1 degree 
   myservo.write(pos);              // tell servo to go to position in variable 'pos' 
   delay(7);                       // waits 15ms for the servo to reach the position 
 } 
 for(byte pos = 170; pos>=5; pos-=5)     // goes from 170 degrees to 0 degrees 
 {                                
   myservo.write(pos);              // tell servo to go to position in variable 'pos' 
   delay(7);
 }
 digitalWrite(zatrzymanie,LOW);
}

[deshipu]

Olaboga! A po co to przerwanie tam? Nie możesz po prostu zawołać funkcji sterującej serwomechanizmem od razu z głównej pętli? Po prostu ustawiaj jakąś globalną flagę.

Druga sprawa, jeśli twoje serwo nie ma naddźwiękowego turbo dopalania, to raczej nie poruszy się 8 razy na sekundę za daleko. Może przetestuj na wolniejszej prędkości najpierw?

[dusi007]

Przerwanie dodałem ponieważ dla zwykłego wywołania funkcji serwo również się "telepało".

Czy patrząc na fragment kodu odpowiadający za odbieranie 3 składowych od TSC3200, możesz powiedzieć co zrobić, gdzie na przykład dodać delay, aby czujnik wykonywał 1 czy 2 odczyty na sek. a nie 8 ? może zmniejszyć wypełnienie? Ten fragment kodu nie jest mój niestety, lecz ze strony botland.

[deshipu]

Proponuję całość przepisać bez żadnych przerwań, na początek po prostu z delay-ami, wszystko w pętli głównej. Zrobienie odczytu, sprawdzenie, ewentualne wyslanie do serwa komendy, poczekanie na nastepny pomiar, powrot serwa i od nowa. Nie powinno być bardzo skomplikowane i znacznie prościej się powinno debugować (możesz powstawiać Serial.print wszędzie). No i wykonuje się jedna rzecz na raz, więc się nie musisz zastanawiać co się tak naprawdę dzieje.