Prohodit ty cykly, pripadne prochazet vetsimi kroky (a trochu ucesat ten kod, hlavne to odsazovani)
Asi nejak takto (ale pisu to z hlavy, bez testovani, muzou tam byt chyby)
Kód: Vybrat vše
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver driverPCA = Adafruit_PWMServoDriver(0x40);
#define tl1 11
#define tl2 12
const int SERV=6;
const int POCET=20;
int m[POCET][SERV]; // lepsi je mit souvisejici pozice serv ulozene u sebe, nez rozhazene po celem poli
int pos[SERV] ; // pozice serv
int pot[SERV]={A0,A1,A2,A3,A6,A7}; // piny pro potenciometry
int PWM_pin[SERV]={0,1,2,3,4,5}; // piny pro serva
// pocet kroku, ktere chceme udelat mezi starou a novou pozici
#define STEPS 20
// pocet milisekund, kolik cekame mezi jednotlivymi kroky - STEPS * CEKANI je doba presunu mezi jednotlivymi body (napriklad 20*50 = 1 sec)
#define CEKANI 50
int posledniPoloha=0; // p a i jsou lokalni promenne ruznych cyklu a deklaruji se az v nich
// posledni Poloha je globalni a ma od zacatku nejakou hodnotu (protoze co kdyz uzivatel zakerne stiskne tlacitko pro autoposun driv, nez neco ulozi)
// (sice nulu nemusime jako pocatecni hodnotu uvadet, ale je to prehlednejsi a pokud nemusime setrit kazdym bytem ...)
void setup() {
driverPCA.begin();
driverPCA.setPWMFreq(60);
pinMode(tl1,INPUT);
pinMode(tl2,INPUT);
}
void loop() {
if ( digitalRead(tl1)==HIGH ){ // ulozit pozici (tlacitko 1)
ulozPozici();
delay(50); // debounce tlacitka
while ( digitalRead(tl1)==HIGH ) {}; // pockat, dokud nepusti tlacitko
delay(50); // debounce pusteni tlacitka
} // end ulozeni pozice (tlacitko 1)
if( digitalRead(tl2)==HIGH ){ // automaticky pohyb (tlacitko 2)
ulozPozici(); // po skonceni se vratime tam, kde jsme zacali, at nasledujici manualServo nam necukne vsema servama,
// pokud jsme odjeli z posledniho ulozeneho bodu
for (int p=0; p < posledniPoloha; p++){
autoServo(p);
}
posledniPoloha--; // uz jsme zpatky, takze tu polohu muzeme zase zapomenout
} // konec automatickeho pohybu
else { // manualni pohyb
manualServo();
} // konec tlacitka 2
// pokud treti tlacitko neni RESET a/nebo nechceme cukat servama
// if ( digitalRead(tl3)==HIGH ){ // vymazat pozice
// posledniPoloha = 0; // resetovat index posledni Pozice
// delay(50); // debounce tlacitka
// while ( digitalRead(tl3)==HIGH ) {}; // pockat, dokud nepusti tlacitko
// delay(50); // debounce pusteni tlacitka
// } // end vymazani pozic
}
void ulozPozici() { // prida aktualni pozici na konec pole
// globalni pole m, pos a promenna posledniPoloha
if (posledniPoloha >= POCET) { // jsme na konci pole, nechceme pretect
posledniPoloha=POCET-1; // zapomeneme posledni bod (ci body, ale to by se nam nemelo stat) a ted uz mame kam ukladat
} // end testovani preteceni
for (int i=0; i<SERV; i++){
m[posledniPoloha][i]=pos[i];
} // end for i
posledniPoloha++; // zvednout index posledni Pozice
} // end ulozPozici
void manualServo(){
// globalni pole pos[?]
for(int i=0; i<SERV; i++) {
pos[i] = analogRead(pot[i]);
driverPCA.setPWM(PWM_pin[i], 0, map(pos[i], 0, 1023, 100, 600););
} // end for i
} // end manualServo
void autoServo(int p){ // p je index pozadovane pozice
// globalni pole m[p][?] obsahuje cilove pozice pro jednotliva serva
// globalni pole pos[?] obsahuje vychozi pozice jednotlivych serv
for (int st=1; st<STEPS+1; st++) { // v bode nula uz jsem, potrebuju skoncit v bode STEPS, ne o krok driv, cyklus 1..STEPS
for (int i=0; i<SERV; i++) { // pro kazde servo ...
// int x = ((pos[i]*(STEPS-st)/STEPS) + (m[i][p]*st/STEPS)) ; // moc hezky, ale pro velke STEPS muze pretect, radeji to opatrne rozepiseme
long int x = (long)pos[i] * (STEPS-st) + (long)m[i][p] * st; // porad bychom meli pocitat s long cisly
x = x / STEPS; // a ted to podelime. Staci nam jedna promenna, protoze ji hned pouzijeme a pak uz dal nepotrebujeme
driverPCA.setPWM(PWM_pin[i], 0, map(x, 0, 1023, 100, 600));
} // end for i
delay(CEKANI); // nejdriv nastavime naraz pohyb vsemi servy o jeden krok, pak teprve pockame, az ten krok udelaji (spolecne)
} // end for st
for (int i=0; i<SERV; i++) { // pro kazde servo ...
pos[i] = m[i][p]; // ... nastavime stavajici pozici na cilovy bod
}
} // end autoServo
Plus by samozrejme slo obejit delay a behem ni delat neco uzitecneho (cist tlacitka, blikat, ... ), ale tim by se ten program ponekud zkomplikoval (v podstate bychom pak potrebovali stavovy automat)