Atto di moto di un'asta

[Girogiro]

..a parte il dondolio durante il moto traslatorio del robot ..che dovresti in qualche modo smorzare..
..rispetto al tuo problema tu simulassi con la coppia sul motore un pendolo sottostante che ho disegnato io ..ci vedrei meno complicazioni e accelerometri vari ..poi fai te... io torno ai miei post!

P.s
..ho scritto "simulassi" il robot rimarrebbe fatto come il tuo!
..ti dovrebbe solo rispondere ad una equazione del tipo..
Coppia motore=funzione "es. tipo pendolo"(angolo di inclinazione asse robot)

[EcoTan]

mi chiedo a che altezza sia meglio piazzare l'accelerometro (è il quadratino bianco nella foto) affinchè sia disturbato il meno possibile dalle accelerazioni orizzontali dovute alla spinta delle ruote

poiché queste piccole costruzioni sono il mio passatempo, ho fatto altre prove e mi sono reso conto che invece non conviene piazzare l'accelerometro dove l'accelerazione orizzontale è minore.
E' meglio metterlo più in basso, probabilmente perché il particolare algoritmo deduce il segno dell'accelerazione orizzontale da quella delle ruote.
O forse perché così risente meno delle imperfezioni della partenza.

[EcoTan]

Riporto il codice per poterne dare il link in un articolo.
Se le righe vengono spezzate, conviene ridurre la percentuale di zoom.

Codice: Seleziona tutto

#include "p33FJ128MC802.h"             //fusebit fastRC wPLL e tutto libero
_FOSCSEL (FNOSC_FRCPLL & IESO_ON)      //Oscillatore FastRC 7,37 MHz with PLL
#define ledr _LATA4                    // RA4 pin12 led rosso
#define ledv _LATB4                    // RB4 pin11 led verde
#define ph1 PWM1CON1bits.PEN1H      //PWM1chan1 pin25 RB14 high side enable
#define pl1 PWM1CON1bits.PEN1L      //PWM1chan1 pin26 RB15 low side enable

int ya0=2860, ya180=4173, ya90=19684, ya270=-12489;//dati da calibrazione nelle
int za0=14450, za180=-18727, za90=-1352, za270=-2799;//4 posizioni in quadratura
int xg0=-214, xg180=-180, xg90=-179, xg270=-180, xg00=-181;//00 appoggiato

int in, inn;//controllo regolarità interrupt I2C
int con, aux, seg, att=400;//contatore, ausiliarie, segno, duty compens.attriti
int dc, dcp;//duty cycle da -2000 a 2000, valore precedente
int  stato, lbyte, hbyte, pos;//variabili per colloquio I2C, encoder         
int za, ya, xg;//letture da strumentazione
float gx, gxg, gxgp;//segnale giroscopio floating, grezzo con offset, precedente
float az, ay; //segnali accelerometro floating
float g=.5, gg=.25, tilt; //gravità/2g, quadrato, inclinazione da accelerometro
float a, den;//accelerazione, denominatore
float fus, fur, furr, fuc, fup;//fusione, riferimenti, correzione, precedente
float auf, dcf;//ausiliaria, duty float   
float dy, szm, dz;//per calibrazione letture

int main (void) {             //Main Program, configurazioni
PLLFBDbits.PLLDIV=41;         // M=43 PLLDIV=41 clock oscillator Tcy=25 ns
CLKDIVbits.PLLPOST=0;         // N1=2  N2=2(default) 7.37*M/N1/N2=79Mhz~40MIPS
while (OSCCONbits.COSC != 0b001){}; // Sure for FastRC w PLL   
while(OSCCONbits.LOCK!=1) {};       // Wait for PLL to lock

TRISA = 0b1111111111101111;   //OUT RA4 pin 12 LED rosso
TRISB = 0b0000111111101111;   //OUT RB4 pin 11 LED verde

P1TCONbits.PTCKPS = 2;//PWM1 clock prescale 16
P1TPER = 999;         //PWM1 time base half period/presc.Tcy PWM 1,25 kHz motori
P1TCONbits.PTMOD = 2; //PWM1 up/down mode P1DCx= 0-2000
PWM1CON2bits.IUE = 1; //PWM1 duty cycle immediate update enable
P1TCONbits.PTEN = 1;  //PWM1 time base enabled

T1CONbits.TCKPS = 3;   //timer1 prescale 256 6,4 uS/count 0,4 sec/rollover
T1CONbits.TON = 1;     //timer1 run ON

RPINR14bits.QEA1R = 5; //remap QuadEncodA su RP5 pin 14 da encoder infrarosso
RPINR14bits.QEB1R = 6; //remap QuadEncodB su RP6 pin 15 da encoder infrarosso
//CNPU2bits.CN27PUE = 1;  //pull-up pin14 RB5 CN27 encoder A(6,8kohm)
//CNPU2bits.CN24PUE = 1;  //pull-up pin15 RB6 CN24 encoder B(6,8kohm)
QEI1CONbits.QEIM = 6;   //x4 mode, index pulse resets
DFLT1CONbits.QEOUT = 1; //digital filter enabled
DFLT1CONbits.QECK = 7;  //digital filter clock divide 256

I2C1BRG=100;//BoudRate I2C=400k (1/400.000-0,000000120)x39.600.000-2=92,3
while (TMR1 <64000) ;  //attesa circa 0,4 sec per assestamento alimentazioni
I2C1CONbits.I2CEN=1;   //abilitazione modulo per comunicazione I2C
IFS1bits.MI2C1IF = 0;  //serve!
IEC1bits.MI2C1IE = 1;  //Master events enable bit
stato=31;              //inizio configurazione giroscopio
I2C1CONbits.SEN=1;     //start bit per attivare la catena di interrupt

//guardando i led, ya positivo se pende a sinistra, za positivo a riposo,
//xg positivo se ruota a sinistra, a accelerazione verso sinistra
//aux positivo per spingere verso sinistra con trasmissione a cinghia
TMR1=0; while (TMR1 <30000) ; ledr=1;//ancora 0,2s poi led rosso prepreallarme
TMR1=0; while (TMR1 <30000) ; ledv=1;//ancora 0,2s poi verde preallarme
TMR1=0; while (TMR1 <30000) ;        //ancora 0,2s poi parte in presunto equil.

dy=ya90-ya270; szm=za0+za180; szm=szm/2.0; dz=za0-za180;//per calibraz.letture
fus=ya-ya0; fus=fus*2000.0/dy;//inizializzazione filtro
rip: TMR1=0; in=0; //loop di regolazione ogni ms
ay=ya-ya0; ay=ay/dy;//acc.y orizzontale/2g
az=za; az=(az-szm)/dz;//acc.z verticale/2g
gxg=xg-xg00;//gx*.133=mrad/sec
gx=gx+gxg-gxgp;//taglia basso su giroscopio
gx=gx-gx*.0002;//costante di tempo 1/.0002=5000 msec
gxgp=gxg;//memorizza valore precedente

den=ay*ay+az*az;//denominatore della formula
auf=den-gg;//calcola il quadrato del modulo della accelerazione
if (auf<=0.0){ a=0.0; goto til; }//evita valori erronei negativi
a=sqrtf(auf);//modulo della accelerazione in count
a=a*seg;//accelerazione con segno, positiva verso sinistra
til: tilt=(ay*g+az*a)/den;//formula inclinazione da accelerometro
tilt=asinf(tilt); tilt=1000.0*tilt;//affinata con arcoseno e espressa in mrad
//tilt=2000.0*ay;//formula semplificatissima
fus=fus+(tilt+gx*.0666-fus)*.002;//filtro fusione .0666=.5sec*.133rad/sec
//fus=fus+(tilt+gx*.0666-fus)*.004;//filtro fusione .0666=.5sec*.133rad/sec
if (con==0) { fur=fus; furr=fur; } con=1;//al primo passo imposta i riferimenti

auf=fus-fur;//errore dell'angolo da fusione rispetto al riferimento o setpoint
auf=auf+gx*.002;//azione derivativa da giroscopio anticipo=.002/.133 sec
//auf=auf-(fur-fup)*15;//depurata della variaz.del setpoint 15=.002/.001/.133
auf=auf-(fur-fup)*.1;//correzione empirica del rigo precedente
auf=auf*150;//guadagno regolazione angolo(130) azione/errore
dcf=auf+dcf*.000015;//dutycycle per az.velocità+az.accelerazione(era.15)

aux=-(int)POS1CNT;//posizione ruota da encoder
pos=aux;//memorizzata
if (pos>1 && pos<24) { ledr=1; ledv=0; }//ne segnala il valore
if (pos<-1 && pos>-24) { ledv=1; ledr=0; }//verde negativo, rosso positivo
if (pos>=24 || pos<=-24) { ledr=1; ledv=1; }//verde+rosso se elevato
if (pos>=-1 && pos<=1) { ledr=0; ledv=0; }//spenti se piccolo

auf=dcf;//ricopia dutycycle per correggere il setpoint
if(auf>2000-att)auf=2000-att;//considera il duty cycle effettivo
if(auf<-2000+att)auf=-2000+att;//compensato e limitato
auf=auf*.0005;//ne prende una piccola parte(.0015))
fuc=fuc-auf;//ne fa l'integrale negativo
fuc=fuc-fuc*.0001;//gli toglie la componente continua
furr=furr-pos*.0001;//posiz.da encoder correggerà setpoint con az.integrale
fup=fur;//memorizza setpoint prima di correggerlo
fur=furr+fuc;//applica le correzioni da dutycyle e da encoder
fur=fur-auf*.0025;//e aggiunge una azione di anticipo da dutycycle(25)
fur=fur-pos*5;//e una azione proporzionale da encoder

aux=dcf;//passa il duty cycle in integer
if(aux>0)aux=aux+att;//compensa l'attrito
if(aux<0)aux=aux-att;//anche all'indietro
if(aux>2000)aux=2000;//limita al valore realizzabile del duty cycle
if(aux<-2000)aux=-2000;//anche nel campo negativo
seg=0;//imposta il segno dell'accelerazione
if(aux > dcp)seg=1;//lo ricava dalla variazione del duty cycle
if(aux < dcp)seg=-1;//anche nel campo negativo
dcp=aux;//memorizza il duty cycle

if(aux>0) { pl1=0;ph1=1; //PWM1 abilitato high side, ponte H polarità diretta   
P1DC1=aux; }             //duty cycle da 0 a 2000
if(aux<0) { ph1=0;pl1=1; //PWM1 abilitato low side, ponte H polarità inversa
P1DC1=2000+aux; }        //duty cycle da 2000 a 0
if(aux==0) {ph1=0;pl1=0;P1DC1=1000;}// spegne se duty cycle=zero
//noo: while (TMR1 <312) ;        //attesa 312*0.0064=2msec
noo: while (TMR1 <156) ;        //attesa 156*0.0064=1msec
if (in==0) { ledr=1; inn=1; //se manca l'interrupt da I2C
ph1=0;pl1=0; goto noo; }//spegne motore e si blocca
goto rip; }                //fine di 1 ciclo di regolazione, ricomincia daccapo

//GY-521 omega[rad/sec]=gx*250*pigreca/(180*32768)=2.182*gx/16384=.000133*gx
//acc[m/sec2]=-2g*ay/32768; alfa[rad]=(circa)ay/16384
void __attribute__((no_auto_psv)) _ISRFAST _MI2C1Interrupt(void) {//ISR I2C master 
    switch(stato) {
        case 0:     //inizio
if (I2C1STATbits.P==1)stato++; else goto res;//se ozioso (idle) fa start altrimenti resetta
I2C1CONbits.SEN=1;    //start bit
  goto endd; break; 
        case 1:     //dopo start
if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
I2C1TRN=0b11010000;  //slave address GY521 x68 con bit trasmissione=0 
goto endd;  break;   
        case 2:     //se ha scritto slave address
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK scrive sub-add.reg.slave
  I2C1TRN=0b00111101; //x3D indirizzo registro accelerometro Y
goto endd; break; 
        case 3:     //se ha scritto indirizzo registro slave
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK ripete start
I2C1CONbits.RSEN=1;  //repeated start
goto endd; break; 
        case 4:     //dopo start
if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
I2C1TRN=0b11010001;  //slave address GY521 x68 con bit ricezione=1 
goto endd; break; 
        case 5:     //se era indirizzo slave lettura
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto endd; break; 
        case 6:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto primo byte fa ACK
hbyte=I2C1RCV;  //e memorizza lettura
I2C1CONbits.ACKDT=0;//predispone Ack da master
I2C1CONbits.ACKEN=1;//Ack
goto endd; break; 
        case 7:     //
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se  ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto endd; break; 
        case 8:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto secondo byte fa ACK
lbyte=I2C1RCV;  //e memorizza lettura
ya=((hbyte<<8)+lbyte);//lettura asse y accelerometro
I2C1CONbits.ACKEN=1;//Ack
goto endd; break;

        case 9:     //se era indirizzo slave lettura
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto endd; break; 
        case 10:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto primo byte fa ACK
hbyte=I2C1RCV;  //e memorizza lettura
I2C1CONbits.ACKDT=0;//predispone Ack da master
I2C1CONbits.ACKEN=1;//Ack
goto endd; break; 
        case 11:     //
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se  ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto endd; break; 
        case 12:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto secondo byte fa ACK
lbyte=I2C1RCV;  //e memorizza lettura
za=((hbyte<<8)+lbyte);//lettura asse z accelerometro
I2C1CONbits.ACKEN=1;//Ack
goto endd; break;
        case 13:     //se era indirizzo slave lettura
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto endd; break; 
        case 14:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto primo byte fa ACK
hbyte=I2C1RCV;  //e memorizza lettura
I2C1CONbits.ACKDT=0;//predispone Ack da master
I2C1CONbits.ACKEN=1;//Ack
goto endd; break; 
        case 15:     //
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se  ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto endd; break; 
        case 16:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto secondo byte fa ACK
lbyte=I2C1RCV;  //e memorizza lettura
//temp=((hbyte<<8)+lbyte);//lettura temperatura
I2C1CONbits.ACKEN=1;//Ack
goto endd; break;
        case 17:     //se era secondo byte
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura altro asse
goto endd; break; 
        case 18:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto primo byte fa ACK
hbyte=I2C1RCV;  //e memorizza lettura
I2C1CONbits.ACKEN=1;//Ack
goto endd; break; 
        case 19:     //
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res;//se  ACK
I2C1CONbits.RCEN=1;  //abilitazione lettura
goto endd; break; 
        case 20:     //
if (I2C1STATbits.RBF == 1)stato++; else goto res;//se letto secondo byte manda NACK
lbyte=I2C1RCV;  //e memorizza lettura
I2C1CONbits.ACKDT=1;//predispone NAck da master
xg=((hbyte<<8)+lbyte);        //lettura asse x giroscopio
I2C1CONbits.ACKEN=1;//NAck
goto endd; break; 
        case 21:     //dopo Nack mandato da master
I2C1CONbits.PEN=1;//Stop
stato=0; goto endd; break; 

        case 31:     //configurazione dopo start
if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
  I2C1TRN=0b11010000;  //slave address GY521 x68 con bit trasmissione=0
goto endd; break; 
        case 32:     //se ha scritto slave address
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK scrive indirizzo reg.slave
  I2C1TRN=0b01101011; //sub address registro configurazione power management x6B
goto endd; break; 
        case 33:     //se ha scritto sub address registro slave
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK manda configurazione
  I2C1TRN=0b00000001; //LSB=clock da accelerometro x
goto endd; break;
        case 34:     //se ha mandato configurazione
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK stop poi configura
I2C1CONbits.PEN=1;//Stop
goto endd; break;
        case 35:     //prosegue configurazione
if (I2C1STATbits.P==1)stato++; else goto res;//se era ozioso (idle) fa start altrimenti resetta
I2C1CONbits.SEN=1;    //start bit
goto endd; break; 
        case 36:     //dopo start
if (I2C1STATbits.S==1)stato++; else goto res;  //se fatto start altrimenti resetta
I2C1TRN=0b11010000;  //slave address GY521 x68 con bit trasmissione=0
goto endd; break; 
        case 37:     //se ha scritto slave address
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK scrive sub-add.reg.slave
  I2C1TRN=0b00011100; //sub address registro configurazione accelerometro x1C
goto endd; break; 
        case 38:     //se ha scritto sub address registro slave
if (I2C1STATbits.ACKSTAT == 0)stato++; else goto res; //se ACK manda configurazione accelerometro
I2C1TRN=0b00000000; //configurazione accelerometro, dovrebbe essere ininfluente
goto endd; break;
        case 39:     //se ha mandato configurazione
if (I2C1STATbits.ACKSTAT == 0)stato=0; else goto res; //se ACK fa stop
I2C1CONbits.PEN=1;//Stop
goto endd; break; } 

res://tentativo di reset in caso di errore nel colloquio I2C
ledr=1;//segnale rosso
I2C1CONbits.I2CEN=0;   //disabilitazione modulo per comunicazione I2C
IFS1bits.MI2C1IF = 0;  //reset int.flag
IEC1bits.MI2C1IE = 0;  //reset Master events enable bit
I2C1CONbits.I2CEN=1;   //abilitazione modulo per comunicazione I2C
IFS1bits.MI2C1IF = 0;  //serve!
IEC1bits.MI2C1IE = 1;  //Master events enable bit
stato=31;//(stato 1 dopo start?)
I2C1CONbits.SEN=1;    //start bit per riattivare la catena di interrupt
endd:; if(inn==0)in=1; IFS1bits.MI2C1IF = 0;} // fine ISR