-         

Ergebnis 1 bis 5 von 5

Thema: Servos tun nicht was sie sollen

  1. #1
    Benutzer Stammmitglied
    Registriert seit
    06.04.2008
    Ort
    Rottenburg a.N.
    Alter
    34
    Beiträge
    94

    Servos tun nicht was sie sollen

    Anzeige

    Hi,

    als Basis meines Greifarms V2 mit fünf Servos habe ich die M32-Libs von Dirk genommen, mit denen man bis zu 8 Servos an der M32 benutzen kann.

    Problem:
    die Servos machen einfach nicht das was sie sollen.
    Bsp.:
    initSERVO(SERVO1); --> Es passiert nix!
    initSERVO(SERVO1 | SERVO2); ---> Servo 1 zittert wie blöd, geht aber an die Position, die in der Servolib.h eingestellt ist und zittert weiter, Servo2 fährt ganz nach rechts und will weiter raus. (Ich habe diverse Werte für RIGHT_TOUCH ausprobiert, es geht nix)
    initSERVO(SERVO1 | SERVO2 | SERVO3 | SERVO4 | SERVO5);
    ---> alle fünf Servos fahren ganz nach rechts und wollen weiter, egal was in der Lib eingestellt ist.

    Kann mir mal einer auf die Sprünge helfen, wo ich den Fehler mache?

    BITTE BITTE !!!

    Hier noch der Code zum Programm:
    Code:
    #include "RP6ControlServoLib.h"
    #include "RP6ControlLib.h"
    
    uint16_t pos_GREIFER = 0;
    uint16_t pos_GELENK = 0;
    uint16_t pos_NIVEAU = 0;
    uint16_t pos_ARM_HOCH = 0;
    uint16_t pos_ARM_SEIT = 0;
    
    int main(void)
    { 
       initRP6Control();
       initSERVO(SERVO1 | SERVO2 | SERVO3 | SERVO4 | SERVO5);
       startStopwatch2();
    
    while(true) 
       {
    
    if (getStopwatch2() > 48) {
    servo1_position = pos_GREIFER;
    servo2_position = pos_GELENK;
    servo3_position = pos_NIVEAU;
    servo4_position = pos_ARM_HOCH;
    servo5_position = pos_ARM_SEIT;
    pos_ARM_HOCH = 60; 
    pos_ARM_SEIT = 80;
    }
     setStopwatch2(0);
      task_SERVO();
    	  }
       }
    }
    Ich weiß, theoretisch sollte hier ganr nix gehen. In der Servolib.h ist eingestellt, dass wenn keine Werte übergeben werden, die Servos nach links fahren sollen.

    Hier die lib.h:
    Code:
    /* 
    #include "RP6ControlLib.h"       // The RP6 Control Library.
    
    // Servo constants:
    #define SERVO1            0b00000001
    #define SERVO2            0b00000010
    #define SERVO3            0b00000100
    #define SERVO4            0b00001000
    #define SERVO5            0b00010000
    #define SERVO6            0b00100000
    #define SERVO7            0b01000000
    #define SERVO8            0b10000000
    
    /*define LEFT_TOUCH                 80         					
    #define RIGHT_TOUCH                80        					
    #define MIDDLE_POSITION            (RIGHT_TOUCH / 2) 		
    #define PULSE_REPETITION           15 */        				
    #define F_TIMER1                   66667      					
    
    #define LEFT_TOUCH_GREIFER          120        			
    #define RIGHT_TOUCH_GREIFER         60       			
    #define MIDDLE_POSITION_GREIFER     (RIGHT_TOUCH / 2) 	
    #define PULSE_REPETITION_GREIFER    15	         		
    
    
    #define LEFT_TOUCH_GELENK           110        			
    #define RIGHT_TOUCH_GELENK          80      			
    #define MIDDLE_POSITION_GELENK      (RIGHT_TOUCH / 2)
    #define PULSE_REPETITION_GELENK     15   	      		
    
    #define LEFT_TOUCH_NIVEAU      		100        			
    #define RIGHT_TOUCH_NIVEAU      	60	      			
    #define MIDDLE_POSITION_NIVEAU  	(RIGHT_TOUCH / 2) 	
    #define PULSE_REPETITION_NIVEAU 	15	         		
    
    #define LEFT_TOUCH_ARM_HOCH         80        			
    #define RIGHT_TOUCH_ARM_HOCH        60	      			
    #define MIDDLE_POSITION_ARM_HOCH    (RIGHT_TOUCH / 2)
    #define PULSE_REPETITION_ARM_HOCH   17.5         		
    
    #define LEFT_TOUCH_ARM_SEIT         80        			
    #define RIGHT_TOUCH_ARM_SEIT        60      			
    #define MIDDLE_POSITION_ARM_SEIT    (RIGHT_TOUCH / 2)
    #define PULSE_REPETITION_ARM_SEIT   17.5         		
    
    // Servo ports:
    #define SERVO1_PULSE_ON      (PORTC |= IO_PC2)   // PC2
    #define SERVO1_PULSE_OFF   (PORTC &= ~IO_PC2)
    #define SERVO2_PULSE_ON      (PORTC |= IO_PC3)   // PC3
    #define SERVO2_PULSE_OFF   (PORTC &= ~IO_PC3)
    #define SERVO3_PULSE_ON      (PORTC |= IO_PC4)   // PC4
    #define SERVO3_PULSE_OFF   (PORTC &= ~IO_PC4)
    #define SERVO4_PULSE_ON      (PORTC |= IO_PC5)   // PC5
    #define SERVO4_PULSE_OFF   (PORTC &= ~IO_PC5)
    #define SERVO5_PULSE_ON      (PORTC |= IO_PC6)   // PC6
    #define SERVO5_PULSE_OFF   (PORTC &= ~IO_PC6)
    #define SERVO6_PULSE_ON      (PORTC |= IO_PC7)   // PC7
    #define SERVO6_PULSE_OFF   (PORTC &= ~IO_PC7)
    #define SERVO7_PULSE_ON      (PORTD |= IO_PD5)   // PD5
    #define SERVO7_PULSE_OFF   (PORTD &= ~IO_PD5)
    #define SERVO8_PULSE_ON      (PORTD |= IO_PD6)   // PD6
    #define SERVO8_PULSE_OFF   (PORTD &= ~IO_PD6)
    // -----------------------------------------------------------
    // Other possible ports for connecting Servos to RP6Control:
    //#define SERVOx_PULSE_ON      (PORTA |= ADC6)      // PA6
    //#define SERVOx_PULSE_OFF   (PORTA &= ~ADC6)
    //#define SERVOx_PULSE_ON      (PORTA |= ADC7)      // PA7
    //#define SERVOx_PULSE_OFF   (PORTA &= ~ADC7)
    // -----------------------------------------------------------
    
    /*****************************************************************************/
    // Variables:
    
    uint16_t servo1_position;      // Servo 1 position [0..RIGHT_TOUCH]
    uint16_t servo2_position;      // Servo 2 position [0..RIGHT_TOUCH]
    uint16_t servo3_position;      // Servo 3 position [0..RIGHT_TOUCH]
    uint16_t servo4_position;      // Servo 4 position [0..RIGHT_TOUCH]
    uint16_t servo5_position;      // Servo 5 position [0..RIGHT_TOUCH]
    uint16_t servo6_position;      // Servo 6 position [0..RIGHT_TOUCH]
    uint16_t servo7_position;      // Servo 7 position [0..RIGHT_TOUCH]
    uint16_t servo8_position;      // Servo 8 position [0..RIGHT_TOUCH]
    
    /*****************************************************************************/
    // Functions:
    
    void initSERVO(uint8_t servos);
    void startSERVO(void);
    void stopSERVO(void);
    void pulseSERVO(void);
    void task_SERVO(void);
    
    /******************************************************************************
     * Additional info
     * ****************************************************************************
     * Changelog:
     * - v. 1.0 (initial release) 31.12.2008 by Dirk
     *
     * ****************************************************************************
     */
    
    /*****************************************************************************/
    // EOF
    und die lib.c:
    Code:
    /* 
    #include "RP6ControlServoLib.h"
    // Variables:
    
    uint8_t usedservos;
    uint8_t servo_on = FALSE;
    
    uint16_t impulselength1 = 0;
    uint16_t impulselength2 = 0;
    uint16_t impulselength3 = 0;
    uint16_t impulselength4 = 0;
    uint16_t impulselength5 = 0;
    uint16_t impulselength6 = 0;
    uint16_t impulselength7 = 0;
    uint16_t impulselength8 = 0;
    
    volatile uint16_t intcounter = 0;
    
    // Functions:
    
    void initSERVO(uint8_t servos)
    {
       usedservos = servos;            // Save used Servos
       impulselength1 = 0;
       impulselength2 = 0;
       impulselength3 = 0;
       impulselength4 = 0;
       impulselength5 = 0;
       impulselength6 = 0;
       impulselength7 = 0;
       impulselength8 = 0;
       if (servos & SERVO1) {DDRC |= IO_PC2; PORTC &= ~IO_PC2;}
       if (servos & SERVO2) {DDRC |= IO_PC3; PORTC &= ~IO_PC3;}
       if (servos & SERVO3) {DDRC |= IO_PC4; PORTC &= ~IO_PC4;}
       if (servos & SERVO4) {DDRC |= IO_PC5; PORTC &= ~IO_PC5;}
       if (servos & SERVO5) {DDRC |= IO_PC6; PORTC &= ~IO_PC6;}
       if (servos & SERVO6) {DDRC |= IO_PC7; PORTC &= ~IO_PC7;}
       if (servos & SERVO7) {DDRD |= IO_PD5; PORTD &= ~IO_PD5;}
       if (servos & SERVO8) {DDRD |= IO_PD6; PORTD &= ~IO_PD6;}
    
       cli();
       // Timer 1: Normal port operation, mode 4 (CTC), clk/8
       TCCR1A =  (0 << COM1A1)
             | (0 << COM1A0)
             | (0 << COM1B1)
             | (0 << COM1B0)
             | (0 << FOC1A)
             | (0 << FOC1B)
             | (0 << WGM11)
             | (0 << WGM10);
       TCCR1B =  (0 << ICNC1)
             | (0 << ICES1)
             | (0 << WGM13)
             | (1 << WGM12)
             | (0 << CS12)
             | (1 << CS11)
             | (0 << CS10);
       OCR1A = ((F_CPU/8/F_TIMER1)-1);   // 19 at 100kHz
       startSERVO();
       sei();
       startStopwatch1();               // Needed for 20ms pulse repetition
    }
    
    void startSERVO(void)
    {
       TIMSK |= (1 << OCIE1A);
       servo_on = TRUE;
    }
    
    void stopSERVO(void)
    {
       TIMSK &= ~(1 << OCIE1A);
       servo_on = FALSE;
    }
    
    void pulseSERVO(void)
    {
       if (servo_on) {
          if (usedservos & SERVO1) {
             SERVO1_PULSE_ON; impulselength1 = LEFT_TOUCH_GREIFER + servo1_position;}
          if (usedservos & SERVO2) {
             SERVO2_PULSE_ON; impulselength2 = LEFT_TOUCH_GELENK + servo2_position;}
          if (usedservos & SERVO3) {
             SERVO3_PULSE_ON; impulselength3 = LEFT_TOUCH_NIVEAU + servo3_position;}
          if (usedservos & SERVO4) {
             SERVO4_PULSE_ON; impulselength4 = LEFT_TOUCH_ARM_HOCH + servo4_position;}
          if (usedservos & SERVO5) {
             SERVO5_PULSE_ON; impulselength5 = LEFT_TOUCH_ARM_SEIT + servo5_position;}
          /*if (usedservos & SERVO6) {
             SERVO6_PULSE_ON; impulselength6 = LEFT_TOUCH + servo6_position;}
          if (usedservos & SERVO7) {
             SERVO7_PULSE_ON; impulselength7 = LEFT_TOUCH + servo7_position;}
          if (usedservos & SERVO8) {
             SERVO8_PULSE_ON; impulselength8 = LEFT_TOUCH + servo8_position;}
         */ intcounter = 0;
       }
    }
    
    ISR (TIMER1_COMPA_vect)
    {
       intcounter++;
       if (intcounter == impulselength1) {SERVO1_PULSE_OFF;}
       if (intcounter == impulselength2) {SERVO2_PULSE_OFF;}
       if (intcounter == impulselength3) {SERVO3_PULSE_OFF;}
       if (intcounter == impulselength4) {SERVO4_PULSE_OFF;}
       if (intcounter == impulselength5) {SERVO5_PULSE_OFF;}
       if (intcounter == impulselength6) {SERVO6_PULSE_OFF;}
       if (intcounter == impulselength7) {SERVO7_PULSE_OFF;}
       if (intcounter == impulselength8) {SERVO8_PULSE_OFF;}
    }
    
    void task_SERVO(void)
    {
       if (getStopwatch1() > 2) {TIMSK &= ~(1 << OCIE1A);}
       
       if (getStopwatch1() > PULSE_REPETITION_GREIFER) { // Pulse every ~20ms
          pulseSERVO();               // Servo pulse generation
          if (servo_on) {TIMSK |= (1 << OCIE1A);}
          setStopwatch1(0);
       }
      if (getStopwatch1() > PULSE_REPETITION_GELENK) { // Pulse every ~20ms
          pulseSERVO();               // Servo pulse generation
          if (servo_on) {TIMSK |= (1 << OCIE1A);}
          setStopwatch1(0);
       }
    
       if (getStopwatch1() > PULSE_REPETITION_NIVEAU) { // Pulse every ~20ms
          pulseSERVO();               // Servo pulse generation
          if (servo_on) {TIMSK |= (1 << OCIE1A);}
          setStopwatch1(0);
       }
    
    
       if (getStopwatch1() > PULSE_REPETITION_ARM_HOCH) { // Pulse every ~20ms
          pulseSERVO();               // Servo pulse generation
          if (servo_on) {TIMSK |= (1 << OCIE1A);}
          setStopwatch1(0);
       }
    
       if (getStopwatch1() > PULSE_REPETITION_ARM_SEIT) { // Pulse every ~20ms
          pulseSERVO();               // Servo pulse generation
          if (servo_on) {TIMSK |= (1 << OCIE1A);}
          setStopwatch1(0);
       }
    
    }
    HILFE!!!


    Gruß

    topgunfb
    Wer Interesse an Mangas hat, muss meine Website besuchen:
    http://www.manga-bl.de
    ---Bald auch mit RP6-Infos zu Projekten---

  2. #2
    Benutzer Stammmitglied
    Registriert seit
    06.04.2008
    Ort
    Rottenburg a.N.
    Alter
    34
    Beiträge
    94
    Um irgendwelchen Frage entgegen zu wirken:
    ARM_SEIT, ARM_HOCH sind die hier: http://www.conrad.de/goto.php?artikel=233751

    NIVEAU, GELENK & GREIFER sind die hier: http://www.conrad.de/goto.php?artikel=230500

    die Anschlüsse sind: PC2, PC3, PC4, PC5, PC6 an der M32
    Strom bekommen die Servos aus zusätzlichen Batterien.

    Gruß

    topgunfb
    Wer Interesse an Mangas hat, muss meine Website besuchen:
    http://www.manga-bl.de
    ---Bald auch mit RP6-Infos zu Projekten---

  3. #3
    Erfahrener Benutzer Robotik Einstein Avatar von Dirk
    Registriert seit
    30.04.2004
    Ort
    NRW
    Beiträge
    3.791
    Hi topgunfb,

    was soll dein Programm denn machen?
    Du gibst allen Servos Werte (0), dann nach 50ms ARM_HOCH und ARM_SEIT zwei neue Werte (60, 80), die dann so bleiben.

    1. Zum Testen würde ich für die Stopwatch2 z.B. Werte von 5000 (anstelle von 48 ) nehmen, damit jede Stellung 5s gehalten wird.

    2. In dem if (getStopwatch2() > 5000) Teil würde ich als Test einen Zähler setzen und bei jedem Durchlauf (also alle 5s) per Zähler den Servos neue Werte zuweisen:
    Wenn Zähler = 1 -> pos = 30
    Wenn Zähler = 2 -> pos = 50 usw. (z.B. als switch-Konstrukt)

    3. setStopwatch2(0) gehört ans Ende des if (getStopwatch2() > 5000) Teils, nicht in die Hauptschleife mit task_SERVO().

    4. Irgendwie ist eine "}" zuviel. Da müßte der Compiler meckern.

    Gruß Dirk

  4. #4
    Benutzer Stammmitglied
    Registriert seit
    06.04.2008
    Ort
    Rottenburg a.N.
    Alter
    34
    Beiträge
    94
    Hi Dirk,

    danke für die Antwort. Sowas habe ich mir schon gedacht. (mal wieder zu blöd ... )

    Die Klammer ist wirklich zuviel. Da geht nix. Die muss beim Kommentar entfernen reingerutscht sein.

    Ich werds nochmal probieren und mich wieder melden.

    Gruß

    topgunfb
    Wer Interesse an Mangas hat, muss meine Website besuchen:
    http://www.manga-bl.de
    ---Bald auch mit RP6-Infos zu Projekten---

  5. #5
    Benutzer Stammmitglied
    Registriert seit
    06.04.2008
    Ort
    Rottenburg a.N.
    Alter
    34
    Beiträge
    94
    Hi,

    scheinbar habe ich ein größeres Problem...
    Hab die gesamte Verkabelung und Lötpunkte nochmal überarbeitet und komme auf keinen grünen Zweig.
    Die Servos machen exht nicht das, was sie sollen.....

    Ich poste heute, sofern die Zeit reicht, mal noch den Code und ein Video, damit es besser zu verstehen geht...



    Danke + Gruß

    topgunfb
    Wer Interesse an Mangas hat, muss meine Website besuchen:
    http://www.manga-bl.de
    ---Bald auch mit RP6-Infos zu Projekten---

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •