//  Listing 10.10. Mechanizm tłumaczący PUMT — wybrane możliwości ruchu mechanicznego ramienia


    1    #include <ax12.h>
    2    #include <BioloidController.h>
    3    #include "poses.h"
    4    
    5    BioloidController bioloid = BioloidController(1000000);
    6    
    7    class robot_arm{
    8       private:
    9           int ServoCount;
   10       protected:
   11           int id;
   12           int pos;
   13           boolean IDCheck;
   14           boolean StartupComplete;
   15       public:
   16           robot_arm(void);  
   ...
////AKCJE: SEKCJA 2.
   18           void scanServo(void);
   19           void moveCenter(void);
   20           void checkVoltage(void);
   21           void moveHome(void);
   22           void relaxServos(void);
   23           void retrieveObject(void);
   24    };    
   25    
   ...
//ZADANIA: SEKCJA 3.
   82    void robot_arm::scanServo(void)
   83    {
   84          id = 1;  
   85          Serial.println("Skanowanie serwomotorów");    
   86          while (id <= ServoCount)
   87          {
   88               pos =  ax12GetRegister(id, 36, 2);
   89               Serial.print("Identyfikator serwomotoru: ");
   90               Serial.println(id);
   91               Serial.print("Pozycja serwomotoru: ");
   92               Serial.println(pos); 
   93               if (pos <= 0){
   94                   Serial.println("=============================");
   95                   Serial.print("BŁĄD! Serwomotor o identyfikatorz: ");
   96                   Serial.print(id);
   97                   Serial.println(" nie został znaleziony. Sprawdź połączenie i poprawność
                                       wybranego identyfikatora.");
   98                   Serial.println("============================="); 
   99                   IDCheck = false;
  100               }
  101      
  102               id = (id++)%ServoCount;
  103               delay(1000);
  104          }
  105          if (!IDCheck){
  106               Serial.println("================================");
  107               Serial.println("BŁĄD! Brak identyfikatorów serwomotorow do skanowania.");
  108               Serial.println("================================");  
  109          }
  110          else{
  111                 Serial.println("Proces sprawdzania serwomotorów zakończony sukcesem");
  112          }
  ...

  223    void robot_arm::checkVoltage(void)  
  224    {   
  225          float voltage = (ax12GetRegister (1, AX_PRESENT_VOLTAGE, 1)) / 10.0;   
  226          Serial.print ("Napięcie zasilające: ");
  227          Serial.print (voltage);
  228          Serial.println (" volts.");
  229          if (voltage < 10.0){
  230              Serial.println("Napięcie niższe od 10 V. Naładuj akumulator.");
  231              while(1);
  232          }  
  233          if (voltage > 10.0){
  234              Serial.println("Napięcie ma wartość znamionową.");
  235          }
  236          if (StartupComplete){
  237               ...
  238          }
  239          
  240    }
  241
  242    void robot_arm::moveCenter()
  243    {
  244          delay(100);                    
  245          bioloid.loadPose(Center);   
  246          bioloid.readPose();            
  247          Serial.println("Przestawianie serwomotorów w środkowe położenia");    
  248          delay(1000);
  249          bioloid.interpolateSetup(1000); 
  250          while(bioloid.interpolating > 0){  
  251                bioloid.interpolateStep();     
  252                delay(3);
  253          }
  254          if (StartupComplete){
  255            …
  256          }
  257    }
  258    
  259    
  260    void robot_arm::moveHome(void)
  261    {
  262         delay(100);                    
  263         bioloid.loadPose(Home);   
  264         bioloid.readPose();            
  265         Serial.println("Przestawianie serwomotorów w położenia wyjściowe");
  266         delay(1000);
  267         bioloid.interpolateSetup(1000); 
  268         while(bioloid.interpolating > 0){  
  269               bioloid.interpolateStep();     
  270               delay(3);
  271         }
  272         if (StartupComplete){
  273             …
  274         }
  275    }
  ...
  277    void robot_arm::retrieveObject()
  278    {
  279      
  280        Serial.println("=======================");
  281        Serial.println("Przenieś przedmiot");
  282        Serial.println("=======================");
  283        delay(500);
  284        id  = 1;
  285        pos = 512;
  286      
  287      
  288        Serial.print(" Przestawianie serwomotoru : ");
  289        Serial.println(id);
  290        while(pos >= 312)
  291        {
  292              SetPosition(id,pos);
  293              pos = pos--;
  294              delay(10);
  295        }
  296        while(pos <= 512){  
  297              SetPosition(id, pos);  
  298              pos = pos++;
  299              delay(10);
  300        }
  301        
  302        
  303        id = 3;
  304        Serial.print("Przestawianie serwomotoru : ");
  305        Serial.println(id);
  306        while(pos >= 200)
  307        {
  308              SetPosition(id,pos);
  309              pos = pos--;
  310              delay(15);
  311        }
  312        while(pos <= 512){  
  313              SetPosition(id, pos);  
  314              pos = pos++;
  315              delay(15);
  316        }
  317         
  318        id = 3;
  319        while(pos >= 175)
  320        {
  321              SetPosition(id,pos);
  322              pos = pos--;
  323              delay(20);
  324        }
  325         
  326     
  327        id = 5;
  328        Serial.print(" Przestawianie chwytaka : ");
  329        Serial.println(id);
  330        pos = 512;
  331        while(pos >= 170)
  332        {
  333              SetPosition(id,pos);
  334              pos = pos--;
  335              delay(30);
  336        }
  337        while(pos <= 512){  
  338              SetPosition(id, pos);  
  339              pos = pos++;
  340              delay(30);
  341        }
  342        // Chwytak oznaczony jest identyfikatorem nr 5
  343        id = 5;
  344        while(pos >= 175)
  345        {
  346              SetPosition(id,pos);
  347              pos = pos--;
  348              delay(20);
  349        }
  350        while(pos <= 512){  
  351              SetPosition(id, pos);  
  352              pos = pos++;
  353              delay(20);
  354        }
  355        id = 4;
  356        Serial.print(" Przestawianie serwomotoru : ");
  357        Serial.println(id);
  358     
  359        while(pos >= 200)
  360        {
  361              SetPosition(id,pos);
  362              pos = pos--;
  363              delay(10);
  364        }
  365        while(pos <= 512){  
  366              SetPosition(id, pos);  
  367              pos = pos++;
  368              delay(20);
  369        }
  370      
  371        if(StartupComplete == 1){
  372            ...
  373        }    
  374         
  375    }    
  376      
  377
