Buongiorno a tutti, ho realizzato questo codice per pilotare il segnale dei servocomandi che utilizza il braccio robot 6 Dof.
Adesso vorrei implementare il numero dei servi passando graduatamente da 2 a 3 e così via fino a 6.
Ho provato a modificare il codice che mi gestisce due servi ed il relativo settaggio ,ma purtroppo non funziona,potreste darmi un'aiuto?
Credo che il problema possa essere nelle impostazioni dell'interrupt, ho dedicato una nottata a simulare il programma su carta per aggiungere un servocomando al progetto ma purtroppo non ne vengo fuori.
Questo e il programma a 2 servi e settaggio:
#include <PIC.h> // contiene i nomi mnemonici di registri e porte
__CONFIG(0x3FF9); //configurazione fuses
#include "settaggioservo2.h" // file che contiene il settaggio delle porte
#include "Delay.h"
void interrupt ISR (void)
{
if (T0IF==1) // se si e verificato un'interrupt
{
if (intNumber<byteIntTime) {
SER1=0; //azzero servo1
SER2=0; // azzero servo2
if (intTime[intNumber]==0) intNumber++;
TMR0=intTime[intNumber]; //assegno il tiemr0
if (intNumber==0) SER1=1; // se siamo nel primo periodo
if (intNumber==2) SER2=1; // se siamo nel secondo periodo
}
if (intNumber>=byteIntTime) { // se il numero di interrupt e magg.o uguale a 4
TMR0=valuePausa; // imposto il timer0 a valuePausa
}
intNumber++; //incremento di 1 il numero di interrupt
if (intNumber==maxIntNumber) // se gli interrupt sono arrivati a 16
intNumber=0; // azzero il conteggio degli interrupt per ricontare da 0
T0IF=0; // azzero il flag per nuovo interrupt
}
}
void setServo(unsigned char servo, unsigned char time) //Routine per movimento servo
{
if (time>valueLeft) time=valueLeft; // controllo che non vengano impostati valori oltre i limiti previsti
if (time<valueRight) time=valueRight; // controllo che non vengano impostati valori oltre i limiti previsti
servo--; // decremento servo di 1 quindi da 2 diventa 1
servo=servo<<1; // assegno a servo il valore di servo con shift a sinistra cioe diventa 2
intTime[servo]=time; // salvo il tempo Ton richiesto nella posizione dell'interrupt
servo++; // incremento servo di 1
intTime[servo]=356-time; //Calcolo (356-time) e salvo il temponecessario a completare 2500us
// per il servo
}
unsigned char getServo(unsigned char servo)
{
servo--; // decremento servo di 1
servo=servo<<1; // moltiplico per 2
return intTime[servo]; // ritorno posizione attuale
}
void main(void)
{
settaggio();
setServo(1, valueCenter); // inizio con il servo1 in posizione tutto dx
setServo(2, valueCenter); // inizio con il servo2 in posizione centrale
while(1)
{
DelayMs(10);
LED=0;
if (PL1==0) setServo(1, getServo(1)+1),LED=1;
if (PL2==0) setServo(1, getServo(1)-1),LED=1;
if (PL3==0) setServo(2, getServo(2)+1),LED=1;
if (PL4==0) setServo(2, getServo(2)-1),LED=1;
}
}
#define PL1 RB0 // puls1
#define PL2 RB1 // puls2
#define PL3 RB2 // puls3
#define PL4 RB3 // puls4
#define LED RA0 //led rete
#define SER1 RB4 //segnale servo 1
#define SER2 RB6 //segnale servo 2
#define numServo 2 // imposto che i servo sono 2
#define byteIntTime numServo*2 // imposto i byte di IntTime a 4 che sono i primi 4 periodi per il movimento dei servo ,e cosi? il segno * è un per(x) ?
#define maxIntNumber byteIntTime*4 // imposto a 16 il numero max di interrupt per ottenere i 20ms per il segnale
#define valueLeft 222 // posizione dx 90°
#define valueRight 102 // posizione sx 90°
#define valueCenter 162 // posizione centrale
#define valuePausa 178 // valore per pausa di 1250us
#define PIC_CLK 4000000
volatile unsigned char intNumber; //dichiaro la variabile volatile (cioe che puo essere modificata sia dall'interrupt che dal programma) di tipo char per il conteggio interrupt
volatile unsigned char intTime[byteIntTime]; //dichiaro un'array volatile di tipo char,di dimensione 4
void settaggio(void)
{
OPTION=0b10000011;
INTCON=0b10100000;
TRISA=0b00011110;
TRISB=0b00001111;
PORTB=0;
LED=0;
SER1=0;
SER2=0;
TMR0=131; // assegno al timer0 il valore 131 ,ovvero mmancano 125 cicli dei 256 per andare in overflow
intNumber=0; // assegno alla variabile di conteggio intrrupt il valore 0
}
Questo e il settaggio e codice a 3 servo che non funziona
#define SER1 RA0 //segnale servo1
#define SER2 RA1 // segnale servo2
#define SER3 RA2 // segnale servo3
#define PL1 RB0 //pulsante 1
#define PL2 RB1 //pulsante 2
#define PL3 RB2 //pulsante3
#define PL4 RB3 //pulsante4
#define PL5 RB4 //pulsante5
#define PL6 RB5 //pulsante6
#define LED RB7 //led presenza rete
#define N1 RA3
#define N2 RA4
#define N3 RB6
#define numServo 3 // imposto che i servo sono 3
#define byteIntTime numServo*2 // imposto i byte di IntTime a 6 che sono i primi 6 periodi per il movimento dei servo
#define maxIntNumber byteIntTime+10 // imposto a 16 il numero max di interrupt per ottenere i 20ms per il segnale
#define valueLeft 222 // posizione dx 90°
#define valueRight 102 // posizione sx 90°
#define valueCenter 162 // posizione centrale
#define valuePausa 178 // valore per pausa di 1250us
#define PIC_CLK 4000000
volatile unsigned char intNumber; //dichiaro la variabile volatile (cioe che puo essere modificata sia dall'interrupt che dal programma) di tipo char per il conteggio interrupt
volatile unsigned char intTime[byteIntTime]; //dichiaro un'array volatile di tipo char,di dimensione 4
void settaggio(void)
{
OPTION=0b10000000;
TRISA=0b00011000;
TRISB=0b00111111;
PORTB=0;
N1=0;
N2=0;
N3=0;
LED=0;
SER1=0;
SER2=0;
SER3=0;
TMR0=131; // assegno al timer0 il valore 131 ,ovvero mmancano 125 cicli dei 256 per andare in overflow
intNumber=0; // assegno alla variabile di conteggio intrrupt il valore 0
}
codice:
#include <PIC.h> // contiene i nomi mnemonici di registri e porte
__CONFIG(0x3FF9); //configurazione fuses
#include "settaggioservo3.h" // file che contiene il settaggio delle porte
#include "delay.h"
void interrupt ISR (void)
{
if (T0IF==1) // se si e verificato un'interrupt
{
if (intNumber<byteIntTime) {
SER1=0; //azzero servo1
SER2=0; // azzero servo2
SER3=0; // azzero servo3
if (intTime[intNumber]==0) intNumber++;
TMR0=intTime[intNumber]; //assegno il tiemr0
if (intNumber==0) SER1=1; // se siamo nel primo periodo
if (intNumber==2) SER2=1; // se siamo nel secondo periodo
if (intNumber==4) SER3=1; // se siamo nel terzo periodo
}
if (intNumber>=byteIntTime) { // se il numero di interrupt e magg.o uguale a 6
TMR0=valuePausa; // imposto il timer0 a valuePausa
}
intNumber++; //incremento di 1 il numero di interrupt
if (intNumber==maxIntNumber) // se gli interrupt sono arrivati a 16
intNumber=0; // azzero il conteggio degli interrupt per ricontare da 0
T0IF=0; // azzero il flag per nuovo interrupt
}
}
void setServo(unsigned char servo, unsigned char time) //Routine per movimento servo
{
if (time>valueLeft) time=valueLeft; // controllo che non vengano impostati valori oltre i limiti previsti
if (time<valueRight) time=valueRight; // controllo che non vengano impostati valori oltre i limiti previsti
servo--; // decremento servo di 1 quindi da 2 diventa 1
servo=servo<<1; // assegno a servo il valore di servo con shift a sinistra cioe diventa 2
intTime[servo]=time; // salvo il tempo Ton richiesto nella posizione dell'interrupt
servo++; // incremento servo di 1
intTime[servo]=356-time; //Calcolo (356-time) e salvo il temponecessario a completare 2500us
// per il servo
}
unsigned char getServo(unsigned char servo)
{
servo--; // decremento servo di 1
servo=servo<<1; // moltiplico per 2
return intTime[servo]; // ritorno posizione attuale
}
void main(void)
{
settaggio();
setServo(1, valueCenter); // inizio con il servo1 in posizione centrale
setServo(2, valueCenter); // inizio con il servo2 in posizione centrale
setServo(3, valueCenter); // inizio con il servo3 in posizione centrale
while(1)
{
DelayMs(10);
LED=0;
if (PL1==0) setServo(1, getServo(1)+1),LED=1;
if (PL2==0) setServo(1, getServo(1)-1),LED=1;
if (PL3==0) setServo(2, getServo(2)+1),LED=1;
if (PL4==0) setServo(2, getServo(2)-1),LED=1;
if (PL5==0) setServo(3, getServo(3)+1),LED=1;
if (PL6==0) setServo(3, getServo(3)-1),LED=1;
}
}