#include

#include
#include
#include
#include
#include
#include

<p18F4520.h>
<delays.h>
<adc.h>
<timers.h>
<pwm.h>
<stdio.h>
<usart.h>

#include "./tipos.h"
#include "./int_defs_C18.h"
#pragma
#pragma
#pragma
#pragma

config
config
config
config

OSC=HS
PWRT = OFF, BOREN = OFF
WDT = OFF, WDTPS = 128
PBADEN = OFF, LVP = OFF

#define BUF_SIZE 128

#define inc(x) {x++; x&=(BUF_SIZE-1);}
uint8 tx_buf[BUF_SIZE];
// TX buffer
uint8 tx_next=0; uint8 tx_sent=0; // TX indexes
uint8 msg[16];
uint8 msg_cont=0;
uint8 quad=0;
uint8 last_quad=0;
int8 vel=0;
int32 Pref=0;
int32 pos=0;
uint8 cont_servo=0;
int16 duty=512;
int16 error=0;
int32 error_int=0;
int16 error_der;
#define NPAST 32
int16 past[NPAST]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
,0};
//0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
,0,0};
uint8 err_idx=0;
float K[3]={0.0, 0.0, 0.0};
void set_pwm1(uint16 duty)
{
CCP1CONbits.DC1B0=(duty& 0x01); duty>>=1;
CCP1CONbits.DC1B1=(duty& 0x01); duty>>=1;
CCPR1L=(duty);
}

void to_motor(float dd)

{
if (dd>=1.0) dd=1.0; else if (dd<=-1.0) dd=-1.0; // dd in -1..1
// if (dd>0) dd = 528+dd*512; else if (dd<0) dd=496+dd*512; else dd=512;
// dd*=312; if(dd>0) dd+=712; else if(dd<0) dd+=300; else dd=512; // Pololu
// dd = 508+dd*512.0; //Maxon
// Maxon
dd*=500; if (dd>0) dd+=518; else if (dd<0) dd+=500; else dd+=512;
//dd = 504 + dd*500;
//dd=508+508*dd;
duty = (int16)dd;
set_pwm1(duty);
}

void compute_pid(void)
{
float ee,ed,ei;
float pid;
ee =
//ed
ed =
ei =

error; // Current error

= -vel; // Derivative error = (Pref-pos)' = -pos' = -vel
error_der;
error_int/NPAST;

pid = K[0]*ee + K[1]*ei + K[2]*ed;// + 0.02*ei;

to_motor(pid);
return;
}
void TX_isr(void)
{
if (tx_sent==tx_next) disable_TX_int;
else { TXREG=tx_buf[tx_sent]; inc(tx_sent); }
TX_flag=0;
}
void send_msg(uint8* msg,uint8 n)
{
uint8 k;
for(k=0;k<n;k++) { tx_buf[tx_next]=msg[k]; inc(tx_next);}
enable_TX_int;
}
// msg = 'A' + pos (4 bytes) + vel (1 byte) + Pref (2 bytes) + counter(1byte)+ '
Z'
void create_msg(void)
{
int16 temp;
uint8 k,*ptr;
msg[0]='A';
ptr=(uint8*)&pos; for(k=0;k<4;k++) msg[k+1]=ptr[k];

msg[5]=vel;
temp=(int16)Pref;
ptr=(uint8*)&temp; for(k=0;k<2;k++) msg[k+6]=ptr[k];
ptr=(uint8*)&duty; for(k=0;k<2;k++) msg[k+8]=ptr[k];
msg[10]=msg_cont; msg_cont++;
msg[11]='Z';
}
#define h 1
#define jump 224
// Low priority interruption
#pragma interruptlow low_ISR
void low_ISR (void)
{
uint8 temp;
uint8 p;
int16 last;
if (TX_flag && (PIE1bits.TXIE==1)) TX_isr();
if (TMR2_flag)
{
cont_servo++;
if ((cont_servo&0x0F)==0)
{
PORTCbits.RC0=1; //~PORTCbits.RC0;
temp=quad; vel=temp-last_quad;
// delta = delta counter
last_quad=temp;
if (vel>128) vel-=256;
// correct for rollover
else if (vel<-128) vel+=256;
pos=pos+vel;
// increment position
error=Pref-pos;
// Derivativo
if (err_idx==0) last=past[NPAST-1]; else last=past[err_idx-1];
error_der = (error - last);
//
p=err_idx-h; if (p>=NPAST) p-=jump; error_der-=past[p];
//
//
//

error_der=3*error;
p=err_idx-h; if (p>=NPAST) p-=jump; error_der-=(4*past[p]);
p=p-h; if (p>=NPAST) p-=jump; error_der+=past[p];
// Error integral
error_int-=past[err_idx]; error_int+=error;
past[err_idx]=error;
err_idx++; err_idx&=(NPAST-1);
compute_pid();
create_msg();
send_msg(msg,12);
PORTCbits.RC0=0;
}
TMR2_flag=0; return;

}
// Code @ 0x0018 -> Jump to ISR for Low priority interruption
#pragma code low_vector = 0x0018
void code_0x0018 (void){_asm goto low_ISR _endasm}
#pragma code
// High priority interruption
#pragma interrupt high_ISR
void high_ISR (void)
{
if (INT0_flag) // INT0 ISR (keeps track of position)
{
if (PORTBbits.RB1==PORTBbits.RB0) quad++; else quad--; // 1x encoder
INTCON2bits.INTEDG0=~ INTCON2bits.INTEDG0; // 2X encoder
INT0_flag=0;
}
}
// Code @ 0x0008 -> Jump to ISR for high priority interruption
#pragma code high_vector = 0x0008
void code_0x0008(void) {_asm goto high_ISR _endasm}
#pragma code
uint8 get_usart_speed(uint32 baud, uint32 Fosc, uint8* spbrg)
// baud rate 1200,2400,4800,9600,19200, ..., 115200
// Fosc = Frequency osc in KHz -> 20MHz = 20000
// Returns 0 (USART_BRGH_LOW), 1 (USART_BRGH_HIGH), or 2 (cannot get speed reque
sted)
// It places in spbrg the value to be used as argument spbrg in OpenUSART
{
uint16 FF;
uint8 brgh;
uint8 shift;
Fosc=Fosc*1000;
baud>>=1; Fosc=Fosc/baud+1; Fosc>>=1; // computes round(Fosc/baud)
FF=(uint16)Fosc;
if ((FF>16320)||(FF<16)) return 2;

// Baud rate too low or too high

brgh = (FF>4096)? 0:1; // Decides between BRGH=0 o BRGH=1

shift= (brgh==1)? 3:5;
FF>>=shift; FF--; FF>>=1; // Computes round(FF/64 -1) or round(FF/16-1)
*spbrg = (uint8)FF;
return brgh;
}
void main(void)
{
char ch;
uint16 veces=0;
uint8 brgh,brgh_config,sp;

duty=512; set_pwm1(duty);
for(err_idx=0;err_idx<NPAST;err_idx++) past[err_idx]=0; err_idx=0;
TRISC=0; PORTC=0;
TRISBbits.TRISB0=1; // RB0 and RB1 inputs (QUAD encoders)
TRISBbits.TRISB1=1;
// Configure PWM1 (RC2) @ 19 KHz with 1024 levels od duty cicle
OpenPWM1(255);
OpenTimer2(TIMER_INT_OFF & T2_PS_1_1 & T2_POST_1_16); // Starts TMR2 with 1:1
prescaler
// 1:16 POST
brgh=get_usart_speed(115200,20000,&sp);
OpenUSART(USART_TX_INT_OFF & USART_RX_INT_OFF & USART_ASYNCH_MODE &
USART_EIGHT_BIT & USART_BRGH_HIGH , sp);
enable_priority_levels;
enable_TMR2_int; set_TMR2_low; set_TX_low;
enable_INT0_int; INT0_low2high;
enable_global_ints;
enable_high_ints; enable_low_ints;
Pref=0;
while(1)
{
Delay10TCYx(200); veces++;
if (DataRdyUSART())
{
ch = getcUSART();
switch(ch)
{
case '7': K[0]+=0.001; break;
case '4': K[0]-=0.001; break;
case '1': K[0]=0.0; break;
case '8': K[1]+=0.0005; break;
case '5': K[1]-=0.0005; break;
case '2': K[1]=0.0; break;
case '9': K[2]+=0.01; break;
case '6': K[2]-=0.01; break;
case '3': K[2]=0.0; break;
}
}
if (veces==5000)
{
veces=0; Pref=500-Pref; // Change the targer position once in a while
}
}
}