Zbyl mě program na ovládání serva, nemáte někdo zájem?

Tvoříte zajímavý projekt? Pochlubte se s ním.
Pravidla fóra
Vkládejte prosím jen HOTOVÉ projekty, které chcete představit ostatním.
Odpovědět
jankop
Příspěvky: 1029
Registrován: 06 zář 2017, 20:04
Reputation: 0
Bydliště: Brno
Kontaktovat uživatele:

Zbyl mě program na ovládání serva, nemáte někdo zájem?

Příspěvek od jankop » 02 pro 2018, 14:02

Soused chtěl cosi naprogramovat, ale nakonec to šlo tak nějak do ztracena. Výsledný produkt ale není zas tak strašný. V podstatě jde o ovládání dvou serv podle předem naprogramovaných tabulek. Tyto tabulky je možné do programu nahrávat dodatečně přes sériový port. Jediné drobné úskalí je, že je vhodné program kompilovat s velikostí serial bufferu 256 byte. Jinak by mohlo docházet ke ztrátě dat, protože Arduino je občas nestíhá uložit do EEPROM. V této verzi se akce serv spouští opticky - při poklesu osvětlení fotodiody. Ta je zapojena anodou na A0 a katodou na zem. Není problém upravit program na kontakt, řídící IRQ. Popis je v přiloženém dokumentu.
Všechno to docela pěkně funguje, když budete chtít něco vysvětlit, dejte vědět.

Kód: Vybrat vše

#include <Servo.h>          // knihovna pro serva
/*
   The Servo library supports up to 12 motors on most Arduino boards and 48 on the Arduino Mega.
  On boards other than the Mega, use of the library disables analogWrite() (PWM) functionality
  on pins 9 and 10, whether or not there is a Servo on those pins. On the Mega, up to 12 servos
  can be used without interfering with PWM functionality; use of 12 to 23 motors will disable
  PWM on pins 11 and 12.
  A servo is activated by creating an instance of the Servo class passing
  the desired pin to the attach() method.
  The servos are pulsed in the background using the value most recently
  written using the write() method.
  Note that analogWrite of PWM on pins associated with the timer are
  disabled when the first servo is attached.
  Timers are seized as needed in groups of 12 servos - 24 servos use two
  timers, 48 servos will use four.
  The sequence used to sieze timers is defined in timers.h
  The methods are:
    Servo - Class for manipulating servo motors connected to Arduino pins.
    attach(pin )  - Attaches a servo motor to an i/o pin.
    attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
    default min is 544, max is 2400
    write()     - Sets the servo angle in degrees.  (invalid angle that is valid as pulse in microseconds is treated as microseconds)
    writeMicroseconds() - Sets the servo pulse width in microseconds
    read()      - Gets the last written servo pulse width as an angle between 0 and 180.
    readMicroseconds()   - Gets the last written servo pulse width in microseconds. (was read_us() in first release)
    attached()  - Returns true if there is a servo attached.
    detach()    - Stops an attached servos from pulsing its i/o pin.
    #define Servo_VERSION           2     // software version of this library
    #define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
    #define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
    #define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
    #define REFRESH_INTERVAL    20000     // minimum time to refresh servos in microseconds
    ---------------------------------------------------------------------------------------*/
#include <EEPROM.h>         // knihovna pro  EEPROM 
#define VERSION           2 // verze programu
#define MIN_PULSE       600 // minimalni delka pulzu pro servo [us]
#define MAX_PULSE      2400 // maximalni delka pulzu pro servo [us]
#define START_ANGLE      90 // inicializacni uhel pro tabulku 
#define WAIT_PARAM     5000 // doba do hlaseni o chybejicim ciselnem parametru [ms]
#define DATA_LENGTH      20 // delka datovych tabulek            
#define DELAY            20 // prodleva mezi kroky v tabulce   [ms]
#define GAIN              5 // citlivost - cim vetsi cislo, tim mensi citlivost
#define VERSION_ADR       0 // adresa verze programu v EEPROM
#define LGTH_ADR          1 // adresa delky tabulek v EEPROM
#define DELAY_ADR         2 // adresa prodlevy DELAY v EEPROM
#define GAIN_ADR          3 // adresa zesileni v EEPROMd
#define PIN_SERVO_A       4 // pin pro pripojeni serva A
#define PIN_SERVO_B       5 // pin pro pripojeni serva B                
#define PIN_ANA           A0// pin pro fotodiodu
#define LED_PIN          13 // pin pro integrovanou LED
#define PATSET     "set\r\n"// nastaveni systemu
#define PATSAL     "sal\r\n"// testovaci pohyb serva
#define PATEXI     "exi\r\n"// ukonceni nastaveni systemu
#define PATLST     "lis\r\n"// listing vsech hodnot
#define PATLNG     "del\r\n"// nastaveni delky datovych tabulek
#define PATDTA     "dta\r\n"// data pro servo A [ 0 - 180 stupnu]
#define PATDTB     "dtb\r\n"// data pro servo B [ 0 - 180 stupnu]
#define PATMVA     "mva\r\n"// pohyb servem A   [ 0 - 180 stupnu]
#define PATMVB     "mvb\r\n"// pohyb servem B   [ 0 - 180 stupnu]
#define PATDLY     "dly\r\n"// nastaveni prodlevy DELAY v [ms]
#define PATGAI     "gai\r\n"// nastaveni zesileni fotodiody
bool Salut = false;         // priznak pro provedeni pohybu serv
bool SetParam = false;      // priznak nastavovaciho modu
char buff [6];
int light = 0;
int ramp = 0;
Servo ServoA;               // vytvoreni objektu servo
Servo ServoB;               // vytvoreni objektu servo

void setup() {
  Serial.begin(9600);
  //analogReference(INTERNAL);           // zasadni zvyseni citlivosti
  pinMode(LED_PIN, OUTPUT);              // nastaveni vystupu pro LED
  pinMode(PIN_ANA, INPUT);               // nastaveni analogoveho vstupu
  digitalWrite(LED_PIN, LOW);            // zhasnuti interni LED
  Serial.println("\r\nSTART!");
  if (EEPROM.read(VERSION_ADR) == 0xff) {// pri prvnim zapnuti - cista pamet EEPROM, uloz inicializacni hodnoty
    EEPROM.update(VERSION_ADR, VERSION); // ulozi verzi programu
    EEPROM.update(LGTH_ADR, DATA_LENGTH);// ulozi delku datovych poli A a B
    EEPROM.update(DELAY_ADR, DELAY);     // ulozi prodlevu mezi jednotlivymi kroky serva
    EEPROM.update(GAIN_ADR, GAIN);       // ulozi zesileni
    // inicializuje cele mozne pole A a pole B do zakladni polohy serv 90 stupnu
    for (byte indy = 0; indy < 240; indy++) { //pole zacinaji na adrese 4 a maji delku max 2*120 byte v EEPROM
      EEPROM.update(indy + 4, START_ANGLE);
    }
  }
  ServoA.attach(PIN_SERVO_A, MIN_PULSE, MAX_PULSE);    // pripojeni servo A
  ServoB.attach(PIN_SERVO_B, MIN_PULSE, MAX_PULSE);    // pripojeni servo B
  ServoA.write(EEPROM.read(4));                        // servo A do vychozi pozice
  ServoB.write(EEPROM.read(EEPROM.read(LGTH_ADR + 4)));// servo B do vychozi pozice
  ramp = analogRead(PIN_ANA);            // prvotni nacteni intenzity osvetleni
}
void loop() {
  // ocekava string "SET\r\n" za predpokladu, ze neni mod nastavovani
  while ((Serial.available() > 0) && (!SetParam)) {
    buff[5] = Serial.read();
    for (byte indy = 0; indy < 5; indy++) {
      buff[indy] = buff[indy + 1];       // rotace buffru dolu
    }
    buff[5] = '\0';                      // terminace bufferu koncovym znakem za patym znakem
    if (String (PATSET) ==  String (buff)) { // prepnuti do modu nastavovani
      SetParam = true;
      Serial.println("\r\nNASTAVOVANI!");
    }
  }
  if (SetParam) {   // pokud je mod nastavovani, volej podprogram pro nastavovani
    digitalWrite(LED_PIN, HIGH); // rozsviceni interni LED, signalizace modu nastavovani
    SetParamRut();  // nastavovaci podprogram
  }
  else {            //pokud neni nastavovani, obsluhuj fotodiodu
    light = analogRead(PIN_ANA); // zmer svetlo
    //Serial.println(light);
    if (light > ramp)ramp++;     // dorovnavej automatickou uroven ramp
    if (light < ramp)ramp--;
    if (light < (ramp - EEPROM.read(GAIN_ADR))) {// pokud je velky rozdil pak spust serva
      MoveServos();    // start serv do akce
      //UnMoveServos();  // navrat serv do zakladni polohy
      ramp = analogRead(PIN_ANA);    // zmer svetlo, po dobu chodu serv se mohlo zmenit
    } else  delay(20); // pokud nebyl pohyb serv tak poseckej
  }
}
void MoveServos() {                                               // pohyb serv do pozice bez prvni polozky
  for (byte indy = 1; indy < (EEPROM.read(LGTH_ADR)); indy++) {
    ServoA.write( EEPROM.read(indy + 4));                         // krok serva A do pozice
    //Serial.println(EEPROM.read(indy + 4));                      // pouze pro testovani
    ServoB.write( EEPROM.read(indy + 4 + EEPROM.read(LGTH_ADR))); // krok serva B do pozice
    //Serial.println(EEPROM.read(indy + 4 + EEPROM.read(LGTH_ADR)));// pouze pro testovani
    delay(EEPROM.read(DELAY_ADR));                                // cekani - zpozdeni na posuv serv
  }
}
// *** nepoužitá funkce ***
void UnMoveServos() {                                             // pohyb serv z pozice bez posledni polozky
  for (byte indy = EEPROM.read(LGTH_ADR); indy > 1; indy--) {
    ServoA.write(EEPROM.read(indy + 2));                          // krok serva A z pozice
    //Serial.println(EEPROM.read(indy + 2));                      // pouze pro testovani
    ServoB.write(EEPROM.read(indy + 2 + EEPROM.read(LGTH_ADR)));  // krok serva B z pozice
    //Serial.println(EEPROM.read(indy + 2 + EEPROM.read(LGTH_ADR)));// pouze pro testovani
    delay(EEPROM.read(DELAY_ADR));                                // cekani - zpozdeni na posuv serv
  }
}
void SetParamRut() {
  if (Serial.available() > 0) {// cti nastavovaci prikaz
    buff[5] = Serial.read();
    for (byte indy = 0; indy < 5; indy++) { // rotuj buffer dolu
      buff[indy] = buff[indy + 1];
    }
    buff[5] = '\0';                     // uzavri bufer koncovym znakem na delku
  }
  if (String(PATLST) == String(buff)) { // detekce prikazu listingu
    List();
  }
  if (String(PATSAL) == String(buff)) { // detekce prikazu pro testovaci pohyb serv
    MoveServos();
    //UnMoveServos();
  }
  if (String(PATDLY) == String(buff)) { // prikaz nastaveni prodlevy mezi kroky v tabulkach
    Serial.println("Zpozdeni[ms]?");
    byte pom = ReadShortNumber();
    if (pom < 2)pom = 2;
    Serial.print  ("Zpozdeni:" );
    Serial.println(pom);
    EEPROM.update(DELAY_ADR, pom);
  }
  if (String(PATLNG) == String(buff)) { // prikaz nastaveni delky tabulky
    Serial.println("Delka tabulky?");
    byte pom = ReadShortNumber();
    if (pom < 2)pom = 2;     // uprava mezi
    if (pom > 120)pom = 120; // uprava mezi
    Serial.print  ("Delka tabulky:");
    Serial.println(pom);
    EEPROM.update(LGTH_ADR, pom);
  }
  if (String(PATGAI) == String(buff)) { // nastaveni zesileni fotodiody
    Serial.println("Gain?");
    byte pom = ReadShortNumber();
    Serial.print  ("Gain:" );
    Serial.println(pom);
    EEPROM.update(GAIN_ADR, pom);
  }
  if (String(PATMVA) == String(buff)) { // pohyb serva A do zvolene polohy
    Serial.println("Servo A pozice?");
    byte pom = ReadShortNumber();
    if (pom > 180)pom = 180; // uprava mezi uhlu
    Serial.print  ("Servo A pozice:");
    Serial.println(pom);
    ServoA.write(pom);
  }
  if (String(PATMVB) == String(buff)) { // pohyb serva B do zvolene polohy
    Serial.println("Servo B pozice?");
    byte pom = ReadShortNumber();
    if (pom > 180)pom = 180; // uprava mezi uhlu
    Serial.print  ("Servo B pozice:");
    Serial.println(pom);
    ServoB.write(pom);
  }
  if (String(PATDTA) == String(buff)) { // nacitani tabulky A
    Serial.print("Tabulka A:");
    Serial.println(EEPROM.read(LGTH_ADR));
    for (byte indy = 0; indy < EEPROM.read(LGTH_ADR); indy++) {// uklada tabulku A
      byte pom = ReadShortNumber();
      if (pom > 180)pom = 180; // uprava mezi uhlu
      EEPROM.update(indy + 4, pom);
    }
  }
  if (String(PATDTB) == String(buff)) { // nacitani tabulky B
    Serial.print("Tabulka B:");
    Serial.println(EEPROM.read(LGTH_ADR));
    for (byte indy = 0; indy < EEPROM.read(LGTH_ADR); indy++) { // uklada tabulku B
      byte pom = ReadShortNumber();
      if (pom > 180)pom = 180; // uprava mezi uhlu
      EEPROM.update(indy + 4 + EEPROM.read(LGTH_ADR), pom);
    }
  }
  if (String(PATEXI) == String(buff)) { // ukonceni nastavovani a prechod do pracovniho modu
    SetParam = false; // zrus priznak nastavovani
    digitalWrite(LED_PIN, LOW);         // zhasni LED
    Serial.println("\r\nPROVOZ!");
    ramp = analogRead(PIN_ANA);         // zmer svetlo, po dobu nsatavovani se mohlo zmenit
  }
  buff[0] = '\0';                       // vynuluj buffer, aby se prikaz neopakoval
}
// listovani nastavenych parametru
void List() {
  Serial.print("Verze: ");
  Serial.println(EEPROM.read(VERSION_ADR), DEC);
  Serial.print("Delka tabulek: ");
  Serial.println(EEPROM.read(LGTH_ADR), DEC);
  Serial.print("Zpozdeni[ms]: ");
  Serial.println(EEPROM.read(DELAY_ADR), DEC);
  Serial.print("Gain: ");
  Serial.println(EEPROM.read(GAIN_ADR), DEC);
  Serial.println("Tabulka______A:");
  for (byte indy = 0; indy < (EEPROM.read(LGTH_ADR)); indy++) {
    Serial.print("Index: ");
    Serial.print(indy);
    Serial.print(" ");
    Serial.println(EEPROM.read(indy + 4), DEC);
  }
  Serial.println("Tabulka______B:");
  for (byte indy = 0; indy < (EEPROM.read(LGTH_ADR)); indy++) {
    Serial.print("Index: ");
    Serial.print(indy);
    Serial.print(" ");
    Serial.println(EEPROM.read(indy + 4 + EEPROM.read(LGTH_ADR)), DEC);
  }
}
int ReadShortNumber() {
  // cekani a cteni jednoho cisla ve tvaru "DDD\r\n"
  String StringBuff;
  char buffs[6] = "00000";
  unsigned long counter = millis();
  do {
    if (Serial.available() > 0) {
      buffs[5] = Serial.read();
      for (byte indy = 0; indy < 5; indy++) {
        buffs[indy] = buffs[indy + 1];
      }
    }
    // prevod retezce na kratke cislo - byte
    if (isDigit(buffs[0]) && isDigit(buffs[1]) && isDigit(buffs[2]) && buffs[3] == '\r' && buffs[4] == '\n') {
      StringBuff = String (buffs[0]) + String (buffs[1]) + String (buffs[2]);
      byte pom = (byte) StringBuff.toInt();
      return pom;
    }
    if (!isDigit(buffs[0]) && isDigit(buffs[1]) && isDigit(buffs[2]) && buffs[3] == '\r' && buffs[4] == '\n') {
      StringBuff = String (buffs[1]) + String (buffs[2]);
      byte pom = (byte) StringBuff.toInt();
      return pom;
    }
    if (!isDigit(buffs[0]) && !isDigit(buffs[1]) && isDigit(buffs[2]) && buffs[3] == '\r' && buffs[4] == '\n') {
      StringBuff =  String (buffs[2]);
      byte pom = (byte) StringBuff.toInt();
      return pom;
    }
    if ((millis() - counter) > WAIT_PARAM) {
      counter = millis();
      Serial.println("Cekam na parametr!");
    }
  } while (true);
}
Přílohy
Demo_Tabulka.txt
(1.06 KiB) Staženo 156 x
Sweep_TEST_24.ino
(13.47 KiB) Staženo 142 x
SALUTE_03.doc
(64 KiB) Staženo 161 x

Odpovědět

Kdo je online

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