void AggiornaDatiVelocita(void)
{
long tmp;
/*
* RPMruota = RPMmotore * ( RapportoRiduzioneMotore:RapportoRiduzioneAsse)
*
* MotoreX.I_MotorAxelSpeed : RPM asse motore, un motore "standard" ruota nel range 0-10000RPM, il dato è un intero.
* MotoreX.I_GearAxelSpeed : RPM uscita riduttore, considerando un rapporto riduzione minimo di 1:100 per motori da
* 10000RPM e di 1:10 per motori sotto i 3000RPM il dato varierà al max nel range 0-300
* **** Il dato GearAxelSpeed è un intero moltiplicato per un fattore 100 ******
* **** Range 0-30000 con due cifre decimali in visualizzazione ******
* I calcoli vengono effettuati passando per un LONG per evitare problemi di overflow nelle moltiplicazioni tra interi
*/
tmp = __builtin_mulss(Motore1.I_MotorAxelSpeed, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_LEFT]);
tmp *= 100; // Per ottenere I_GearAxelSpeed riscalato di un fattore 100
tmp = __builtin_divsd(tmp, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_LEFT]);
Motore1.I_GearAxelSpeed = (int) tmp;
tmp = __builtin_mulss(Motore2.I_MotorAxelSpeed, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_AXE_RIGHT]);
tmp *= 100; // Per ottenere I_GearAxelSpeed riscalato di un fattore 100
tmp = __builtin_divsd(tmp, ParametriEEPROM[EEPROM_MODBUS_ROBOT_GEARBOX_RATIO_MOTOR_RIGHT]);
Motore2.I_GearAxelSpeed = (int) tmp;
/* Calcolo la velocità di rotazione delle ruote espressa in centesimi di radianti al secondo
* MotoreX.L_WheelSpeed deriva da I_GearAxelSpeed ed è ritornato alla GUI moltiplicato per 100
*
* EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_xxxx : Raggio espresso in decimi di millimetro 360 = 3,6Cm
* */
tmp = __builtin_mulss(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_LEFT], Motore1.I_GearAxelSpeed);
tmp *= TWOPI;
tmp = __builtin_divsd(tmp, 6000);
Motore1.L_WheelSpeed = tmp;
tmp = __builtin_mulss(ParametriEEPROM[EEPROM_MODBUS_ROBOT_WHEEL_RADIUS_RIGHT], Motore2.I_GearAxelSpeed);
tmp *= TWOPI;
tmp = __builtin_divsd(tmp, 6000);
Motore2.L_WheelSpeed = tmp;
}
void __attribute__((__interrupt__,__no_auto_psv__)) _DMA0Interrupt(void)
{
indicate_loading_inter;
interrupt_save_set_corcon;
#if (RECORD_FREE_STACK_SPACE == 1)
uint16_t stack = SP_current();
if (stack > maxstack)
{
maxstack = stack;
}
#endif
#if (HILSIM != 1)
int16_t *CurBuffer = (DmaBuffer == 0) ? BufferA : BufferB;
udb_xrate.input = CurBuffer[xrateBUFF-1];
udb_yrate.input = CurBuffer[yrateBUFF-1];
udb_zrate.input = CurBuffer[zrateBUFF-1];
udb_xaccel.input = CurBuffer[xaccelBUFF-1];
udb_yaccel.input = CurBuffer[yaccelBUFF-1];
udb_zaccel.input = CurBuffer[zaccelBUFF-1];
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].input = CurBuffer[analogInput1BUFF-1];
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].input = CurBuffer[analogInput2BUFF-1];
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].input = CurBuffer[analogInput3BUFF-1];
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].input = CurBuffer[analogInput4BUFF-1];
#endif
#endif
DmaBuffer ^= 1; // Switch buffers
IFS0bits.DMA0IF = 0; // Clear the DMA0 Interrupt Flag
if (udb_flags._.a2d_read == 1) // prepare for the next reading
{
udb_flags._.a2d_read = 0;
udb_xrate.sum = udb_yrate.sum = udb_zrate.sum = 0;
udb_xaccel.sum = udb_yaccel.sum = udb_zaccel.sum = 0;
#ifdef VREF
udb_vref.sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].sum = 0;
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].sum = 0;
#endif
sample_count = 0;
}
// perform the integration:
udb_xrate.sum += udb_xrate.input;
udb_yrate.sum += udb_yrate.input;
udb_zrate.sum += udb_zrate.input;
#ifdef VREF
udb_vref.sum += udb_vref.input;
#endif
udb_xaccel.sum += udb_xaccel.input;
udb_yaccel.sum += udb_yaccel.input;
udb_zaccel.sum += udb_zaccel.input;
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].sum += udb_analogInputs[0].input;
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].sum += udb_analogInputs[1].input;
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].sum += udb_analogInputs[2].input;
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].sum += udb_analogInputs[3].input;
#endif
sample_count ++;
// When there is a chance that read_gyros() and read_accel() will execute soon,
// have the new average values ready.
if (sample_count > ALMOST_ENOUGH_SAMPLES)
{
udb_xrate.value = __builtin_divsd(udb_xrate.sum , sample_count);
udb_yrate.value = __builtin_divsd(udb_yrate.sum , sample_count);
udb_zrate.value = __builtin_divsd(udb_zrate.sum , sample_count);
#ifdef VREF
udb_vref.value = __builtin_divsd(udb_vref.sum , sample_count);
#endif
udb_xaccel.value = __builtin_divsd(udb_xaccel.sum , sample_count);
udb_yaccel.value = __builtin_divsd(udb_yaccel.sum , sample_count);
udb_zaccel.value = __builtin_divsd(udb_zaccel.sum , sample_count);
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].value = __builtin_divsd(udb_analogInputs[0].sum, sample_count);
#endif
#if (NUM_ANALOG_INPUTS >= 2)
udb_analogInputs[1].value = __builtin_divsd(udb_analogInputs[1].sum, sample_count);
#endif
#if (NUM_ANALOG_INPUTS >= 3)
udb_analogInputs[2].value = __builtin_divsd(udb_analogInputs[2].sum, sample_count);
#endif
#if (NUM_ANALOG_INPUTS >= 4)
udb_analogInputs[3].value = __builtin_divsd(udb_analogInputs[3].sum, sample_count);
#endif
}
interrupt_restore_corcon;
}
void motor_get_vind(int * u) {
u[0] =__builtin_divsd(__builtin_mulss(vind_filtered[0],64), settings.mot256[0]);
u[1] =__builtin_divsd(__builtin_mulss(vind_filtered[1],64), settings.mot256[1]);
}
void __attribute__((__interrupt__,__no_auto_psv__)) _ADCInterrupt(void)
{
indicate_loading_inter ;
interrupt_save_set_corcon ;
#if (RECORD_FREE_STACK_SPACE == 1)
unsigned int stack = WREG15 ;
if ( stack > maxstack )
{
maxstack = stack ;
}
#endif
#if (HILSIM != 1)
udb_xrate.input = xrateBUFF ;
udb_yrate.input = yrateBUFF ;
udb_zrate.input = zrateBUFF ;
#ifdef VREF
udb_vref.input = vrefBUFF ;
#endif
udb_xaccel.input = xaccelBUFF ;
udb_yaccel.input = yaccelBUFF ;
udb_zaccel.input = zaccelBUFF ;
#endif
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].input = analogInput1BUFF ;
#endif
#if (NUM_ANALOG_INPUTS == 2)
udb_analogInputs[1].input = analogInput2BUFF ;
#endif
if ( udb_flags._.a2d_read == 1 ) // prepare for the next reading
{
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].sum = 0;
#endif
#if (NUM_ANALOG_INPUTS == 2)
udb_analogInputs[1].sum = 0 ;
#endif
udb_flags._.a2d_read = 0 ;
udb_xrate.sum = udb_yrate.sum = udb_zrate.sum = 0 ;
udb_xaccel.sum = udb_yaccel.sum = udb_zaccel.sum = 0 ;
#ifdef VREF
udb_vref.sum = 0 ;
#endif
sample_count = 0 ;
}
// perform the integration:
udb_xrate.sum += udb_xrate.input ;
udb_yrate.sum += udb_yrate.input ;
udb_zrate.sum += udb_zrate.input ;
#ifdef VREF
udb_vref.sum += udb_vref.input ;
#endif
udb_xaccel.sum += udb_xaccel.input ;
udb_yaccel.sum += udb_yaccel.input ;
udb_zaccel.sum += udb_zaccel.input ;
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].sum += udb_analogInputs[0].input ;
#endif
#if (NUM_ANALOG_INPUTS == 2)
udb_analogInputs[1].sum += udb_analogInputs[1].input ;
#endif
sample_count ++ ;
// When there is a chance that read_gyros() and read_accel() will execute soon,
// have the new average values ready.
if ( sample_count > ALMOST_ENOUGH_SAMPLES )
{
udb_xrate.value = __builtin_divsd( udb_xrate.sum , sample_count ) ;
udb_yrate.value = __builtin_divsd( udb_yrate.sum , sample_count ) ;
udb_zrate.value = __builtin_divsd( udb_zrate.sum , sample_count ) ;
#ifdef VREF
udb_vref.value = __builtin_divsd( udb_vref.sum , sample_count ) ;
#endif
udb_xaccel.value = __builtin_divsd( udb_xaccel.sum , sample_count ) ;
udb_yaccel.value = __builtin_divsd( udb_yaccel.sum , sample_count ) ;
udb_zaccel.value = __builtin_divsd( udb_zaccel.sum , sample_count ) ;
#if (NUM_ANALOG_INPUTS >= 1)
udb_analogInputs[0].value = __builtin_divsd( udb_analogInputs[0].sum, sample_count ) ;
#endif
#if (NUM_ANALOG_INPUTS == 2)
udb_analogInputs[1].value = __builtin_divsd( udb_analogInputs[1].sum, sample_count ) ;
#endif
}
_ADIF = 0 ; // clear the AD interrupt
interrupt_restore_corcon ;
return ;
}
