8-Servo-Lib fuer das M256 WiFi - Modul
Hier mal die 8-Servo-Lib fuer das M256 WiFi - Modul.
Konnte es nicht lassen meinen Nick mit in den Comment-Header zu schreiben
Hab auch noch die Comments an sich ein bisschen angepasst.
Alle Servos koennen also an den IO_PWM/T0/T1 - Stecker angeschlossen werden:
Das ist nach belieben Veraenderbar.Code:Stecker IO_PWM/T0/T1 _______ Vdd <-|10 9| -> Servo1 Servo6 <-| 8 7| -> Servo2 Servo7 <-| 6 5| -> Servo3 Servo8 <-| 4 3| -> Servo4 Ground <-| 2 1| -> Servo5 -------
Und nicht vergessen: Externe Spannungsquelle fuer die Servos & die Masse dieser Quelle mit der Masse (Ground) des Roboters Verbinden!
RP6ControlServo.c
RP6ControlServoLib.cCode:// Uncommented Version of RP6ControlServo.c // written by Dirk / converted to WiFi Module M256 by MM2forever // ------------------------------------------------------------------------------------------ // https://www.roboternetz.de/community/threads/40090-RP6Control-M32-Library-f%C3%BCr-8-Servos/page9?p=571435#post571435 // ------------------------------------------------------------------------------------------ #include "RP6M256Lib.h" #include "RP6ControlServoLib.h" uint16_t pos = 0; uint16_t input; void demo(void) { if (getStopwatch2() > 24) { servo1_position = pos; servo2_position = pos; servo3_position = pos; setCursorPosLCD(0, 0); writeStringLCD_P("Servopos.: "); writeIntegerLCD(pos, DEC); writeStringLCD_P(" "); pos++; if (pos > RIGHT_TOUCH) {pos = 0;} setStopwatch2(0); } } int main(void) { initRP6M256(); initLCD(); showScreenLCD("################", "################"); mSleep(1500); showScreenLCD("<<RP6 Control>>", "<<LC - DISPLAY>>"); mSleep(2500); showScreenLCD(" Servo - Test 1 ", " Version 1.00 "); mSleep(2500); clearLCD(); setLEDs(0b111111); mSleep(500); setLEDs(0b000000); initSERVO(SERVO1 | SERVO2 | SERVO3); startStopwatch2(); while(true) { demo(); task_SERVO(); mSleep(3); } return 0; }
RP6ControlServoLib.hCode:/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> RP6 CONTROL * ---------------------------------------------------------------------------- * ------------------ [c]2008/2013 - Dirk / MM2forever ------------------------ * **************************************************************************** * File: RP6ControlServoLib.c * Version: 1.0 * Target: RP6 CONTROL - M256 WiFi @16.00MHz * Author(s): Dirk, MM2forever * https://www.roboternetz.de/community/threads/40090-RP6Control-M32-Library-f%C3%BCr-8-Servos/page9?p=571435#post571435 * **************************************************************************** * 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 -> PB6 (Pin 9 @ IO_PWM/T0/T1) SERVO5 -> PD4 (Pin 1 @ IO_PWM/T0/T1) * SERVO2 -> PB5 (Pin 7 @ IO_PWM/T0/T1) SERVO6 -> PK5 (Pin 8 @ IO_PWM/T0/T1) * SERVO3 -> PG5 (Pin 5 @ IO_PWM/T0/T1) SERVO7 -> PD7 (Pin 6 @ IO_PWM/T0/T1) * SERVO4 -> PD6 (Pin 3 @ IO_PWM/T0/T1) SERVO8 -> PB7 (Pin 4 @ IO_PWM/T0/T1) * * **************************************************************************** * ATTENTION: Stopwatch 1 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) {DDRB |= OC1B_PI6; PORTB &= ~OC1B_PI6;} if (servos & SERVO2) {DDRB |= OC1A_PI5; PORTB &= ~OC1A_PI5;} if (servos & SERVO3) {DDRG |= IO_OC0B; PORTG &= ~IO_OC0B;} if (servos & SERVO4) {DDRD |= IO_PD6_T1; PORTD &= ~IO_PD6_T1;} if (servos & SERVO5) {DDRD |= IO_PD4_ICP1; PORTD &= ~IO_PD4_ICP1;} if (servos & SERVO6) {DDRK |= IO_ADC13_PI21; PORTK &= ~IO_ADC13_PI21;} if (servos & SERVO7) {DDRD |= IO_PD7_T2; PORTD &= ~IO_PD7_T2;} if (servos & SERVO8) {DDRB |= OC0A_OCM_PI7; PORTB &= ~OC0A_OCM_PI7;} // ----------------------------------------------------------- // 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); TCCR1C = 0; 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(); startStopwatch1(); // 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) { TIMSK1 |= (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) { TIMSK1 &= ~(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 (getStopwatch1() > 2) {TIMSK1 &= ~(1 << OCIE1A);} if (getStopwatch1() > PULSE_REPETITION) { // Pulse every ~20ms pulseSERVO(); // Servo pulse generation if (servo_on) {TIMSK1 |= (1 << OCIE1A);} setStopwatch1(0); } } /****************************************************************************** * Additional info * **************************************************************************** * Changelog: * - v. 1.0 (initial release) 31.12.2008 by Dirk * * **************************************************************************** */ /*****************************************************************************/ // EOF
Code:/* **************************************************************************** * _______________________ * \| RP6 ROBOT SYSTEM |/ * \_-_-_-_-_-_-_-_-_-_/ >>> RP6 CONTROL * ---------------------------------------------------------------------------- * ------------------ [c]2008/2013 - Dirk / MM2forever ------------------------ * **************************************************************************** * File: RP6ControlServoLib.h * Version: 1.0 * Target: RP6 CONTROL - M256 WiFi @16.00MHz * Author(s): Dirk, MM2forever * https://www.roboternetz.de/community/threads/40090-RP6Control-M32-Library-f%C3%BCr-8-Servos/page9?p=571435#post571435 * **************************************************************************** * 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: // The Control M256 Library. #include "RP6M256Lib.h" // 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 // Left servo touch (~0.7ms) #define RIGHT_TOUCH 162 // 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 (PORTB |= OC1B_PI6) // PB6 #define SERVO1_PULSE_OFF (PORTB &= ~OC1B_PI6) #define SERVO2_PULSE_ON (PORTB |= OC1A_PI5) // PB5 #define SERVO2_PULSE_OFF (PORTB &= ~OC1A_PI5) #define SERVO3_PULSE_ON (PORTG |= IO_OC0B) // PG5 #define SERVO3_PULSE_OFF (PORTG &= ~IO_OC0B ) #define SERVO4_PULSE_ON (PORTD |= IO_PD6_T1)// PD6 #define SERVO4_PULSE_OFF (PORTD &= ~IO_PD6_T1) #define SERVO5_PULSE_ON (PORTD |= IO_PD4_ICP1)//PD4 #define SERVO5_PULSE_OFF (PORTD &= ~IO_PD4_ICP1) #define SERVO6_PULSE_ON (PORTK |= IO_ADC13_PI21)// PK5 #define SERVO6_PULSE_OFF (PORTK &= ~IO_ADC13_PI21) #define SERVO7_PULSE_ON (PORTD |= IO_PD7_T2) // PD7 #define SERVO7_PULSE_OFF (PORTD &= ~IO_PD7_T2) #define SERVO8_PULSE_ON (PORTB |= OC0A_OCM_PI7)// PB7 #define SERVO8_PULSE_OFF (PORTB &= ~OC0A_OCM_PI7) // ----------------------------------------------------------- // 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
Zitieren
Lesezeichen