Codice ottimizzato
Codice originale
#define DivLongS16(x) ((x) >= 0 ? (x>>4) : ((x>>4) | 0xF0000000)) // div 16 signed long
#define DivLongS128(x) ((x) >= 0 ? (x>>7) : ((x>>7) | 0xFE000000)) // div 128 signed long
#define DivLongS256(x) ((x) >= 0 ? (x>>8) : ((x>>8) | 0xFF000000)) // div 256 signed long
if (DeltaT >= 64) //ogni 64 mSec campiona la velocita'
{
// velocita' = spazio/Tempo. Dividendo millesimi di mm con millisec il risultato e' in mm/Sec
SpeedR = DivIntS64((EncoderRcount - EncoderRcountPrev) * stepR); // divide per 64 (DeltaT)
=======================
if (DeltaT >= 50) //ogni 50 mSec campiona la velocita'
{
// velocita' = spazio/Tempo. Dividendo millesimi di mm con millisecondi il risultato e' in mm/Sec
SpeedR = ((EncoderRcount - EncoderRcountPrev) * stepR) / DeltaT;
=======================
{
/*
PWM = 256 -> motore fermo
PWM = 512 -> velocita' massima FWD (circa 690 mm/sec)
PWM = 0 -> velocita' massima REW (circa -690 mm/sec)
per riportare i conti effettuati sulla velocita' in valori di PWM:
const divisore =((690-0)/(512-256)); rapporto tra range velocita' e range PWM = 2,70.
Per mantenere la precisione di due decimali nei calcoli intermedi senza usare la virgola mobile
la costante e' moltiplicata per 256, il risultato finale e' poi diviso per 256 (fixed point base 2)
Per velocizzare ancora di piu' i calcoli, invece che fare PWM / divisore
e' meglio fare PWM * (1/divisore) -> la moltiplicazione e' piu' veloce della divisione.
Quindi: 1/2,70*256 = 94,81 approssimato a 95
*/
#define divisore 95
.............................................
#define kp 40 // costante errore proporzionale (fattore P) moltiplicato 16
#define ki 24 // costante errore integrale (fattore I) moltiplicato 16
#define kd 24 // costante errore derivativo (fattore D) moltiplicato 16
.............................................
PWMR = DivLongS16((long)(ErroreR * kp + IntR + DevR)); // P + I + D
.............................................
// diviso 256 per riportare alla giusta dimensione
PWMR = DivLongS256(PWMR * divisore) + biasFwd;
SetDCPWM2(PWMR);
=======================
{
/* const divisore =((690-0)/(1023-512)); rapporto tra range velocità e range PWM = 1,35
viene moltiplicato per 100, il risultato finale è poi diviso per 100,
per mantenere la precisione di due decimali nei calcoli intermedi senza usare la virgola mobile
*/
int const divisore = 135;
.............................................
int const kp = 25; // costante errore proporzionale (fattore P) moltiplicato 10
int const ki = 15; // costante errore integrale (fattore I) moltiplicato 10
int const kd = 15; // costante errore derivativo (fattore D) moltiplicato 10
.............................................
PWMR = (ErroreR * kp + IntR + DevR) / 10; // P + I + D
.............................................
PWMR = (PWMR * 100 / divisore) + biasFwd;
SetDCPWM2(PWMR);
=======================
void Turn(long Gradi)
{
long const unGrado = 1220; // arco di circonferenza corrispondente ad un grado, in micron
.............................................
// spazio (in impulsi encoder) da percorrere
Space2RunR = unGrado * Gradi / stepR;
void Turn(long Gradi)
{
// fixed point base 2, la costante e' moltiplicata per 128, il risultato finale e' poi
// shiftato di 7 bit (diviso 128) per riportare il valore all'intero.
// Arco di circonferenza corrispondente ad un grado, in impulsi
#define unGradoR 1220/stepR*128
.............................................
// spazio (in impulsi encoder) da percorrere
Space2RunR = DivLongS128((unGradoR * Gradi));
