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 ; }