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