Zpomalení serv s použitím driveru PCA9685

Dark
Příspěvky: 8
Registrován: 21 pro 2019, 21:37
Reputation: 0

Zpomalení serv s použitím driveru PCA9685

Příspěvek od Dark » 21 pro 2019, 21:43

Zdravím,
používám driver PCA9685 kterým řídím 6 serv, ale nevím jak je zpomalit při přejezdu z polohy A do polohy B (používám serva MG946R, které napájím externě 7,2V a při tomto napětí se otáčí rychlost cca 60 stupňů/0,15s, což je pro můj projekt příliš rychlé. Jedná se o robotické rameno, kterým pohybuje 6 serv (základna, rameno, předloktí...) a ty jsou ovládány potenciometry, ze kterých je sestaven model tohoto ramene v měřítku 1/2 a ten funguje jako ovladač, něco na principu "voodoo panenky" (co udělá ovladač, tak ihned napodobí i rameno). Všechno to řídím deskou Arduino NANO. Rameno umí ještě nejen jezdit přesně podle ovladače, ale i samo automatický jezdit postupně mezi pozicemi uloženými pomoci tlačítka (ovladačem nastavím pozici, zmáčknu tlačítko a pozice všech 6 serv se uloží do paměti, těchto pozic si to je schopné zapamatovat až 20). Pak je tam druhé tlačítko s aretací, které přepíná z manuálního režimu do automatického, ve kterém rameno jezdí po sepnutí neustále mezi uloženými pozicemi (pohybuje se všech 6 serv zároveň), dokud toto tlačítko opět nevypnu (když ho vypnu, přepne se opět do manuálního režimu) a tu právě nastává ten problém, protože serva se pohybuji mezi těmito pozicemi v automatickém režimu maximální možnou rychlosti a to by znamenalo jistou zkázu, protože rameno měří cca 50cm. (V manuálním režimu není třeba rychlost omezovat, protože je řízena ovladačem). Třetí tlačítko je pouze hardwarový reset, který resetuje celé Arduino a tím smaže i pozice z paměti. Program, který přikládám mi funguje bez problému, až na snížení rychlostí těchto serv. Vím, že je to potřeba řešit přes funkci millis(), ale nevím jak to do programu implementovat, vyzkoušel jsem už tolik možností, ale vždy se v něčem objevil problém, na internetu jsem našel články jak pro blikání led bez delay, tak i dokonce pro sweep servo bez delay (bez driveru PCA9685), problém je že používám driver PCA9685 a ještě k tomu ukládám spoustu hodnot z potenciometru do proměnných (hodnota je někdy větší, někdy menší, nejedná se o hodnoty pouze rostoucí nebo klesající. Budu nesmírně rád a vděčný za jakýkoliv nápad nebo radu.

Kód: Vybrat vše

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

Adafruit_PWMServoDriver driverPCA = Adafruit_PWMServoDriver(0x40);

#define pot1 A0
#define pot2 A1
#define pot3 A2
#define pot4 A3
#define pot5 A6
#define pot6 A7

#define tl1 11
#define tl2 12

const int POCET=20;
int posledniPoloha, i=0;

int m1[POCET], m2[POCET], m3[POCET], m4[POCET], m5[POCET], m6[POCET];
int pos1, pos2, pos3, pos4, pos5, pos6;

void setup() {
  driverPCA.begin();
  driverPCA.setPWMFreq(60);

  pinMode(tl1,INPUT);
  pinMode(tl2,INPUT);
}

void loop() {
if(digitalRead(tl1)==HIGH){
m1[i]=pos1;
m2[i]=pos2;
m3[i]=pos3;
m4[i]=pos4;
m5[i]=pos5;
m6[i]=pos6;

i++;
delay(500);
}

if(digitalRead(tl2)==HIGH){
  i=0;
  while(i<posledniPoloha){
autoServo1();
autoServo2();
autoServo3();
autoServo4();
autoServo5();
autoServo6();

i++;
delay(500);
  }
}
 else
 { 
manualServo1();
manualServo2();
manualServo3();
manualServo4();
manualServo5();
manualServo6();

posledniPoloha=i;
 }
}

void manualServo1(){
pos1 = analogRead(pot1);
int pulse1 = map(pos1, 0, 1023, 100, 600);
driverPCA.setPWM(0, 0, pulse1);
}

void manualServo2(){
 pos2 = analogRead(pot2);
int pulse2 = map(pos2, 0, 1023, 100, 600);
driverPCA.setPWM(1, 0, pulse2); 
}

void manualServo3(){
pos3 = analogRead(pot3);
int pulse3 = map(pos3, 0, 1023, 100, 600);
driverPCA.setPWM(2, 0, pulse3);
}

void manualServo4(){
pos4 = analogRead(pot4);
int pulse4 = map(pos4, 0, 1023, 100, 600);
driverPCA.setPWM(3, 0, pulse4);
}

void manualServo5(){
pos5 = analogRead(pot5);
int pulse5 = map(pos5, 0, 1023, 100, 600);
driverPCA.setPWM(4, 0, pulse5);
}

void manualServo6(){
pos6 = analogRead(pot6);
int pulse6 = map(pos6, 0, 1023, 100, 600);
driverPCA.setPWM(5, 0, pulse6);
}

void autoServo1(){
int pulse1 = map(m1[i], 0, 1023, 100, 600);
driverPCA.setPWM(0, 0, pulse1);
}

void autoServo2(){
int pulse2 = map(m2[i], 0, 1023, 100, 600);
driverPCA.setPWM(1, 0, pulse2);
}

void autoServo3(){
int pulse3 = map(m3[i], 0, 1023, 100, 600);
driverPCA.setPWM(2, 0, pulse3);
}

void autoServo4(){
int pulse4 = map(m4[i], 0, 1023, 100, 600);
driverPCA.setPWM(3, 0, pulse4);
}

void autoServo5(){
int pulse5 = map(m5[i], 0, 1023, 100, 600);
driverPCA.setPWM(4, 0, pulse5);
}

void autoServo6(){
int pulse6 = map(m6[i], 0, 1023, 100, 600);
driverPCA.setPWM(5, 0, pulse6);
}

Uživatelský avatar
gilhad
Příspěvky: 779
Registrován: 07 bře 2018, 11:22
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od gilhad » 21 pro 2019, 22:53

No, ja bych zacal tim, ze bych to vyrazne zjednodusil - misto funkci autoServo1() .. autoServo6() bych pouzil jedinou funkci autoServo(cislo) a prochazel to cyklem pro cislo = 1 .. 6 (obdobne manualServo)

Takze misto m1[POCET]... bych mel dvojrozmerne pole m[CISEL][POCET], stejne tak pot[CISEL] a PWM_pin[CISEL]

Dale bych odboural globalni promennou i a predaval ji tem funkcim jako parametr (a idealne i ty prvky odkud a kam)

Nasledne bych si zavedl pole predchozi[CISEL], kde bych vzdy po dosazeni pozice ulozil tu dosazenou pozici.

Pak uz je jednoduche pridat tem obema funkcim dalsi parametr rel_pos (ktery by nabyval hodnot od 0 do 100, kde 0% znaci predchozi[x] a 100% znaci nove[x] - pro x 1..CISEL) a nechal je najet jen do pozice ((predchozi[x]*(100-rel_pos)/100) + (nove[x]*rel_pos/100)) - cili pro plny presun by se prosly vsechna serva mnohokrat (treba 100x) a po prislusnych rel_pos usecich by se doslo do cile - coz by melo vyhodu jednak vyrazne plynulejsiho pohybu (vsech 6 serv se hybe prakticky najednou po malickatych kruckach) a jednak by se dalo pripadne mezi jednotlivymi cykly cekat, takze ten pohyb by byl pomalejsi (on bude i tak o neco, protoze vic vypoctu, ale to IMHO stacit nebude, tak brutalni ty vypocty nejsou)

Nasledne by sis jeste mohl pohrat bud tema pauzama mezi a/nebo s delkou toho rel_pos kroku, takze by se serva rozjizdela pomalu (dlouhe pausy/kratky krok), uprostred jela rychle (kratke pausy/dlouhy krok) a na konec by se zase dotocila pomalu a opatrne - takze bys mel plynule zrychlujici a zpomalujici pohyb.

A diky tomu, ze bys mel ty funkce jen dve (auto/manual - ale ona by stacila jedna, pokud by zadaval i kam jako parametr), tak rozsireni na vic serv (nebo min) by bylo trivialni a neopakovaly by se ti stejne casti kodu porad dokola

Dark
Příspěvky: 8
Registrován: 21 pro 2019, 21:37
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od Dark » 22 pro 2019, 09:51

Mockrát děkuji za rozsáhlou odpověď, rady i nápady. Zkusím ten program přepsat, snad se mi to povede.

Dark
Příspěvky: 8
Registrován: 21 pro 2019, 21:37
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od Dark » 22 pro 2019, 12:27

Převedl jsem to na jednu funkci (zatím řeším pouze manuální režim), ale narazil jsem bohužel na problém s polem pro potenciometry (potenciometry jsou na analogových vstupech A0,A1,A2,A3,A6,A7 na vstupech A4 a A5 nemůžou být z důvodu komunikace přes I2C s driverem). Při kompilaci mi to vždy vyhodí chybu (zkoušel jsem přímo deklarovat jen tyto prvky do pole, to ale házelo opět chybu při kompilaci, pak jsem ze zoufalosti zkoušel přiřazovat poli pot[CISEL] jednotlivé analogové vstupy pomoci průchodu podmínkou, to také nefunguje sice šlo projekt kompilovat, ale program pak stejně fungoval pro první potenciometr.

Uživatelský avatar
gilhad
Příspěvky: 779
Registrován: 07 bře 2018, 11:22
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od gilhad » 22 pro 2019, 16:11

Tak sem kdyz tak dej ten program problemama a popis problemu (aby sedely verze programu na verzi problemu)

Ja bych na to sel nejak takto:

Kód: Vybrat vše

#define CISEL 6
int pot[CISEL]={A0,A1,A2,A3,A6,A7 };
//....
for (int i =0; i<CISEL;i++) { 
   Serial print("pot[");
   Serial.print(i);
   Serial print("] = ");
   Serial.println( analogRead(pot[i]) );
  };
//.....
---

Jo, jeste jeden mozny problem - pole se v C/C++ indexuji od nuly, takze pokud mas 6 serv, tak je budes indexovat 0..5, to same pro potenciometry, a CISEL bude definovano jako 6

Dark
Příspěvky: 8
Registrován: 21 pro 2019, 21:37
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od Dark » 22 pro 2019, 20:01

Podle seriové komunikace to funguje bez problému:
pot[0] = 376
pos: 380
pulse: 285
__________________________
pot[1] = 301
pos: 304
pulse: 248
__________________________
pot[2] = 1023 (Na tento analogový vstup jsem měl připojený potenciometr, natočený do max polohy na ostatních ne,
pos: 1023 ale i s ostatními vstupy to jde podle seriové komunikace bez problému!)
pulse: 600
__________________________
pot[3] = 704
pos: 709
pulse: 446
__________________________
pot[4] = 442
pos: 453
pulse: 321
__________________________
pot[5] = 313
pos: 317
pulse: 254
______________________

Ve skutečnosti, když zakomentuji seriovou komunikaci, tak servo na kanále 0 reaguje na potenciometr, ale neustále se třepe a přeskakuje o cca +-20 stupňů kolem nastavené pozice, to i když potenciometr vůbec nepřenastavuji, na ostatních kanálech (2-5) se servo ani nehne a to i přesto, že zapojím potenciometr na příslušný vstup pro kanál, na kterém je právě servo. Vyzkoušel jsem pro jistotu i další 3 serva, v servech chyba není.

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 CISEL=6;
const int POCET=20;

int m[CISEL][POCET];
int pos[CISEL], pulse[CISEL], PWM_pin[CISEL], predchozi[CISEL];
int pot[CISEL]={A0,A1,A2,A3,A6,A7};

void setup() {
//  Serial.begin(9600);
  driverPCA.begin();
  driverPCA.setPWMFreq(60);

  pinMode(tl1,INPUT);
  pinMode(tl2,INPUT);
}

void loop() {
manualServo();
  }

void manualServo(){ 
for (int i=0; i<CISEL; i++) {
pos[i] = analogRead(pot[i]);
pulse[i] = map(pos[i], 0, 1023, 100, 600);
driverPCA.setPWM(PWM_pin[i], 0, pulse[i]);
//   Serial.print("pot[");
//   Serial.print(i);
//   Serial.print("] = ");
//   Serial.println(analogRead(pot[i]));
//   Serial.print("pos: ");
//   Serial.println(pos[i]);
//   Serial.print("pulse: ");
//   Serial.println(pulse[i]);
//   Serial.println("__________________________");
//   delay(1000);
}
}
Vůbec nevím co dělám špatně

ondraN
Příspěvky: 932
Registrován: 08 srp 2019, 20:01
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od ondraN » 22 pro 2019, 20:29

Kde je inicializované pole PWM_pin[] ?

Dark
Příspěvky: 8
Registrován: 21 pro 2019, 21:37
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od Dark » 22 pro 2019, 21:14

Díky, tak tohle mě opravdu nenapadlo, myslel jsem si, že když používám kanály 0-5, tak není potřeba inicializovat jednotlivé prvky, ale to jsem se pletl. Fajn, takže teď už všechny kanály fungují a servo se ani netřese.

Kód: Vybrat vše

int PWM_pin[CISEL]={0,1,2,3,4,5};
Tak, teď už stačí "jen" vyřešit automatický režim. Ještě mě napadá, že bych do projektu přidal OLED displej také připojený přes I2C, který by vypisoval počet uložených poloh a v auto režimu by ukazoval jednotlivé polohy, mezi kterýma se pohybuje, bylo by to možné takto udělat nebo by se kvůli toho vyskytovali nějaké problémy?

ondraN
Příspěvky: 932
Registrován: 08 srp 2019, 20:01
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od ondraN » 22 pro 2019, 22:12

Pokud bude mít ten displej jinou adresu než driver serv, tak by žádný problém byt neměl. Akorát si dej pozor na terminaci linky I2C, většinou jsou terminátory (pullUp rezistory) na každé desce, měly by být jen jednou na fyzickém konci linky. Doporučuji nejdříve rozchodit uspokojivě základní variantu a pak přidávat další srandičky :)

Uživatelský avatar
gilhad
Příspěvky: 779
Registrován: 07 bře 2018, 11:22
Reputation: 0

Re: Zpomalení serv s použitím driveru PCA9685

Příspěvek od gilhad » 22 pro 2019, 23:59

Tak to treseni je celkem jasne :) Neinicializovana RAM v Arduinuy obsahuje same nuly - cili to pole obsahovalo same nuly a ty jsi teda nastavoval servo nula na vsechny mozne ruzne hodnosty tak rychle, jak rychle jsi je upocital :D

Odpovědět

Kdo je online

Uživatelé prohlížející si toto fórum: Žádní registrovaní uživatelé a 14 hostů