Hola a todos! En esta primera entrega completa quiero dejarles algo que a muchos les puede interesar, un mezclador de cuatro canales servo para modelismo.
Esta escrito para C18 y el proyecto armado en MPLAB X.
Básicamente, se leen 4 canales de un receptor RC estándar, se mezclan segun se necesite, salpimentar a gusto, y por último se controlan las cuatro salidas.
En el pdf adjunto está explicado el funcionamiento, la conexión y el diseño del código. La función de mezcla
calc_vel() está vacía porque tiene derechos de autor de quienes la desarrollaron para su tesis de ingeniería.
Inicialmente este dispositivo se desarrollo para hacer funcionar un pequeño robot con ruedas mecanum (esas rueditas raras q se consiguen en internet!), pero programando la función de mezcla con lo que necesiten se puede hacer cualquier cosa o manejar otro tipo de vehículos (V-tail, choper mix, etc).
Antes de ponerles el código quiero agradecer al maestro RedPic, en todos mis trabajos siempre hay un cacho de código suyo!
Por último, si quedan detalles, o hay que pulir algo que no anda bien, se debe a que tengo 2.578.466 versiones del código que se me traspapelan!
Igual un poquito de trabajo para hacer andar las cosas es divertido y enseña!
Enjoy!
main.c
#define _INIT
#include <p18f14k50.h>
#include <stdio.h>
#include <ctype.h>
#include <delays.h>
#include "variables.h"
#include "isr.h"
#pragma config FOSC = IRC
#pragma config MCLRE = OFF
#pragma config WDTEN = OFF
//#pragma config WDTPS = 32
#pragma config PWRTEN = ON
#pragma config LVP = OFF
void calc_vel(void);
void main(void)
{
ISR_disableGlobalInterrupt;
TRISA = 0b00000000;
TRISB = 0b11110000;
TRISC = 0b00000000;
/* OSCILADOR INTERNO */
OSCTUNEbits.INTSRC = 1; //31.25kHz device clock derived from 8MHzINTOSC source
OSCTUNEbits.TUN = 00000; //Oscillator is running at the calibrated freq.
OSCCONbits.IDLEN = 0; //Device enters Idle mode on SLEEP instruction
OSCCONbits.IRCF = 110; //8MHz -> INTOSC drives clock directly
OSCCONbits.OSTS = 0; //
OSCCONbits.SCS1 = 1; //Internal Oscillator block
/* ADC OFF */
ADCON0bits.ADON = 0; //ADC OFF
ANSEL = 0; //All DI/DO channels
ANSELH = 0; //All DI/DO channels
/* INIT variables*/
SERVO1 = 0;
SERVO2 = 0;
SERVO3 = 0;
SERVO4 = 0;
LED = 1;
/* INIT TIMER 0 */
INTCONbits.TMR0IE = 0; //disable TMR0 interrupt
T0CONbits.T08BIT = 0; //16 bit timer/counter
T0CONbits.T0CS = 0; //internal clock
T0CONbits.PSA = 1; //no prescaler
T0CONbits.TMR0ON = 0; //TMR0 OFF
TMR0H = 0; //clear timer
TMR0L = 0; //clear timer
/* INIT TIMER1 */
T1CONbits.RD16 = 1; //16 bit mode
T1CONbits.T1CKPS = 00; //no pescaler
T1CONbits.T1OSCEN = 0; //Timer 1 oscillator enabled
T1CONbits.TMR1CS = 0; //intenal clock Fosc/4
T1CONbits.TMR1ON = 0; //TMR1 OFF
/* INIT Interrupts */
ISR_init();
ISR_enableGlobalInterrupt;
/* INIT WatchDog */
// WDTCONbits.SWDTEN = 1;
while(1)
{
if(flag_Calc==1) calc_vel();
}
}
void calc_vel(void)
{
int i;
static int timer_LED=0;
for(i=0; i<4; i++){
Servo_PWM[i]=Receiver_PWM[i];
}
flag_Calc = 0;
if(++timer_LED == 15){ //20msx15=300ms
LED = !LED;
timer_LED=0;
}
}
variables.h
#ifndef _VARIABLES
#define _VARIABLES
#ifdef _INIT
const int Ticks4Window = 5000; // PWM Window for servo = 2.5 ms x 8 = 20 ms
const int Ticks4Minimum = 1000; // PWM High for Minimum Position = 0.7 ms
const int Ticks4Center = 2000; // PWM High for Center Position = 1.5 ms
const int Ticks4Maximum = 4500; // PWM High for Maximum Position = 2.3 ms
int Ticks4NextInterrupt;
int Servo_PWM[4]={0,0,0,0};
int Receiver_PWM[4]={500,500,500,500};
unsigned char Servo_Idx=0;
unsigned char SERVO1_ON=1;
unsigned char SERVO2_ON=1;
unsigned char SERVO3_ON=1;
unsigned char SERVO4_ON=1;
unsigned char flag_Phase=0;
unsigned char flag_Calc=0;
#else
extern const int Ticks4Window;
extern const int Ticks4Minimum;
extern const int Ticks4Center;
extern const int Ticks4Maximum;
extern int Ticks4NextInterrupt;
extern int Servo_PWM[4];
extern int Receiver_PWM[4];
extern unsigned char Servo_Idx;
extern unsigned char SERVO1_ON;
extern unsigned char SERVO2_ON;
extern unsigned char SERVO3_ON;
extern unsigned char SERVO4_ON;
extern unsigned char flag_Phase;
extern unsigned char flag_Calc;
#endif
#define SERVO1 LATCbits.LATC6
#define SERVO2 LATCbits.LATC3
#define SERVO3 LATCbits.LATC4
#define SERVO4 LATCbits.LATC5
#define LED LATCbits.LATC0
#endif
isr.c (la mas jugosa)
#include <p18f14k50.h>
#include <stdio.h>
#include "variables.h"
#include "isr.h"
/**********************************/
// Interrupt vectors //
/**********************************/
#pragma code InterruptVectorHigh = 0x08
void InterruptVectorHigh (void)
{
_asm
goto InterruptHandlerHigh //jump to interrupt routine
_endasm
}
#pragma code
#pragma code InterruptVectorLow = 0x18
void InterruptVectorlow (void)
{
_asm
goto InterruptHandlerLow //jump to interrupt routine
_endasm
}
#pragma code
/********************************/
// Funciones //
/********************************/
void ISR_init(void){
RCONbits.IPEN = 1; //habilita logica de prioridades para las interupciones
/* INIT TIMER 1 on Overflow */
PIE1bits.TMR1IE = 1; //habilita interr x timer1
IPR1bits.TMR1IP = 1; //TIMER1 Overflow Int priority HIGH
/* INIT PORTAB int on change */
//PORTB<7:4>
INTCONbits.RABIE = 1; //enable RB port cahnge int
IOCBbits.IOCB4 = 1;
IOCBbits.IOCB5 = 1;
IOCBbits.IOCB6 = 1;
IOCBbits.IOCB7 = 1;
INTCON2bits.RABIP = 0; //RB port change low priority
}
/********************************/
// Interrupt routines //
/********************************/
#pragma interrupt InterruptHandlerHigh
void InterruptHandlerHigh (){
if (PIR1bits.TMR1IF){ //check for TIMER1 overflow
PIR1bits.TMR1IF=0; //limpio INT FLAG bit
if(flag_Phase==0){
if(Servo_Idx==0 && SERVO1_ON) SERVO1 = 1;
if(Servo_Idx==1 && SERVO2_ON) SERVO2 = 1;
if(Servo_Idx==2 && SERVO3_ON) SERVO3 = 1;
if(Servo_Idx==3 && SERVO4_ON) SERVO4 = 1;
// LATCbits.LATC2 = 1;
Ticks4NextInterrupt = 65535 - Servo_PWM[Servo_Idx];
TMR1H = Ticks4NextInterrupt >> 8;
TMR1L = Ticks4NextInterrupt;
flag_Phase=1;
}
else if(flag_Phase==1){
if(Servo_Idx==0 && SERVO1_ON) SERVO1 = 0;
if(Servo_Idx==1 && SERVO2_ON) SERVO2 = 0;
if(Servo_Idx==2 && SERVO3_ON) SERVO3 = 0;
if(Servo_Idx==3 && SERVO4_ON){SERVO4 = 0;
T1CONbits.TMR1ON = 0; //apago hasta el pròximo ciclo, sincroniza con el receptor
Servo_Idx=-1;} //para que empiece de cero
// LATCbits.LATC2 = 0;
TMR1H = 0xFF;
TMR1L = 0xF0; //tiempo muy corto hasta el pròximo pulso
flag_Phase=0;
++Servo_Idx;
}
}
}
#pragma interruptlow InterruptHandlerLow
void InterruptHandlerLow (){
int aux, auxH;
if(INTCONbits.RABIF){ //check for RB Port change int flag bit
// LED = !LED;
if(Servo_Idx==0){
T0CONbits.TMR0ON = 1; //enciende TMR 0
TMR1H = 0xB1; //set timer1 high byte 10ms
TMR1L = 0xDF; //set timer1 low byte 10ms
T1CONbits.TMR1ON = 1; //enciende TMR 1
}
if(Servo_Idx==1 || Servo_Idx==2 ||Servo_Idx==3){
aux = (int) TMR0L;
auxH= (int) TMR0H << 8;
TMR0H = 0; //clear timer
TMR0L = 0; //clear timer
Receiver_PWM[Servo_Idx-1] = auxH |aux;
}
if(Servo_Idx==4){
aux = (int) TMR0L;
auxH= (int) TMR0H << 8;
T0CONbits.TMR0ON = 0; //apaga TMR 0
TMR0H = 0; //clear timer
TMR0L = 0; //clear timer
Receiver_PWM[Servo_Idx-1] = auxH |aux;
flag_Calc = 1;
// ClrWdt();
}
++Servo_Idx;
if(Servo_Idx==5) Servo_Idx=0;
aux=PORTB; //to avoid mismatch
Nop(); //to avoid mismatch
INTCONbits.RABIF = 0; //clear interrupt flag
}
}
isr.h
#ifndef _ISR
#define _ISR
void ISR_init(void);
void InterruptHandlerHigh (void);
void InterruptHandlerLow (void);
#define ISR_enableGlobalInterrupt INTCONbits.GIEH=1;INTCONbits.GIEL=1;
#define ISR_disableGlobalInterrupt INTCONbits.GIEH=0;INTCONbits.GIEL=0;
#endif
