Non l'ho provato.
Ho modificato la setServo per controllare che non vengano impostati valori oltre i limiti previsti.
- Codice: Seleziona tutto
//Routine per impostare pisizione di un servo
// riceve 2 parametri:
// char servo = numero del servo da impostare. Può valere 1 o 2
// char time = durata del Ton che si desidera impostare al servo
// imdicato nel primo parametro
void setServo(unsigned char servo, unsigned char time)
{
if (time>valueLeft) time=valueLeft;
if (time<valueRight) time=valueRight;
servo--; // decremento servo di 1 quindi:
// - se valeva 1 adesso diventa 0
// - se valeva 2 adesso diventa 0
servo=servo<<1; // assegno a servo il valore di servo con shift a sinistra
// cioè moltiplico per 2:
// - se routine richiamata con servo che valeva 1 adesso vale 0
// - se routine richiamata con servo che valeva 2 adesso vale 2
intTime[servo]=time; // salvo il tempo Ton richiesto nella posizione dell'interrupt
servo++; // incremento servo di 1
intTime[servo]=200-time; // Calcolo (200-time) e salvo il temponecessario a completare 2500us
// per il servo
}
Ho creato una nuova getServo che serve a sapere la poszione attuale del servo.
- Codice: Seleziona tutto
// Routine per recuperare posizione attuale di un servo
// riceve in input il numero del servo
// ritorna la posizione attuale del servo
unsigned char getServo(unsigned char servo) {
servo--; // decremento servo di 1
servo=servo<<1; // moltiplico per 2
return intTime[servo]; // ritorno posizione attuale
}
Nel main ho poi messo un ciclo con gestione dello spostamento
- Codice: Seleziona tutto
while(1) {
// qui il ritardo in funzione della velocità desiderata
delay......
if (PL1==0) setServo(1, getServo(1)+1);
if (PL2==0) setServo(1, getServo(1)-1);
if (PL3==0) setServo(2, getServo(2)+1);
if (PL4==0) setServo(2, getServo(2)-1);
}
In questo punto devi inserire un delay in funzione della velocità di spostamento che vuoi ottenere.
Facciamo un esempio. I valori massimo e minimo di spostamento sono 131 (sinistra) e 6 (destra), questo significa che lo spostamento totale si ha con 125 valori.
Se vuoi ottenere una velocità tale che il servo si sposti da sinistra a destra in ad esempio 5 secondi dovrai inserire un delay di 5/125=0,04 = 40 ms.
Fammi sapere se funziona e se ci sono dubbi.
Ciao.