* Comunicación por el puerto RS232 a 57600bps, pero puede aumentarse a 115200bps
* La forma de control de los servos es secuencial, lo que no permite mover dos servos de forma simultánea.
* El formato de instrucción es [servo][valor en grados][velocidad]
-Para servo usamos un byte char [a|b|c|d|e|f]
-Para valor en grados usamos tres bytes ejemplos [090] [001] [175] 90°, 1°, y 175° respectivamente
* Envia códigos de error cuando son suministrados los datos por el puerto serial
Códigos de mensajes de error
0 -- Buffer lleno Se han enviado demasiados caracteres
1 -- turnPLus error no puedo mover servo seleccionado
2 -- turnMinus error no puedo mover servo seleccionado
3 -- commandProcess error o puedo elegir ese servo
Cuando el proceso termina satisfactoriamente se manda la cadena "ok"
Para definir los servos a mover, usamos las siguientes macros definidas al inicio del código
//Definiciones de servos
#define SERVO1 LATBbits.LATB0
#define SERVO2 LATBbits.LATB1
#define SERVO3 LATBbits.LATB2
#define SERVO4 LATBbits.LATB3
#define SERVO5 LATBbits.LATB4
#define SERVO6 LATBbits.LATB5
#define SERVO7 LATBbits.LATB6
#define SERVO8 LATBbits.LATB7
#define CSERVO1 TRISBbits.TRISB0
#define CSERVO2 TRISBbits.TRISB1
#define CSERVO3 TRISBbits.TRISB2
#define CSERVO4 TRISBbits.TRISB3
#define CSERVO5 TRISBbits.TRISB4
#define CSERVO6 TRISBbits.TRISB5
#define CSERVO7 TRISBbits.TRISB6
#define CSERVO8 TRISBbits.TRISB7
Una característica mas, es que no es necesario usar los 8 pines como salida, sino que podemos elegir mediante el "bit"
boolean SERVO1_ON = true;
boolean SERVO2_ON = true;
boolean SERVO3_ON = true;
boolean SERVO4_ON = true;
boolean SERVO5_ON = true;
boolean SERVO6_ON = true;
boolean SERVO7_ON = true;
boolean SERVO8_ON = true;
Si necesitamos usar dos servos, un servo o los 8, como gustes Very Happy, (Esto tambien es original del código de RedPIC)
/* ----------------------------------------------------------------------- */
/* Plantilla generada por Piklab */
/*
La configuracion del micro nos permite trabajar un con XT de 20MHZ
y pero usar el PLL con el propósito de alcanzar los 12MIPS
Códigos de mensajes de error
0 -- Buffer lleno Se han enviado demasiados caracteres
1 -- turnPLus error no puedo mover servo seleccionado
2 -- turnMinus error no puedo mover servo seleccionado
3 -- commandProcess error o puedo elegir ese servo
ok -- Proceso terminado correctamente
*/
#include <pic18f2550.h>
#include <delay.h>
#include <uart18f2550.c>
#include <stdlib.h>
#include <string.h>
/* ----------------------------------------------------------------------- */
/* Bits de configuración: adapte los parámetros a su necesidad */
code char at __CONFIG1L CONFIG1L = _PLLDIV_DIVIDE_BY_5__20MHZ_INPUT__1L & _CPUDIV__OSC1_OSC2_SRC___1__96MHZ_PLL_SRC___2__1L & _USBPLL_CLOCK_SRC_FROM_96MHZ_PLL_2_1L;
code char at __CONFIG1H CONFIG1H = _OSC_HS__HS_PLL__USB_HS_1H & _FCMEN_ON_1H & _IESO_ON_1H;
code char at __CONFIG2L CONFIG2L = _PUT_ON_2L & _BODEN_ON_2L & _BODENV_4_2V_2L & _VREGEN_ON_2L;
code char at __CONFIG2H CONFIG2H = _WDT_DISABLED_CONTROLLED_2H & _WDTPS_1_32768_2H;
code char at __CONFIG3H CONFIG3H = _CCP2MUX_RC1_3H & _PBADEN_PORTB_4_0__CONFIGURED_AS_DIGITAL_I_O_ON_RESET_3H & _LPT1OSC_ON_3H & _MCLRE_MCLR_OFF_RE3_ON_3H;
code char at __CONFIG4L CONFIG4L = _STVR_ON_4L & _LVP_OFF_4L & _ENHCPU_OFF_4L & _BACKBUG_OFF_4L;
code char at __CONFIG5L CONFIG5L = _CP_0_OFF_5L & _CP_1_OFF_5L & _CP_2_OFF_5L & _CP_3_OFF_5L;
code char at __CONFIG5H CONFIG5H = _CPB_OFF_5H;
code char at __CONFIG6L CONFIG6L = _WRT_0_OFF_6L & _WRT_1_OFF_6L & _WRT_2_OFF_6L & _WRT_3_OFF_6L;
code char at __CONFIG6H CONFIG6H = _WRTC_OFF_6H & _WRTB_OFF_6H;
code char at __CONFIG7L CONFIG7L = _EBTR_0_OFF_7L & _EBTR_1_OFF_7L & _EBTR_2_OFF_7L & _EBTR_3_OFF_7L;
code char at __CONFIG7H CONFIG7H = _EBTRB_OFF_7H;
//Definiciones de servos
#define SERVO1 LATBbits.LATB0
#define SERVO2 LATBbits.LATB1
#define SERVO3 LATBbits.LATB2
#define SERVO4 LATBbits.LATB3
#define SERVO5 LATBbits.LATB4
#define SERVO6 LATBbits.LATB5
#define SERVO7 LATBbits.LATB6
#define SERVO8 LATBbits.LATB7
#define CSERVO1 TRISBbits.TRISB0
#define CSERVO2 TRISBbits.TRISB1
#define CSERVO3 TRISBbits.TRISB2
#define CSERVO4 TRISBbits.TRISB3
#define CSERVO5 TRISBbits.TRISB4
#define CSERVO6 TRISBbits.TRISB5
#define CSERVO7 TRISBbits.TRISB6
#define CSERVO8 TRISBbits.TRISB7
typedef enum{false = 0, true = 1} boolean; //para definir variables booleanas
//boolean tuza = true; este es solo un ejemplo
//Constantes para la definición de ventanas
const unsigned Ticks4WindowH = 0x75;
const unsigned Ticks4WindowL = 0x2F;
const unsigned Ticks4CenterH = 0x46;
const unsigned Ticks4CenterL = 0x4F;
const unsigned Ticks4MinimumH = 0x17;
const unsigned Ticks4MinimumL = 0x6F;
const unsigned Ticks4MaximumH = 0x6B;
const unsigned Ticks4MaximumL = 0xCF;
static unsigned int Ticks4NextInterruptH = 0xCF;
static unsigned int Ticks4NextInterruptL = 0x2C;
const unsigned Ticks4Window = 0x752F;
const unsigned Ticks4Minimum = 0x176F;
const unsigned Ticks4Center = 0x464F;
const unsigned Ticks4Maximum = 0x6BCF;
static unsigned int Ticks4NextInterrupt = 0xCF2C;
static unsigned int Servo_PWM[8];
static unsigned int tempom=0x0000;
static unsigned int tempo=0x0000;
static unsigned int tempol;
static unsigned int tempoh;
static unsigned int Servo_PWMH[8];
static unsigned int Servo_PWML[8];
static unsigned short Servo_Idx = 0;
boolean SERVO1_ON = true;
boolean SERVO2_ON = true;
boolean SERVO3_ON = true;
boolean SERVO4_ON = true;
boolean SERVO5_ON = true;
boolean SERVO6_ON = true;
boolean SERVO7_ON = true;
boolean SERVO8_ON = true;
//definicion de teclas
#define INTRO 0x0D
#define RETROCESO 0x08
#define ESCAPE 0x1B
boolean flag_Phase;
unsigned short j=0xFF;
unsigned int posI;
/*Seccion para la comunicacion en serial*/
#define lenbuff 10
unsigned char xbuff=0x00;
unsigned char cbuff[lenbuff];
char rcvchar=0x00;
boolean flagcommand = false;
//variables para timers
unsigned char CounterA;
unsigned char CounterB;
//Prototipos
void init_board(void);
void EnableTimer1Interrupt(void);
void Tmr1Carga(unsigned char high, unsigned char low);
void initBuff(void);
void add2cbuff(unsigned char c);
void echosel(char c);
void command_process(void);
void delay_ms(int milis);
void delay_us(int milis);
unsigned int SelectServo(unsigned char Servo);
void turnPlus(int vueltas, unsigned char vel, unsigned char Servo);
void turnMinus(int vueltas, unsigned char vel,unsigned char Servo);
//Sección de interrupciones
static void isr_Tmr1() interrupt 1 //Esta es una interrupcion de alta prioridad
{
PIE1bits.RCIE=0; //deshabilitamos la interrupcion del puerto serie
if(flag_Phase == 1)
{
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;}
if(Servo_Idx == 4 && SERVO5_ON) {SERVO5 = 1;}
if(Servo_Idx == 5 && SERVO6_ON) {SERVO6 = 1;}
if(Servo_Idx == 6 && SERVO7_ON) {SERVO7 = 1;}
if(Servo_Idx == 7 && SERVO8_ON) {SERVO8 = 1;}
Ticks4NextInterruptH = 0xFF - Servo_PWMH[Servo_Idx];
Ticks4NextInterruptL = 0xFF - Servo_PWML[Servo_Idx];
Tmr1Carga(Ticks4NextInterruptH,Ticks4NextInterruptL);
}
if(flag_Phase == 0)
{
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;}
if(Servo_Idx == 4 && SERVO5_ON) {SERVO5 = 0;}
if(Servo_Idx == 5 && SERVO6_ON) {SERVO6 = 0;}
if(Servo_Idx == 6 && SERVO7_ON) {SERVO7 = 0;}
if(Servo_Idx == 7 && SERVO8_ON) {SERVO8 = 0;}
Ticks4NextInterruptH = 0xFF - Ticks4WindowH;
Ticks4NextInterruptL = 0xFF - Ticks4WindowL;
if(Ticks4NextInterruptL + Servo_PWML[Servo_Idx] > 0xFF)
{
Ticks4NextInterruptL = Ticks4NextInterruptL + Servo_PWML[Servo_Idx];
Ticks4NextInterruptH = Ticks4NextInterruptH + Servo_PWMH[Servo_Idx] + 1;
}
else
{
Ticks4NextInterruptL = Ticks4NextInterruptL + Servo_PWML[Servo_Idx];
Ticks4NextInterruptH = Ticks4NextInterruptH + Servo_PWMH[Servo_Idx];
}
Tmr1Carga(Ticks4NextInterruptH, Ticks4NextInterruptL);
if(++Servo_Idx > 7 ) Servo_Idx = 0;
}
if(++flag_Phase > 1) flag_Phase = 0;
//Esta instruccion se esta ejecutando cada 13ms con un xtal de 20MHz
LATBbits.LATB7^=1; //complementamos el valor del bit RB7
PIR1bits.TMR1IF=0; //limpiamos la bandera de interrupcion
PIE1bits.RCIE=1; //habilitamos la interrupcion del puerto serie
}
static void isr_Serial() interrupt 2 //Esta es una interrupcion de baja prioridad
{
PIR1bits.RCIF=0;
rcvchar=RCREG;
add2cbuff(rcvchar);
echosel(rcvchar);
}
//Funcion Principal
void main() {
//declaracion de variables
int i=0;
int button_down=1;
int valTimer=0;
RCONbits.IPEN=1; //interrupciones por prioridad habilitadas
init_board();
Print_str("Dispositivo inicializado");
Printf(INTRO);
Print_str("PIC18F2550 en SDCC");
Printf(INTRO);
delay_ms(500); //tiempo de espera
initBuff();
for (i=0;i<9;i++)
{
Servo_PWMH[i] = Ticks4CenterH;
Servo_PWML[i] = Ticks4CenterL;
}
EnableTimer1Interrupt();
tempo = Ticks4Center;
tempoh = tempo & 0xFF00;
tempol = tempo & 0x00FF;
Tmr1Carga(tempoh, tempol);
posI = Ticks4Center;
while (true)
{
if(flagcommand) //si ya presionaste enter
{
command_process();
}
}
}
void init_board(void) {
flagcommand= false;
ADCON1=0x7; // PORTA used for digital I/O
TRISAbits.TRISA4=true; // configure PORTA4 for input (pushbutton)
/* Configuramos las salidas de los servomotores*/
CSERVO1 = false;
CSERVO2 = false;
CSERVO3 = false;
CSERVO4 = false;
CSERVO5 = false;
CSERVO6 = false;
CSERVO7 = false;
CSERVO8 = false;
//Configuramos la comunicacion serial --rutina de jean Pierre Mandon
setup_UART(B57600);
}
void EnableTimer1Interrupt(void)
{
T1CON=1; //Timer1 preescaler 1 interno, carga de 8 bits
PIE1bits.TMR1IE=1;
INTCONbits.PEIE=1;
INTCONbits.GIE=1;
}
void Tmr1Carga(unsigned char high, unsigned char low)
{
TMR1H=high;
TMR1L=low;
}
//Funcion de vaciado del buffer
void initBuff(void){
int i;
for (i=0; i< lenbuff;i++){
cbuff[i]=0x0;
}xbuff=0x00;
}
//agrega un caracter mas al buffer, o realiza un acción
void add2cbuff(unsigned char c)
{
if(xbuff<lenbuff){ //Añade caracteres al buffer mientras no se llene el buffer
switch(c){
case INTRO: //Enter --> Habilita flag para procesar comando
flagcommand=true;
break;
case RETROCESO: //BackSpace --> Borra del buffer el ultimo caracter insertado
if(xbuff>0) cbuff[--xbuff]=0x00;
break;
case ESCAPE: //ESC --> Borra todo el buffer
initBuff();
break;
default: //añade caracter al buffer recibido
cbuff[xbuff++]=c;
}
}
else{
initBuff();//Si el buffer esta lleno, vacialo
Print_str("0"); //Envia mensaje de que el buffer esta lleno Error 0
Printf(INTRO);
}
}
//Solo imprime los caracteres imprimibles
void echosel(char c)
{
switch(c){
case 0x0D: //Si presionamos la tecla intro
Printf(INTRO);
break;
case 0x08: //Si presionamos la tecla BackSpace
Printf(RETROCESO);
break;
case 0x1B: //Si presionamos la tecla ESC
Printf(ESCAPE);
break;
default:
Printf(c); //Echo de cualquier otro caracter
}
}
/*
El formato de los comandos es el siguiente:
[a...f] [123] [xx]
servo posicion velocidad
primer byte un caracter alfabético de entre a y e que define al servo
tres bytes para definir la posicion del servo
dos bytes para definir la velocidad de movimiento del servo
a12301 {enter}
mueve el servo 1 a 123 grados de la posicion de origen a la velocidad 01
*/
void command_process()
{
int i=0;
char DerIzq= 1; //bandera para saber si va a 180 o a 0 grados
// DerIzq = 1 Indicará que vamos de 0 a 180 por tanto suma
// DerIzq = 0 Indicará que vamos de 180 a 0 por tanto resta
unsigned char cmd[lenbuff]; //comando tecleado
unsigned char servo[2];
unsigned char pos[4]; //para almacenar las posiciones
unsigned char posicion;
unsigned char velocidad[4]; //para almacenar los incrementos
unsigned char vel;
unsigned int posF; //Es posible que estos valores los tengas que dividir
unsigned int vueltas;
flagcommand=false;
//Recibe el nombre del servo a mover
strcpy(cmd,cbuff);
strncpy(servo,cmd,1);
//guarda la posicion del servo
strcpy(cmd,cbuff);
for(i=1;i<4;i++){
pos[i-1]= cmd[i];
}
posicion = atoi(pos);
//guarda la velocidad a la que se moverá el servo
strcpy(cmd,cbuff);
for(i=4;i<7;i++)
{
velocidad[i-4]=cmd[i];
}
vel = atoi(velocidad);
tempo = 0;
tempoh=0;
tempol=0;
switch(servo[0]){
case 'a':
posI=SelectServo(0);
break;
case 'b':
posI=SelectServo(1);
break;
case 'c':
posI=SelectServo(2);
break;
case 'd':
posI=SelectServo(3);
break;
case 'e':
posI=SelectServo(4);
break;
case 'f':
posI=SelectServo(5);
break;
default:
Print_str("3"); //Error 3 opcion elegida no válida
Printf(INTRO);
break;
}
posF= 106.66 * posicion + 8399;
if(posF>posI){
vueltas = posF - posI;
vueltas = vueltas / vel;
DerIzq = 1;
}
else
{
vueltas = posI - posF;
vueltas = vueltas/ vel;
DerIzq = 0;
}
/********************************************************/
/*
Debe de leerse el valor que tiene del servo desde el array
Žpara determinar el valor inicial y no unicamente dejarlo
como valor final. esto causa el eeror de que al mover
multiples motores con una unica variable se moveran en conse-
cuencia del anterior
*/
// posI = posF; Esto no es correcto
if(DerIzq)
{
switch(servo[0])
{
case 'a': //mueve al servo uno en incrementos de vel hasta llegar a vueltas
turnPlus(vueltas, vel, 0);
break;
case 'b': //mueve al servo dos en incrementos de vel hasta llegar a vueltas
turnPlus(vueltas, vel, 1);
break;
case 'c':
turnPlus(vueltas, vel, 2);
break;
case 'd':
turnPlus(vueltas, vel, 3);
break;
case 'e':
turnPlus(vueltas, vel, 4);
break;
default:
Print_str("1"); Printf(INTRO); //Error 1 turnPlusError
}
}
else{
switch(servo[0])
{
case 'a': //mueve al servo uno en incrementos de vel hasta llegar a vueltas
turnMinus(vueltas,vel,0);
break;
case 'b': //mueve al servo dos en incrementos de vel hasta llegar a vueltas
turnMinus(vueltas,vel,1);
break;
case 'c':
turnMinus(vueltas,vel,2);
break;
case 'd':
turnMinus(vueltas,vel,3);
break;
case 'e':
turnMinus(vueltas,vel,4);
break;
default:
Print_str("2");Printf(INTRO); // error 2 turnMinus Error
}
}
initBuff();
Print_str("ok");Printf(INTRO);
}
void delay_ms(int milis){
delay1ktcy(10*milis);
}
void delay_us(int milis){
delay10tcy(1*milis);
}
unsigned int SelectServo(unsigned char Servo)
{
tempoh = Servo_PWMH[Servo];
tempo = tempoh << 8;
return (tempo | Servo_PWML[Servo]);
}
void turnPlus(int vueltas, unsigned char vel, unsigned char Servo)
{
int i=0;
for(i=0;i<vueltas;i++)
{
tempoh = Servo_PWMH[Servo];
tempoh = tempoh << 8;
tempo = tempoh | Servo_PWML[Servo];
tempo += vel;
Servo_PWML[Servo] = tempo & 0x00FF;
tempoh = tempo & 0xFF00;
tempoh = tempoh >> 8;
Servo_PWMH[Servo] = tempoh;
delay_us(100);
}
}
void turnMinus(int vueltas, unsigned char vel, unsigned char Servo)
{
int i=0;
for(i=0;i<vueltas;i++)
{
tempoh = Servo_PWMH[Servo];
tempoh = tempoh << 8;
tempo = tempoh | Servo_PWML[Servo];
tempo -= vel;
Servo_PWML[Servo] = tempo & 0x00FF;
tempoh = tempo & 0xFF00;
tempoh = tempoh >> 8;
Servo_PWMH[Servo] = tempoh;
delay_us(100);
}
}