/*
* File: prueva_servos.c
* Author: Enrique
*
* Created on 11 de octubre de 2014, 18:54
*/
#include <p18f2550.h>
#include <stdio.h>
#include <stdlib.h>
#include <delays.h>
#include <string.h>
#include <usart.h>
#include <math.h>
#include <float.h>
#pragma config FOSC=XT_XT, FCMEN=OFF,IESO=OFF,CPUDIV=OSC1_PLL2
#pragma config PWRT=ON,BOR=OFF,BORV=0,WDT=OFF,WDTPS=32768
#pragma config MCLRE=ON,LPT1OSC=OFF,PBADEN=OFF,CCP2MX=OFF
#pragma config STVREN=OFF,LVP=OFF,XINST=OFF,DEBUG=OFF
#pragma config CP0=OFF,CP1=OFF,CP2=OFF,CPB=OFF,CPD=OFF
#pragma config WRT0=OFF,WRT1=OFF,WRT2=OFF
#pragma config WRTB=OFF,WRTC=OFF,WRTD=OFF
#pragma config EBTR0=OFF,EBTR1=OFF,EBTR2=OFF,EBTRB=OFF
#define _XTAL_FREQ 20000000
#define LED PORTAbits.RA3
#define SERVO0 PORTBbits.RB2
#define SERVO1 PORTBbits.RB3
#define SERVO2 PORTBbits.RB4
#define SERVO3 PORTBbits.RB5
#define SERVO4 PORTBbits.RB6
#define SERVO5 PORTBbits.RB7
#define _RAND_MAX 180;
#define _RAND_MIN 0;
unsigned long T_ON[]={0,0,0,0,0,0},T_OFF=0;
float ms_ton[]={0.0,0.0,0.0,0.0,0.0,0.0};
float ms_toff=0;
int servo=0;
// tenemos 6 servos más una dimensión mas para el tiempo en off, de la posición
// 0 a la 5, son las posiciones de los 6 servos, la posición 7, siempre será 0, de
// la posición 8 a la 13 son los índices de los servos, que se inicializarán siempre
// de la misma forma como la actual. será como una matriz de 2x7
int pos[2][7]={0,0,0,0,0,0,0,0,1,2,3,4,5,6};
float b=0.5; // coeficiente "b" de la recta.
float pendiente; // pendiente de la recta para pasar la posición a "ms".
int cont=0; // variable para saber qué tiempo estamos activando.
void ordenar(){
// ordenamos la variable "pos" de menor a mayor grado, en cada columna tendremos
// la posición del motor en la primera fila, y el número del servo al que corresponde
// en la segunda fila.
int comp1,comp2,index1,index2;
for (int i=0;i<6;i++){
comp1=pos[0][i];
index1=pos[1][i];
for (int j=(i+1);j<6;j++){
comp2=pos[0][j];
index2=pos[1][j];
if (comp1>comp2){
pos[0][i]=comp2;
pos[1][i]=index2;
pos[0][j]=comp1;
pos[1][j]=index1;
comp1=comp2;
index1=index2;
}
}
}
}
void calc_tiempos(){
float time_ant;
ordenar();
pendiente=(2.5-b)/(180.0-0.0);
time_ant=0.0;
for (int i=0;i<6;i++){
// con la variable de las posiciones ordenadas, calculamos los tiempos en
// "ms" de cada una de estas posiciones, como vamos a activar todos los servos
// a la vez, cremamos una variable "time_ant" que corresponde con el tiempo
// que esta activado anteriormente ese servo. Por ejemplo: el primer servo se activa
// durante 0.8 ms, y el segundo 1 ms, con lo cual, el segundo estará 0.2 ms
// más tiempo encendido que el primero, con lo cual, este 0.2 es el que tenemos
// que almacenar, ya que los dos servos se han activado a la vez, de igual
// forma para el resto de servos.
ms_ton[i]=(pendiente*(float)pos[0][i]+b)-time_ant;
time_ant=time_ant+ms_ton[i];
// aqui el valor del TMR0 a escrivir cada vez.
T_ON[i]=65536-(unsigned long)((ms_ton[i]/1000.0)*(_XTAL_FREQ)/8.0); // 62556
}
// el tiempo en off será de 20 ms menos el tiempo del servo que este activo mas tiempo.
ms_toff=20.0-time_ant;
T_OFF=65536-(unsigned long)((ms_toff/1000.0)*(_XTAL_FREQ)/8.0); // 28516
//
// printf("Grados %d: %d, T_on=%lu, ms_on=%.2f, pendiente=%.4f\n\r",pos[1][0],pos[0][0],T_ON[0],ms_ton[0],pendiente);
// printf("Grados %d: %d, T_on=%lu, ms_on=%.2f, pendiente=%.4f\n\r",pos[1][1],pos[0][1],T_ON[1],ms_ton[1],pendiente);
// printf("Grados %d: %d, T_on=%lu, ms_on=%.2f, pendiente=%.4f\n\r",pos[1][2],pos[0][2],T_ON[2],ms_ton[2],pendiente);
// printf("Grados %d: %d, T_on=%lu, ms_on=%.2f, pendiente=%.4f\n\r",pos[1][3],pos[0][3],T_ON[3],ms_ton[3],pendiente);
// printf("Grados %d: %d, T_on=%lu, ms_on=%.2f, pendiente=%.4f\n\r",pos[1][4],pos[0][4],T_ON[4],ms_ton[4],pendiente);
// printf("Grados %d: %d, T_on=%lu, ms_on=%.2f, pendiente=%.4f\n\r",pos[1][5],pos[0][5],T_ON[5],ms_ton[5],pendiente);
// printf("T_OFF=%lu, ms_toff=%.2f, pendiente=%.4f\n\r",T_OFF,ms_toff,pendiente);
// printf("----------------------------------\n\r");
}
void putch(unsigned char byte)
{
TXSTA=0x26;
SPEN=1;
TXREG=byte; /* transmit a character to Serial IO */
while(!TXIF)continue;
TXIF=0;
}
void interrupt timer0(void){
if(INTCONbits.TMR0IF){
// inicializamos con "cont=0" para que todos los servos se activen.
// en este instante accedemos a "pos" (variable ordenada), y leemos la
// posición "cont=0" de las variables "T_ON[cont]" y "pos[1][cont], de
// esta forma accedemos a la posición del servo que menos tiempo tiene que
// estar activo, en "servo" guardamos el num del servo, y en "WriteTimer0"
// escribimos el tiempo en ON de este servo. y aumentamos la variable "cont"
// cuando se active este primer timer, accedemos a la sección de este "servo"
// ponemos a "0" la salida de dicho servo, y escrivimos en "WriteTimer0" el valor
// del siguiente servo menos activo, y guardamos en "servo" el valor del
// siguiente servo correspondiente al tiempo activado. aumentamos la variable
// "cont".... y así hasta el final.
if (cont==0){
SERVO0=1;SERVO1=1;SERVO2=1;SERVO3=1;SERVO4=1;SERVO5=1;
WriteTimer0(T_ON[cont]);
servo=pos[1][cont];
cont++;
}else if (servo==0){
SERVO0=0;
WriteTimer0(T_ON[cont]);
//printf(" servo %d, %ul\n\r",servo,T_ON[cont]);
servo=pos[1][cont];
cont++;
}else if(servo==1){
SERVO1=0;
WriteTimer0(T_ON[cont]);
//printf(" servo %d, %ul\n\r",servo,T_ON[cont]);
servo=pos[1][cont];
cont++;
}else if(servo==2){
SERVO2=0;
WriteTimer0(T_ON[cont]);
//printf(" servo %d, %ul\n\r",servo,T_ON[cont]);
servo=pos[1][cont];
cont++;
}else if(servo==3){
SERVO3=0;
WriteTimer0(T_ON[cont]);
//printf(" servo %d, %ul\n\r",servo,T_ON[cont]);
servo=pos[1][cont];
cont++;
}else if(servo==4){
SERVO4=0;
WriteTimer0(T_ON[cont]);
//printf(" servo %d, %ul\n\r",servo,T_ON[cont]);
servo=pos[1][cont];
cont++;
}else if(servo==5){
SERVO5=0;
WriteTimer0(T_ON[cont]);
//printf(" servo %d, %ul\n\r",servo,T_ON[cont]);
servo=pos[1][cont];
cont++;
}else if(servo==6){
SERVO0=0;SERVO1=0;SERVO2=0;SERVO3=0;SERVO4=0;SERVO5=0;
//printf("todos servos a 0 ------------------\n\r");
WriteTimer0(T_OFF);
cont=0;
}
INTCONbits.TMR0IF=0;
}
}
void print_servos(void){
// imprimimos las posiciones de los servos, antes de ser ordenados.
printf("Servo 0: %d grados.\r\n",pos
[0][0]); printf("Servo 1: %d grados.\r\n",pos
[0][1]); printf("Servo 2: %d grados.\r\n",pos
[0][2]); printf("Servo 3: %d grados.\r\n",pos
[0][3]); printf("Servo 4: %d grados.\r\n",pos
[0][4]); printf("Servo 5: %d grados.\r\n",pos
[0][5]); printf("-------------------\r\n"); }
void main(void) {
ADCON1 = 0xFF; // Deshabilita las entradas analogas.
TRISA=0b000110000;
TRISB=0b00000011;
TRISC = 0b11110011; //Coil in RC2 (PWM) and RC3
// inicializamos la USART
OpenUSART(USART_TX_INT_OFF &
USART_RX_INT_ON & //Activar la interrupcion por la recepcion de dato del buffer de Rx del USART
USART_ASYNCH_MODE & //Modo asincrono (fullduplex)
USART_EIGHT_BIT & //8 bits de datos
USART_CONT_RX & //Recepción continua
USART_BRGH_HIGH, 129); //9600 Baudios
// 12-77,20-129,
// inizializamos el TIMER0;
OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_2);
// TiempoTMR0 = (4/Fosc)*(2^bits - ValorTMR0)*Prescaler
// TiempoTMR0 = (4/20000000)*(2^16 - ValorTMR0)*4
RCONbits.IPEN = 1; // Enable interrupt priorities
INTCONbits.GIE = 1; // Globally enable interrupts
INTCONbits.PEIE = 1;
printf("Prueba servos con T0\r\n"); // inicializamos la variable "pos" con unoas valores aleatorios.
pos[0][0]=20;pos[0][1]=130;pos[0][2]=5;
pos[0][3]=100;pos[0][4]=90;pos[0][5]=4;
// recordad que siempre que modifiqueis las variables de las posiciones,
// estas siguientes variables se han de inicializar de igual manera.
pos[1][0]=0;pos[1][1]=1;pos[1][2]=2;
pos[1][3]=3;pos[1][4]=4;pos[1][5]=5;
// imprimimos los valores iniciales sin ordenar
print_servos();
// calculamos los tiempos para el "writetimer0"
calc_tiempos();
// inicializamos "WriteTimer0"
servo=6;
cont=0;
WriteTimer0(T_OFF);
while(1){
if (PORTAbits.RA4==0){
// random para cada uno de los 6 servos,
pos
[0][0]=rand()%180;pos
[0][1]=rand()%180;pos
[0][2]=rand()%180; pos
[0][3]=rand()%180;pos
[0][4]=rand()%180;pos
[0][5]=rand()%180; // inicalizamos los índices de los servos.
pos[1][0]=0;pos[1][1]=1;pos[1][2]=2;
pos[1][3]=3;pos[1][4]=4;pos[1][5]=5;
// imprimimos los servos
print_servos();
// calculamos los tiempos.
calc_tiempos();
while (PORTAbits.RA4==0){
Delay1KTCYx(1);
}
}
}
}
