Creando "Libreria" para N cantidades de servos y control parecido arduino

[marckoe93]

hola a todos me llamo marco soy estudiante de ingenieria mecatronica en mexico.
la idea de este post es que he estado muy relacionado con arduino y hace tiempo entre a programar PIC en CCS ya habia manejado anteriormente emsanblador y realmente utilizar servos en pic se convierte tedioso, conosco los diferentes metodos para utilizar servos en pic ya se por PWM, TIMER1 ,(empleando desvordamientos)   ect , y los calculos nesesario que intervienen en ello.

y bueno dejandonos de rollos estoy a un paso de completar una funcion que nos ayude a simplificar el uso de servos asi de sencillo como la programacion en arduino.

ademas de que como los servos se rigen de un solo timer podremos meter N cantidades de servos bajo nuestro mando.

solo que me queda un pequeño gran detalle en mi programacion relacionada con la operacion y que unas son de tipo punto decimal y fraccion.

osea ya sirve solo me falta ese detalle con las variables ya probe cambiando int16,float,float32,long ect y por el momento no le he encontrado solucion.

LES DEJARE MI PROGRAMACION PARA VER SI JUNTOS PODEMOS HECHAR A ANDAR ESTE MINIPROYECTO QUE SIN LUGAR A DUDA NOS PODRA SERVIR A CADA UNO DE NOSOTROS.

A) LA PROGRAMACION ESTA EXPLICADA DETALLE A DETALLE
B) LES ADJUNTO LA PROGRAMACION Y EL SIMULADO EN PROTEUS 8.0
C) LES ADJUNTO MIS CALCULOS DE LA FUNCION

PROGRAMACION

[/size]

Código: [Seleccionar]

#include <18f4550.h>                       
#fuses XT,NOWDT,NOMCLR,PUT
#Use delay(clock= 4000000) // osc de 4Mhz
#use standard_io(b)
#include <math.h>

int contservo1=0;           //Contadores que me ayudan a controlar pulsos a los servos
int contservo2=0;
int contservo3=0;

int   controlservo1=15;    //Variables que mandan señal de posicion al servo (1ms-2ms)
int   controlservo2=15;    // 15 serian 1.5ms osease 15/10 segun como yo utilize mi timer
int   controlservo3=15;    //entonces 10 = 0grados   y 20 = 180 grados en el servo
                           //osea que inicialmente iniciaran en 90grados por que asi lo quise :)
                           // todo esto por que ise unos calculos utilizando el timer1 de 16bits
                           // ise que mi overflow fuera cada 50us abajo con mas detalles

#Define SERVO1 PIN_B1      // salidas de mis pines que van a controlar al servo
#Define SERVO2 PIN_B2
#Define SERVO3 PIN_B3

#Define BSERVO1_A PIN_C0   //botones que controlan servo 1 (podremos moverlos a 2 angulos deseados)
#Define BSERVO1_B PIN_C1

#Define BSERVO2_A PIN_C2  //botones que controlan servo 2 (podremos moverlos a 2 angulos deseados)
#Define BSERVO2_B PIN_C4

#Define BSERVO3_A PIN_C5  //botones que controlan servo 3 (podremos moverlos a 2 angulos deseados)
#Define BSERVO3_B PIN_C6



#Define LED2 PIN_B7     //este boton enciende un led en ciclo while (TRUE)
#Define BOTON PIN_B6    //no tiene nada que ver con los servos es para simular
                        //que ando corriendo una programacion en otro plano del timer1


float32 servoangulo(float32 grados){  //mi funcion servo les subo mis imagenes explicando mis calculos

float32 pulso;
float32 m;
float32 pulsomin=1000; //microsegundos
float32 pulsomax=2000; //microsegundos
float pulsosalida;

m=(180/(pulsomax-pulsomin));

pulso=((grados + (m*pulsomin))/m);

pulsosalida=(pulso/100);

return (pulsosalida);
}




#int_TIMER1             //interrupcion por TIMER1
void temp1s(){   
set_timer1(65508);      //coloco mi overflow cada 50us   
                        // si uso un osc de 4mhz, prescaler de 2, quiero que overflow sea de 50us, se que mi Timer1 es de 16bits
                        // (2^16)-(0.000050/(4/(4mhz/2)) = 65511 y se que puse ahi 65508 solo es un ajuste y cheque que exacto ahi mi overflow es de 50us
                        //realmente quiero que mi overflow sea de 50us ya que se va poder tener control de variadades de servos

if (contservo1==controlservo1){ output_toggle(SERVO1); contservo1=0;}    //50 us x controlservo1 x Kinventada =ms           Kinventada es 2 lo encontr a base de pruebas
contservo1++;                                 //incrementa contador

if (contservo2==controlservo2){ output_toggle(SERVO2); contservo2=0;}    //50 us x controlservo2 x Kinventada  =ms          Kinventada es 2 lo encontr a base de pruebas
contservo2++;                                 //incrementa contador

if (contservo3==controlservo3){ output_toggle(SERVO3); contservo3=0;}    //50 us x controlservo3 x Kinventada   =ms         Kinventada es 2 lo encontr a base de pruebas
contservo3++;                                 //incrementa contador
}

// osea que suponiendo que si yo tengo 50us x  controlservo1=20 x 2 = 2 milisegundo suponiendo, señal que le llegara a dicho servo y me daria un ang de 180

void main(){
setup_timer_1(T1_INTERNAL | T1_DIV_BY_2);  // Config del TIMER1
enable_interrupts(INT_TIMER1);             // Habilita Interrupcion por timer1
enable_interrupts(global);                 //Habilita Interrupcion Global nesesario ponerlo ojo

while(TRUE){

///////////////////////////////////////////////////        ZONA BOTON QUE ENCIENDE LED (NADA QUE VER CON SERVOS)

if(input(BOTON)==1){output_HIGH(LED2);}
else{output_low(LED2);}

///////////////////////////////////////////////////        ZONA SERVOS

if(input(BSERVO1_A)==1){controlservo1=servoangulo(30);}         //Pruebas a diferentes angulos
if(input(BSERVO1_B)==1){controlservo1=servoangulo(150);}        //se que si realiza bien el proceso pero los
                                                                //decimales en la funcion me producen error
if(input(BSERVO2_A)==1){controlservo2=servoangulo(60);}
if(input(BSERVO2_B)==1){controlservo2=servoangulo(120);}

if(input(BSERVO3_A)==1){controlservo3=servoangulo(5);}
if(input(BSERVO3_B)==1){controlservo3=servoangulo(175);}


}
}



SIMULADO EN PROTEUS CON EL CSS

DESCARGAR
http://www.mediafire.com/download/lt4g4le8d7lcd3a/MUCHOS+SERVOS+CONTROLADOS+TIMER+CON+FUNCION+SERVO.rar

CALCULOS

bueno antetodo espero tengan un excelente fin de año y que todos sus sueños para este proximo 2016 se cumplan saludos!!!!!

[juaperser1]

Código: [Seleccionar]

////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                    //
//    multi.c                                                                                           //
//    Descripcion:   programa fuente de optimizacion de codigo para mover 8 sevos von 1 solo timer    //
//    Autor: Juan Jose Perales Serrano                                                                //
//    Web: www.microrobotica.net                                                                      //
//                                                                                                    //
////////////////////////////////////////////////////////////////////////////////////////////////////////



#include <18F2550.h>
#fuses HSPLL,NOWDT,NOPROTECT,NOLVP,NODEBUG,USBDIV,PLL5,CPUDIV1,VREGEN
#use delay(clock=48000000)


//////////////////////////////////////////////////////////////////////////////
     
#priority timer1//,demas interrupciones, para dar prioridad a los servos

#use standard_io(A)                          //habilitamos los puertos como entrada salida
#use standard_io(B)
#use standard_io(C)

#define LED PIN_C1
#define SW1 PIN_C2

///////////////////////////DEFINICION DE TODOS LOS SERVOS///////////////////////
#define SERVO1 PIN_B7
#define SERVO2 PIN_B6
#define SERVO3 PIN_B5
#define SERVO4 PIN_B4
#define SERVO5 PIN_B3
#define SERVO6 PIN_B2
#define SERVO7 PIN_B1
#define SERVO8 PIN_B0
////////////////////////////////////////////////////////////////////////////////



const int16  TicksVENTANA=7500;                                              // PWM para un servo = 2.5 ms x 8 = 20 ms
const int16  TicksMINIMA=2100;                                               // PWM Position 0 = 0.7 ms 
const int16  TicksCENTRO= 4500;                                              // PWM Position 90= 1.5 ms 
const int16  TicksMAXIMA=6300;                                               // PWM Position 180= 2.1 ms

static int16 Servo_PWM[8]={TicksMINIMA,TicksMINIMA,TicksMINIMA,0,0,0,0,0};   //en este vector cargamos las posiciones de cada uno de los servos

static int8  SERVO_N=0;                                                      //variable para seleccionar el servo
static int1  SERVO1_ON=1;                                                    //variables para activar los servos
static int1  SERVO2_ON=1;
static int1  SERVO3_ON=1;
static int1  SERVO4_ON=0;
static int1  SERVO5_ON=0;
static int1  SERVO6_ON=0;
static int1  SERVO7_ON=0;
static int1  SERVO8_ON=0;
static int1  flag_Phase;
static int16 TicksPROXIMAINTERRUPCION=53036;                                    //cuenta para la proxima interrupcion, VALOR ALEATORIO PARA LA PRIMERA INTERRUPCION

int8 bandera=0;



#int_timer1
void timer1_isr(void){
 
   if(flag_Phase==0){
   
      if(SERVO_N==0 && SERVO1_ON) {output_high(SERVO1);}                         //activacion de los servos en interrupciones impares
      if(SERVO_N==1 && SERVO2_ON) {output_high(SERVO2);}
      if(SERVO_N==2 && SERVO3_ON) {output_high(SERVO3);}
      if(SERVO_N==3 && SERVO4_ON) {output_high(SERVO4);}
      if(SERVO_N==4 && SERVO5_ON) {output_high(SERVO5);}
      if(SERVO_N==5 && SERVO6_ON) {output_high(SERVO6);}
      if(SERVO_N==6 && SERVO7_ON) {output_high(SERVO7);}
      if(SERVO_N==7 && SERVO8_ON) {output_high(SERVO8);}
      TicksPROXIMAINTERRUPCION = 65535 - Servo_PWM[SERVO_N];                     //cargamos el timer con el valor en alto del servo
      set_timer1(TicksPROXIMAINTERRUPCION);
   }
   if(flag_Phase==1){
   
      if(SERVO_N==0 && SERVO1_ON) {output_low(SERVO1);}                         //desactivacion de los servos en posiciones pares
      if(SERVO_N==1 && SERVO2_ON) {output_low(SERVO2);}
      if(SERVO_N==2 && SERVO3_ON) {output_low(SERVO3);}
      if(SERVO_N==3 && SERVO4_ON) {output_low(SERVO4);}
      if(SERVO_N==4 && SERVO5_ON) {output_low(SERVO5);}
      if(SERVO_N==5 && SERVO6_ON) {output_low(SERVO6);}
      if(SERVO_N==6 && SERVO7_ON) {output_low(SERVO7);}
      if(SERVO_N==7 && SERVO8_ON) {output_low(SERVO8);}
      TicksPROXIMAINTERRUPCION = 65535 - TicksVENTANA - Servo_PWM[SERVO_N];      //cargamos el timer con el valor asta completar una ventana
      set_timer1(TicksPROXIMAINTERRUPCION);
     
     
      if(++SERVO_N>7) SERVO_N=0;
   }
   ++flag_Phase;
}

void main(void)
{
   int8 i=0,u=0;
   disable_interrupts(GLOBAL);                          //CONFIGURACIÓN INICIAL
 

   setup_timer_1(T1_INTERNAL | T1_DIV_BY_4);
   enable_interrupts(INT_TIMER1);
 
   enable_interrupts(GLOBAL);
 
   delay_ms(50);
   
      output_low(SERVO1);
      output_low(SERVO2);
      output_low(SERVO3);
      output_low(SERVO4);
      output_low(SERVO5);
      output_low(SERVO6);
      output_low(SERVO7);
      output_low(SERVO8);
     
   set_timer1(TicksPROXIMAINTERRUPCION);
   
   while(TRUE)
   {
   }
   
   
}

Aquí tienes un ejemplo en CCS de usar 8 servos con el método de la ventanas de 20ms, no se necesita ningun float ni punto decimal ni fracción ni nada.

para posicionar los servos solo debes cargar el vector "Servo_PWM[8]" y habilitar el número de servos que necesitas. el programa se encarga del resto.

un saludo

[RedPic]

Con un poco de teoría explicando el proceso (aunque esté escrito en 2008 creo que sigue siendo válido) además este fuente incluye la posibilidad de controlar los servos desde el PC vía serie ...

En el Foro :: Algoritmo para controlar hasta 8 servos con una sola interrupción (en C)

En Picmanía :: Controlando 8 Servos Con Una Sola Interrupción

[juaperser1]

Básicamente es el mismo programa pero el que yo  he puesto reducido a su máxima expresión jeje.

Un saludo

[marckoe93]

Disculpa juaperser1 entonces suponiendo que yo quisiera mover el servo 2 a 90 grados ¿como llamaría el arreglo ?para que dicho servo lo moviera? . Me imaginó que esto desde dentro del ciclo while

Y digamos tambien ¿ le afectaría si le cambiará el oscilador y el preescaler a la programación?.

Ante todo gracias llevó menos de 15 días aprendiendo PIC CSS no se si ya quiero volar haciendo mis propios funciones, lo que tengo son buenas bases de la programación en general

Agradezco la colaboración de redpic por realizar la primera entrega de dicho programa y su tutorial.

[juaperser1]

Disculpa juaperser1 entonces suponiendo que yo quisiera mover el servo 2 a 90 grados ¿como llamaría el arreglo ?para que dicho servo lo moviera? . Me imaginó que esto desde dentro del ciclo while

Y digamos tambien ¿ le afectaría si le cambiará el oscilador y el preescaler a la programación?.

Ante todo gracias llevó menos de 15 días aprendiendo PIC CSS no se si ya quiero volar haciendo mis propios funciones, lo que tengo son buenas bases de la programación en general

Agradezco la colaboración de redpic por realizar la primera entrega de dicho programa y su tutorial.

el arreglo (o vector como yo lo llamo) es Servo_PWM[8]. si quieres mover  el servo dos a posición de 90 grados pones Servo_PWM[8] = {0,0,"90",0,0,0,0,0} en cualquier parte del código, en cuando entre en el timer se actualizaran las posiciones de los servos.

en este caso todos se irán a 0 y el numero 2 se moverá a los 90 grados.

puedes actualizar las posiciones en ese arreglo desde cualquier parte del código.

Si cambias la frecuencia del pic, el prescaler y tal, si, deberás hacer las cuentas para saber a cuantos tick equivalen los grados en el servomotor.

un saludo.