Okay dann hier alles, was es dazu braucht. Da ich vorher alles sorgfältig gelesen habe, kann ich keinen Fehler an einem falschen Buchstaben feststellen. Aber ich hab ja auch nur 2 Augen. =)
RP6ControlServoLib.h:
RP6ControlServoLib.c:Code:/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> RP6 CONTROL * ---------------------------------------------------------------------------- * ------------------------ [c]2008 - Dirk ------------------------------------ * **************************************************************************** * File: RP6ControlServoLib.h * Version: 1.0 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz * Author(s): Dirk * **************************************************************************** * Description: * This is the RP6ControlServoLib header file. * You have to include this file, if you want to use the library * RP6ControlServoLib.c in your own projects. * * **************************************************************************** * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE! * **************************************************************************** */ /*****************************************************************************/ // Includes: #include "RP6ControlLib.h" // The RP6 Control Library. // Always needs to be included! /*****************************************************************************/ // Defines: // 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 // Servo movement limits (depending on servo type): // Standard Servos need an impulse every 20ms (50Hz). This impulse must have // a length of 1ms (0.7 .. 1ms) to move the servo lever to the left touch // and a length of 2ms (2 .. 2.3ms) for moving it to the right touch. In the // middle position the servo needs an impulse length of 1.5ms (1.3 .. 1.6ms). // If you want to modify the following constants for a certain servo type, // you must adapt the LEFT_TOUCH constant first (values ~70 .. 100 = ~0.7 .. // 1ms at 100kHz) by using a servo position value (servoX_position) of zero. // After that you have two "screws" to adjust the servo movement limits: // First you may change the RIGHT_TOUCH constant. If you choose a higher // value than 255, you will use 16-bit values. Higher values mean a longer // impulse length, but longer impulses than 2.3ms do not make sense. // Second you may alter the Timer 1 frequency constant (F_TIMER1). // A higher frequency leads to smaller steps of the servo movement. This of // course reduces the impulse length and may be compensated again by a higher // RIGHT_TOUCH constant. As a possible range of Timer 1 frequency values you // may use 50kHz (20us) .. 105.263kHz (9.5us). // HINT: If you alter F_TIMER1, you'll have to adapt LEFT_TOUCH and // RIGHT_TOUCH again as you can see in the following table! // Steps -> 9.5 10 12.5 15 17.5 20 [us] // ------------------------------------------------------------------ // LEFT_TOUCH 74 71 57 47 41 35 // RIGHT_TOUCH 169 162 129 107 92 80 // F_TIMER1 105263 100000 80000 66667 57143 50000 [Hz] #define LEFT_TOUCH 71//22 //ES-05: 22 alter miniservo: 32 // Left servo touch (~0.7ms) #define RIGHT_TOUCH 162//220//ES-05_ 220 alter miniservo: 235 // Right servo touch (~2.3ms) #define MIDDLE_POSITION (RIGHT_TOUCH / 2) // Middle position (~1.5ms) #define PULSE_REPETITION 17 // Pulse repetition freq. (~50Hz) #define F_TIMER1 100000 // Timer 1 frequency (100kHz) // 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
Code:/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> RP6 CONTROL * ---------------------------------------------------------------------------- * ------------------------ [c]2008 - Dirk ------------------------------------ * **************************************************************************** * File: RP6ControlServoLib.c * Version: 1.0 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz * Author(s): Dirk * **************************************************************************** * Description: * This is my simple RP6 Control Servo Library for up to 8 Servos. * * COMMENT: It is a good idea to use a separate power supply for the servos! * * Servo connections: * SERVO1 -> I/O Pin 7 (IO_PC2) SERVO5 -> I/O Pin 4 (IO_PC6) * SERVO2 -> I/O Pin 5 (IO_PC3) SERVO6 -> I/O Pin 1 (IO_PC7) * SERVO3 -> I/O Pin 6 (IO_PC4) SERVO7 -> I/O Pin 9 (IO_PD5) * SERVO4 -> I/O Pin 3 (IO_PC5) SERVO8 -> I/O Pin 8 (IO_PD6) * * **************************************************************************** * ATTENTION: Stopwatch 8 is used for the servo task! Please do * not use this stopwatch elsewhere in your program! * * **************************************************************************** * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE! * **************************************************************************** */ /*****************************************************************************/ // Includes: #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: /** * INIT SERVO * * Call this once before using the servo function. * Timer 1 is configured to work in "Clear Timer On * Compare Match Mode" (CTC). So no PWM is generated! * The timer runs on a fixed frequency (100kHz). * * Input: Servos -> Used Servos * Examples: * - initSERVO(SERVO1 | SERVO2) -> Use only Servos 1 and 2 * - initSERVO(SERVO1 | SERVO6) -> Use only Servos 1 and 6 * - initSERVO(SERVO1 | SERVO2 | SERVO8) -> Use Servos 1, 2 and 8 * */ 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;} // ----------------------------------------------------------- // Other possible ports for connecting Servos to RP6Control: // if (servos & SERVOx) {DDRA |= ADC6; PORTA &= ~ADC6;} // if (servos & SERVOx) {DDRA |= ADC7; PORTA &= ~ADC7;} // ----------------------------------------------------------- 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 // ------------------------------------------------------ // Possible OCR1A values (F_CPU = 16000000): // OCR1A = 2000000 / F_TIMER1 - 1 // F_TIMER1 (Steps) // OCR1A = 18; // 105263Hz (9.5us) // OCR1A = 19; // 100000Hz (10us) // OCR1A = 24; // 80000Hz (12.5us) // OCR1A = 29; // 66667Hz (15us) // OCR1A = 34; // 57143Hz (17.5us) // OCR1A = 39; // 50000Hz (20us) // ------------------------------------------------------ // Enable output compare A match interrupts: startSERVO(); sei(); startStopwatch8(); // Needed for 20ms pulse repetition } /** * START SERVO * * If the servo function was stopped with the * function stopSERVO() before, it can be * started again with this function. * */ void startSERVO(void) { TIMSK |= (1 << OCIE1A); servo_on = TRUE; } /** * STOP SERVO * * The servo function uses a certain amount of the * processor's calculating time. If the Servos are * not moving for a while, the Timer 1 interrupt * can be stopped with this function. * */ void stopSERVO(void) { TIMSK &= ~(1 << OCIE1A); servo_on = FALSE; } /** * PULSE SERVO * * This is the servo pulse generation. This function * must be called every 20ms (pulse repetition). * * position = 0 : Left touch * position = RIGHT_TOUCH : Right touch * position = MIDDLE_POSITION : Middle position * * ! Please make sure in your main program, that the ! * ! servo position values (servoX_position) don't ! * ! exceed RIGHT_TOUCH!!! ! * * COMMENT: The pulses are only started here! * The pulses end in the Timer 1 ISR! * */ void pulseSERVO(void) { if (servo_on) { intcounter = RIGHT_TOUCH; // Avoid interference of Timer 1 ISR! // (Only necessary, if pulseSERVO() is called // from outside of this library!) if (usedservos & SERVO1) { SERVO1_PULSE_ON; impulselength1 = LEFT_TOUCH + servo1_position;} if (usedservos & SERVO2) { SERVO2_PULSE_ON; impulselength2 = LEFT_TOUCH + servo2_position;} if (usedservos & SERVO3) { SERVO3_PULSE_ON; impulselength3 = LEFT_TOUCH + servo3_position;} if (usedservos & SERVO4) { SERVO4_PULSE_ON; impulselength4 = LEFT_TOUCH + servo4_position;} if (usedservos & SERVO5) { SERVO5_PULSE_ON; impulselength5 = LEFT_TOUCH + 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; } } /** * TIMER1 ISR * * In this ISR the servo pulses are finished, if the * correct pulse length of each servo is reached. * */ 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;} } /** * SERVO TASK * * This is the servo task. The task performes the pulse repetition * with the help of a stopwatch. * At the next call of the servo task (earliest about 3ms after the * last servo pulse generation) the compare A match interrupt will * be disabled to reduce the interrupt load. It will be enabled * again after the next pulseSERVO() function call. * */ void task_SERVO(void) { if (getStopwatch8() > 2) {TIMSK &= ~(1 << OCIE1A);} if (getStopwatch8() > PULSE_REPETITION) { // Pulse every ~20ms pulseSERVO(); // Servo pulse generation if (servo_on) {TIMSK |= (1 << OCIE1A);} setStopwatch8(0); } } /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * - v. 1.0 (initial release) 31.12.2008 by Dirk * * **************************************************************************** */ /*****************************************************************************/ // EOF
RP6Control_Servo.c:
Ist da Tatsächlich nur ein Buchstabe falsch?Code:// Uncommented Version of RP6ControlServo.c // written by Dirk, modyfied by Filou // ------------------------------------------------------------------------------------------ #include "RP6ControlServoLib.h" uint16_t pos = LEFT_TOUCH; int main(void) { initRP6Control(); initLCD(); showScreenLCD("################", "################"); mSleep(500); showScreenLCD("<<RP6 Control>>", "<<LC - DISPLAY>>"); mSleep(500); showScreenLCD(" Servo - Test 1 ", " Version 1.00 "); mSleep(500); clearLCD(); setLEDs(0b111111); mSleep(500); setLEDs(0b000000); initSERVO(SERVO1 | SERVO2); startStopwatch2(); int turn=1; while(true) { if (getStopwatch2() > 48) { servo1_position = pos; servo2_position = pos; setCursorPosLCD(0, 0); writeStringLCD_P("Servopos.: "); writeIntegerLCD(pos, DEC); writeStringLCD_P(" "); if(turn==0) pos=pos-4; if(turn==1) pos=pos+4; if (pos >= RIGHT_TOUCH && turn == 1) turn= 0; if (pos <= LEFT_TOUCH && turn == 0) turn=1; //pos=LEFT_TOUCH; setStopwatch2(0); } task_SERVO(); mSleep(3); } return 0; }
Grüsse Filou
[EDIT]
Ist es möglich, dass der Port D mehr belastet werden kann als Port C ?? Wenn ich den Servo als Servo 8 am IO_PD6 anschliesse, stürtzt nichts mehr ab. [/EDIT]
Zitieren
Lesezeichen