Servos tun nicht was sie sollen
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
