void rotate_stepper2(unsigned long steps, unsigned char direction, unsigned long frequency, unsigned char mode){ if(mode){ // Full Stepper Mode if(direction){ // ClockWise Direction in Full Stepper Mode while(steps > 0){ if(pos_stepper2 > 3) pos_stepper2 = 0; STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_full2[pos_stepper2++]; __delay32(frequency); steps--; } }else{ // CounterClockWise Direction in Full Stepper Mode while(steps > 0){ if(pos_stepper2 < 0) pos_stepper2 = 3; STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_full2[pos_stepper2--]; __delay32(frequency); steps--; } } }else{ // Half Stepper Mode if(direction){ // ClockWise Direction in Half Stepper Mode while(steps > 0){ if(pos_stepper2 > 7) pos_stepper2 = 0; STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_half2[pos_stepper2++]; __delay32(frequency); steps--; } }else{ // CounterClockWise Direction in Half Stepper Mode while(steps > 0){ if(pos_stepper2 < 0) pos_stepper2 = 7; STEPPERS_PORT = (STEPPERS_PORT|0xF0)& step_half2[pos_stepper2--]; __delay32(frequency); steps--; } } } }
/******************************************************************** * Function Name: sendnRFstring * * Parameters: pointer to string, length of string. * * Description: Writes a string to the transmit FIFO of nRF * * and toggels the CE line to start * * transmission. * * * ********************************************************************/ void sendnRFstring(unsigned char *nRFstring, int size) { LDPageWriteSPI(0xA0, nRFstring, size); __delay32(200); // Delay for >10us _LATB15 = 1; __delay32(200); // Delay for >10us _LATB15 = 0; }
void PWMEnable() // TODO: metti un commento, please { // IDLE to 50% PWM PDC1 = PDC2 = PDC3 = LOOPINTCY/2; FLTACON = 0; // L pwm out high P1OVDCON = 0x0015; // allows driver boost capacitors to charge __delay32(24); // 8 //P1OVDCON = 0x0000; // clear fault flag before start fault mechanism IFS3bits.FLTA1IF = 0; P1OVDCON = 0x3f00; SysStatus.PWM_is_disabled = 0; // Enable PWM generation PTCONbits.PTEN = 1; OverCurrentFaultIntEnable(); // PWM fault configuration // pin pair 1,2 and 3 are controlled by Fault Input A #ifdef ENABLE_OVER_CURRENT_PROTECTION FLTACON = 7; #else FLTACON = 0; // fault disabled #endif }
void delay_ms(U32 u32ms) { U32 u32ticks; u32ticks = ( ((U32)u32ms * 1000) / cTcy_ns) * 1000; __delay32(u32ticks); }
void delay_us(U32 u32us) { U32 u32ticks; u32ticks = ( ((U32)u32us * 1000) / cTcy_ns); __delay32(u32ticks); }
void ADCDoOffsetCalibration(void) // ADC Initialization: // Performs offset calibration for ISENSES and initial base value acquisition for vdclink. // Then setup ADC for for sequential sampling on CH0=AN0, CH1=AN1, CH2=AN2. // without PWM sync and DMA, value and stored in MeasCurrParm.Offseta,b signed fractional form. { int i; int ret; int led; // Configure ADC registers for calibration without PWM sync and DMA ADCConfigureRegistersForCalibration(); // Execute calibration and store values for(i=0;i<10000;i++){// delay 1s __delay32(4000); //delay 100us } led = LED_status.RedBlinkRate; while(1){ ret = ADCCalibrate() ; if(-1 == ret){ /* AD is not working properly. * loop forever if fatal error * condition */ SysError.ADCCalFailure = 1; LED_status.RedBlinkRate=BLINKRATE_STILL; while(1); }else if(-2 == ret){ /* VDCLink is too low. * enlight red led, then * wait forever for VDClink to * raise */ SysError.ADCCalFailure = 1; LED_status.RedBlinkRate=BLINKRATE_STILL; // break; continue; } LED_status.RedBlinkRate = led; return; } }
int main() { TRISBbits.TRISB7 = 0; while(1) { LATBbits.LATB7 = ~PORTBbits.RB7; __delay32(1500000); } }
static void s_led_flash(void) { unsigned int i; for(i=1;i<=20;i++) { LATAbits.LATA3 = ~LATAbits.LATA3; __delay32(800000); } }
void ByteReadSPI2(unsigned char Add, unsigned char *rdptr, unsigned char length ) { DS_CS_2 = 0; // Select Device WriteSPI2( Add ); // WRITE word address to EEPROM getsSPI2( rdptr, length ); // read in multiple bytes DS_CS_2 = 1; // Deselect Device __delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select //@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles) }
uint8_t osd_spi_read(int8_t addr) { uint8_t SPIData; OSD_CS = 0; // Set active-low CS low to start the SPI cycle spi_write_raw_byte(addr); // Send the Address __delay32(20000UL * OSD_SF); SPIData = spi_xfer_raw_byte(0); OSD_CS = 1; // Set active-low CS high to end the SPI cycle return SPIData; }
void ByteWriteSPI2(unsigned char Add, unsigned char Data ) { DS_CS_2 = 0; // Select Device WriteSPI2 ( Add ); // write address byte to EEPROM WriteSPI2 ( Data ); // Write Byte to device DS_CS_2 = 1; // Deselect device and initiate Write SPI2STATbits.SPITBF = 0; // Transmit started, SPIxTXB is empty __delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select //@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles) }
int main (void) { while(OSCCONbits.LOCK!=1); /* Wait for PLL to lock */ __delay32(EEPROM_DELAY*10); /* Wait for EEPROMs to settle */ __delay32(30000000); state = STATE_INIT; while(1) { CLRWDT() stateMachine(); /* call state machine */ } }
/* Figure out if there was a pulse without a trigger The trigger pulse is a sample pulse so it comes in the middle of current pulse. We should wait for 10us After this is entered. If there has not been a trigger pulse durring that time then it was a false tirgger */ void __attribute__((interrupt, no_auto_psv)) _INT3Interrupt(void) { // There was trigger on INT3 _INT3IF = 0; __delay32(100); // wait for 10us if (!global_data_A36582.sample_complete) { // There was a current pulse without a sample trigger within the next 10us global_data_A36582.pulse_with_no_trigger_counter++; global_data_A36582.false_trigger_counter++; ResetPulseLatches(); } }
void lights(void){ LED1 = 0; LED2 = 1; LED3 = 1; LED4 = 1; __delay32(8000000); LED1 = 1; LED2 = 0; __delay32(8000000); LED2 = 1; LED3 = 0; __delay32(8000000); LED3 = 1; LED4 = 0; __delay32(8000000); LED4 = 1; __delay32(2500000); LED1 = !(resetStat & RST_POR); LED2 = !(resetStat & RST_BOR); LED3 = !(resetStat & RST_EXTR); LED4 = !(resetStat & RST_CM); __delay32(10000000); LED1 = 1; LED2 = 1; LED3 = 1; LED4 = 1; }
int main(void) { int duty = 500; initialize(); write_duty(duty); //apply duty cycle change motoron = 1;//turn motor on kick();//kick start commutation while (1){ commutate(2); __delay32(40000000); commutate(4); __delay32(40000000); commutate(3); __delay32(40000000); if(USER){ motoron=0; } LED1 = !S3; LED2 = !S2; LED3 = !S1; //test_MayDay(); } return 0; }
/* The initialize function configures the PLL to set the internal clock * frequency. It also configures the digital IO and calls the initialization * functions for each of the modules. A light sequence signals the end of * initialization. */ void initialize(void){ /* Configure Phase Lock Loop for the system clock reference at 40MHz */ // Fosc (Clock frequency) is set at 80MHz // Fin is 7.37 MHz from internal FRC oscillator // FPLLI = Fin/N1 = 3.685 MHz CLKDIVbits.PLLPRE = 0; // N1 = 2 // FVCO = FPLLI*M1 = 162.14MHz PLLFBDbits.PLLDIV = 42; // M = 44 // FPLLO = FVCO/N2 = 81.07 MHz // FOSC ~= 80MHz, FCY ~= 40MHz CLKDIVbits.PLLPOST = 0; // N2 = 2 /* Initiate Clock Switch */ //The __builtin macro handles unlocking the OSCCON register __builtin_write_OSCCONH(1); //New oscillator is FRC with PLL __builtin_write_OSCCONL(OSCCON | 0x01); //Enable clock switch while (OSCCONbits.COSC!= 1); //Wait for FRC with PLL to be clock source while (OSCCONbits.LOCK!= 1); //Wait for PLL to lock /* Configure IO*/ TRISDbits.TRISD10 = 1; //USER input //LED outputs ANSELBbits.ANSB13 = 0; //Disable Analog on B13 TRISBbits.TRISB13 = 0; //LED1 ANSELBbits.ANSB12 = 0; //Disable Analog on B12 TRISBbits.TRISB12 = 0; //LED2 TRISDbits.TRISD11 = 0; //LED3 TRISDbits.TRISD0 = 0; //LED4 //Magnet Control TRISBbits.TRISB14 = 0; //Top Magnet //Store bits indicating reason for reset resetStat = RCON; //Clear reset buffer so next reset reading is correct RCON = 0; /* Initialize peripherals*/ initialize_PWM(); initialize_CN(); initialize_ADC(); initialize_QEI(); initialize_UART(); initialize_UART2(); //initialize_I2C_Master(); lights(); __delay32(10000000); //initialize_MPU(); initialize_encoder_values(1600,1700,1800); }
int main(void) { if (!IS_MCU_PROGRAMMED()) /* Stay in programming */ __delay32((unsigned long)((1000)*(FCY_UP)/1000ULL)); mcu_id = MCU_ID; if (isPOWER_ON(rst_state = get_reset_state())) power_on_init(); /* For POR, BOR and MCLR */ rst_events |= rst_state; /* As RCON register */ ++rst_num; /* Calculate session reset number */ check_traps(); /* Do some possible traps */ /* SIM doesn't clear SRbits.IPL, ICD2 clears it */ SET_CPU_IPL(MAIN_IPL); /* Set default by hands */ clr_reset_state(); // Clear uParam and RCON for(;;); return(0); /* Never return */ }
//ISR for CMP3 void __attribute__((__interrupt__, __auto_psv__)) _CMP3Interrupt(void){ //Normal Mode if (sampling_mode==0){ //Normal Mode int temp=0; //putHex(65); ADCPC0bits.SWTRG0 = 1; //start conversion of AN0 and AN1 store 200 samples each while(temp<400){ while(ADCPC0bits.PEND0){} //conv pending becomes 0 when conv complete AN[temp]=ADCBUF0>>2; temp++; AN[temp]=ADCBUF1>>2; temp++; __delay32(delay_cycles); ADCPC0bits.SWTRG0 = 1; //start conversion of AN0 and AN1 // sets PEND0 to 1 } busy=0; IEC1bits.AC3IE =0; //Disable interrupt //IFS1bits.AC3IF =0; //clear interrupt flag=0 }
void BurstWriteSPI2(unsigned char Address, unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4, unsigned char data5, unsigned char data6, unsigned char data7) { DS_CS_2 = 0; // Select Device WriteSPI2 ( Address ); // write address byte to EEPROM WriteSPI2 ( data0 ); // Write Byte to device WriteSPI2 ( data1 ); // Write Byte to device WriteSPI2 ( data2 ); // Write Byte to device WriteSPI2 ( data3 ); // Write Byte to device WriteSPI2 ( data4 ); // Write Byte to device WriteSPI2 ( data5 ); // Write Byte to device WriteSPI2 ( data6 ); // Write Byte to device WriteSPI2 ( data7 ); // Write Byte to device DS_CS_2 = 1; // Deselect device and initiate Write SPI2STATbits.SPITBF = 0; __delay32(_3MICROSEC); //we have to wait at least 1 SPI-Clock-Cycle before the next chip-select //@400kHz -> 2.5us, I wait 3us -> 120 wait-cycles) }
int main() { int i = 0; _SWDTEN = 0; // Disable watchdog timer _GIE = 0; // Disable interrupts ANSELB = ANSELC = ANSELD = ANSELE = ANSELG = 0; __delay32(8000000/2); // Wait about 500ms before change to PLL Sys_Init(); LED1_W = 1; LED2_W = 1; LED3_W = 1; LED4_W = 1; // turn off LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 1000); LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 1000); // LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 2000); // LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 2000); // LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 3000); // LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 3000); LED1_W = 1; LED2_W = 1; LED3_W = 1; LED4_W = 1; // turn off while(1) { WR_Handle_Comms(); if(led) { i++; //if (rx_data == 200) //{ LED_Set_And_Wait(LEDOn, LEDOn, LEDOn, LEDOn, 100); LED_Set_And_Wait(LEDOff, LEDOff, LEDOff, LEDOff, 100); //} //led = 0; } } return 0; }
void __attribute__((__interrupt__(__preprologue__("BCLR ADCON1, #1")), no_auto_psv)) _INT1Interrupt(void) { /* A sample trigger has been received */ // DPARKER these functions may mess with the timing global_data_A36582.sample_energy_mode = ETMCanSlaveGetPulseLevel(); global_data_A36582.sample_index = ETMCanSlaveGetPulseCount(); global_data_A36582.sample_complete = 1; // Check that there was enough time between pulses // if ((TMR2 <= MINIMUM_PULSE_PERIOD_T2) && (_T2IF == 0)) { global_data_A36582.minimum_pulse_period_fault_count++; } _T2IF = 0; TMR2 = 0; // Wait for the pulse energy to dissipate __delay32(150); // Noise from the PFN will often trigger the pic interrupt input // This checks that the interrupt trigger was a real trigger pulse if (_RA12 == 0) { global_data_A36582.pulse_with_no_trigger_counter++; global_data_A36582.false_trigger_counter++; } // Read the data from port PIN_OUT_TP_F = 1; PIN_ADC_CHIP_SELECT = OLL_ADC_SELECT_CHIP; global_data_A36582.imag_external_adc.filtered_adc_reading = SendAndReceiveSPI(0, ETM_SPI_PORT_2); PIN_ADC_CHIP_SELECT = !OLL_ADC_SELECT_CHIP; PIN_OUT_TP_F = 0; _INT1IF = 0; }
void CalibrateGyro(void) { int i; int ii; GyroOffset[0] = 0; GyroOffset[1] = 0; GyroOffset[2] = 0; AngleOffset[0] = 0; AngleOffset[1] = 0; for (i=0;i<1000;i++) { /** Read sensors ******************************************************/ readSensorData(); __delay32(5000); // Without this delay, the I2C command acts funny... accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG); accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG); for (ii=0;ii<3;ii++) { GyroOffset[ii] = GyroOffset[ii] + gyro[ii]; } AngleOffset[0] += accXangle; AngleOffset[1] += accYangle; } for (ii=0;ii<3;ii++) { GyroOffset[ii] = GyroOffset[ii]/1000; } AngleOffset[0] = AngleOffset[0]/1000.0; AngleOffset[1] = AngleOffset[1]/1000.0; gyroXangle = AngleOffset[0]; gyroYangle = AngleOffset[1]; compAngleX = AngleOffset[0]; compAngleY = AngleOffset[1]; }
void AllOfTheLights(void){ LED1 = 0; __delay32(8000000); LED1 = 1; LED2 = 0; __delay32(8000000); LED2 = 1; LED3 = 0; __delay32(8000000); LED3 = 1; LED4 = 0; __delay32(8000000); LED4 = 1; __delay32(2500000); LED1 = 0; LED2 = 0; LED3 = 0; LED4 = 0; __delay32(10000000); LED1 = 1; LED2 = 1; LED3 = 1; LED4 = 1; }
void ResetPulseLatches(void) { PIN_PULSE_LATCH_RESET = OLL_RESET_LATCHES; __delay32(20); PIN_PULSE_LATCH_RESET = !OLL_RESET_LATCHES; }
/* CS SENZA MOS DOPO ISO -> CS_H = 1 e CS_L = 0 CS CON MOS DOPO ISO -> CS_H = 0 e CS_L = 1 */ void __attribute__((__interrupt__,__auto_psv__)) _DMA4Interrupt(void) // DMA4 SPI { _DMA4IF = 0; unsigned int static tmp __attribute__((space(dma),aligned(2))); int spi_temp; unsigned int cl=0; switch (StatoSPI) { case 0: /* Idle */ break; case 1: /* Start a new SPI Sequence */ SPI2STATbits.SPIROV = 0; spi_temp = SPI2BUF; /* Toggle pin */ PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */ __delay32(4074); /* Delay */ PORTGbits.RG9 = CS_L; /* switch RG9 to 0 */ __delay32(4000); /* Delay */ /* Write only Sequence */ DMA5CNT = 3; /* DMA (SPI) Tx */ //Num byte + 1 DMA5STA = (unsigned int) &SPI1_Buff16[0]; DMA4CNT = 3; /* DMA (SPI) Rx */ DMA4STA = (unsigned int) &tmp; DMA5CON &= 0xFFEF; /* Use DMA Tx Post-Incrementation */ DMA4CON |= 0x10; /* Do not use DMA Rx Post-Incrementation */ DMA4CON &= 0xF7FF; /* Disable Null data Write */ DMA5CONbits.CHEN = 1; /* Enable DMA Tx */ DMA4CONbits.CHEN = 1; /* Enable DMA Rx */ DMA5REQbits.FORCE = 1; /* Force Start DMA Tx */ for (cl=0; cl<4000; cl++); PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */ SPI_WRITING = 0; break; case 2: //READ TX /* Toggle pin */ PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */ __delay32(5074); /* Delay */ PORTGbits.RG9 = CS_L; /* switch RG9 to 0 */ __delay32(5000); SPI2STATbits.SPIROV = 0; spi_temp = SPI2BUF; /* Write only Sequence */ DMA5CNT = 2; /* DMA (SPI) Tx */ DMA5STA = (unsigned int) &data_req[0]; DMA4CNT = 2; /* DMA (SPI) Rx */ DMA4STA = (unsigned int) &tmp; DMA5CON &= 0xFFEF; /* Use DMA Tx Post-Incrementation */ DMA4CON |= 0x10; /* Do not use DMA Rx Post-Incrementation */ DMA4CON &= 0xF7FF; /* Disable Null data Write */ DMA5CONbits.CHEN = 1; /* Enable DMA Tx */ DMA4CONbits.CHEN = 1; /* Enable DMA Rx */ DMA5REQbits.FORCE = 1; /* Force Start DMA Tx */ for (cl=0; cl<5000; cl++); SPI2STATbits.SPIROV = 0; spi_temp = SPI2BUF; DMA4STA = (unsigned int) &data_rx[0]; DMA4CNT = read_size; /* DMA (SPI) Rx */ DMA4CON &= 0xFFEF; /* Use DMA Rx Post-Incrementation */ DMA4CON |= 0x800; /* Null data Write in addition do Peripheral to RAM write */ DMA4CONbits.CHEN = 1; /* Enable DMA Rx */ SPI2BUF = 0; /* Start DMA sequence */ for (cl=0; cl<18000; cl++); PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */ break; case 3: /* Start a new SPI Sequence */ for(cicli_dma=0; cicli_dma <= cicli_init+1; cicli_dma+=4){ PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */ /* Delay */ __delay32(4074); /* Delay */ /* Toggle pin */ PORTGbits.RG9 = CS_L; /* switch RG9 to 0 */ __delay32(4000); SPI1_Buff16[0] = VettoreInit[cicli_dma]; SPI1_Buff16[1] = VettoreInit[cicli_dma+1]; SPI1_Buff16[2] = VettoreInit[cicli_dma+2]; SPI1_Buff16[3] = VettoreInit[cicli_dma+3]; SPI2STATbits.SPIROV = 0; /* Write only Sequence */ DMA5CNT = 3; /* DMA (SPI) Tx */ DMA5STA = (unsigned int) &SPI1_Buff16[0]; DMA4CNT = 3; /* DMA (SPI) Rx */ DMA4STA = (unsigned int) &tmp; DMA5CON &= 0xFFEF; /* Use DMA Tx Post-Incrementation */ DMA4CON |= 0x10; /* Do not use DMA Rx Post-Incrementation */ DMA4CON &= 0xF7FF; /* Disable Null data Write */ DMA5CONbits.CHEN = 1; /* Enable DMA Tx */ DMA4CONbits.CHEN = 1; /* Enable DMA Rx */ DMA5REQbits.FORCE = 1; /* Force Start DMA Tx */ for (cl=0; cl<12000; cl++); PORTGbits.RG9 = CS_H; /* switch RG9 to 1 */ for (cl=0; cl<20000; cl++); } break; default: /* Sequence finished */ StatoSPI = 0; /* Should never happend */ break; } /* End of switch case sequence*/ IFS2bits.DMA4IF = 0; // Clear the DMA0 interrupt flag bit } /* End of interrupt */
void delay_ms(unsigned int N) { __delay32(FCY/1000*N); }
void RCInitCypressCYRF6936(void) { unsigned char Address; //Low Order Address Byte unsigned char Data=0; //Data Byte unsigned char Length; unsigned char PageString[128]; #if 0 RC_reset = 1; // Reset the Cypress __delay32(MILLISEC); // 1 milisec delay RC_reset = 0; // Reset the Cypress __delay32(MILLISEC); // 1 milisec delay #endif Address = 0x81; // TX_LENGTH_ADR Data = 0x10; ByteWriteSPI2(Address, Data); Address = 0x82; // TX_CTRL_ADR -> TX disabled Data = 0x00; ByteWriteSPI2(Address, Data); Address = 0x83; // TX_CFG_ADR Data = 0x2F; ByteWriteSPI2(Address, Data); Address = 0x86; // RX_CFG_ADR Data = 0x4A; ByteWriteSPI2(Address, Data); Address = 0x87; // RX_IRQ_STATUS_ADR Data = 0x00; ByteWriteSPI2(Address, Data); Address = 0x8B; // PWR_CTRL_ADR Data = 0x00; ByteWriteSPI2(Address, Data); Address = 0x8C; // XTAL_CTRL_ADR Data = 0x04; ByteWriteSPI2(Address, Data); Address = 0x8D; // IO_CFG_ADR Data = 0x41; ByteWriteSPI2(Address, Data); Address = 0x8E; // GPIO_CTRL_ADR Data = 0x00; ByteWriteSPI2(Address, Data); Address = 0x90; // FRAMING_CFG_ADR Data = 0xEE; ByteWriteSPI2(Address, Data); Address = 0x9B; // TX_OFFSET_LSB_ADR Data = 0x55; ByteWriteSPI2(Address, Data); Address = 0x9C; // TX_OFFSET_MSB_ADR Data = 0x05; ByteWriteSPI2(Address, Data); Address = 0x9D; // MODE_OVERRIDE_ADR Data = 0x18; ByteWriteSPI2(Address, Data); Address = 0xB2; // AUTO_CAL_TIME Data = 0x3C; ByteWriteSPI2(Address, Data); Address = 0xB5; // AUTO_CAL_OFFSET Data = 0x14; ByteWriteSPI2(Address, Data); Address = 0x9E; // RX_OVERRIDE_ADR Data = 0x90; ByteWriteSPI2(Address, Data); Address = 0x9F; // TX_OVERRIDE_ADR Data = 0x00; ByteWriteSPI2(Address, Data); Address = 0x8F; // XACT_CFG_ADR Data = 0x2C; ByteWriteSPI2(Address, Data); Address = 0xA8; // CLK_EN_ADR Data = 0x02; ByteWriteSPI2(Address, Data); Address = 0xA7; // CLK_OVERRIDE_ADR Data = 0x02; ByteWriteSPI2(Address, Data); Address = 0xA2; // SOP_CODE_ADR BurstWriteSPI2(Address, 0xDF, 0xB1, 0xC0, 0x49, 0x62, 0xDF, 0xC1, 0x49); Address = 0x8F; // XACT_CFG_ADR -> clr FRC END Data = 0x0C; ByteWriteSPI2(Address, Data); Address = 0x41; // TX_LENGTH_ADR Length = 16; ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read Address = 0x5D; Length = 3; ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read Address = 0x66; Length = 3; ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read Address = 0x72; Length = 1; ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read Address = 0x75; Length = 1; ByteReadSPI2(Address, PageString, Length ); //Low Density Byte Read ERROR_Level1_COUNTER=0; ERROR_Level2_COUNTER=0; }
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void) { unsigned int *chptr; // For sending a float value unsigned int command; // For sending commands to Axis Converter double gyroXrate, gyroYrate; char nRFstatus; unsigned char rfCommands[32]; // Commands received from remote unsigned char nRFregisters[10]; int iii; long long xSpeedFilterSum = 0; long long ySpeedFilterSum = 0; // Toggle LED _LATA4 = 1; /** Read sensors ******************************************************/ readSensorData(); __delay32(1000); // Without this delay, the I2C command acts funny... accXangle = ((atan2(accel[0], accel[2]))*RAD_TO_DEG)-AngleOffset[0]; accYangle = ((atan2(accel[1], accel[2]))*RAD_TO_DEG)-AngleOffset[1]; gyroXrate = ((double)gyro[0]-(double)GyroOffset[0])/131; gyroYrate = -(((double)gyro[1]-(double)GyroOffset[1])/131); /** Feedback Loop *****************************************************/ compAngleX = (compFilterGyro*(compAngleX+(gyroYrate*(double)SAMPLE_TIME)))+(compFilterAccel*accXangle); // Calculate the angle using a Complimentary filter compAngleY = (compFilterGyro*(compAngleY+(gyroXrate*(double)SAMPLE_TIME)))+(compFilterAccel*accYangle); if (enableSteppers == 1) { iCompAngleY += compAngleY; iCompAngleX += compAngleX; } else { iCompAngleY = 0.0; iCompAngleX = 0.0; } ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN+gyroXrate*(double)iGAIN); xSpeed = -((compAngleX-sGAIN)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN+gyroYrate*(double)iGAIN); xSpeedFilter[SpeedFilterCounter] = xSpeed; ySpeedFilter[SpeedFilterCounter] = ySpeed; for(iii = 0; iii<=SpeedFilterLength;iii++) { xSpeedFilterSum += xSpeedFilter[iii]; ySpeedFilterSum += ySpeedFilter[iii]; } xSpeed = (long long)xSpeedFilterSum/(SpeedFilterLength+1); ySpeed = (long long)ySpeedFilterSum/(SpeedFilterLength+1); // ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN); // xSpeed = -((compAngleX)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN); // if (xSpeed<0) // xSpeed = -(xSpeed*xSpeed); // else // xSpeed = xSpeed*xSpeed; // if (ySpeed<0) // ySpeed = -(ySpeed*ySpeed); // else // ySpeed = ySpeed*ySpeed; compAngleXlast = compAngleX; compAngleYlast = compAngleY; // xSpeed = gyroXrate *1000; // ySpeed = gyroYrate *1000; /** Build command string **********************************************/ command = 0b1000000000000000; if (enableSteppers == 1) command = command | 0b0100000000000000; /* Send data to Axis Controller ***************************************/ chptr = (unsigned char *) ∠ // For sending a float value CS_Axis = 0; /* Command: MSB * 15: 0 = NO Speed Values * 14: 1 = Enable Stepper Driver * 13: */ WriteSPI2(command); // Command: MSB WriteSPI2(xSpeed); // Send x-velocity vector WriteSPI2(ySpeed); // Send y-velocity vector WriteSPI2(*chptr++); // Sending first part of float value WriteSPI2(*chptr); // Sending second part of float value CS_Axis = 1; /** Prepare telemetry *************************************************/ setRawData2string(); /**** Telemetry string format for command=0x42 ************************/ /** command, axh, axl, ayh, ayl, azh, azl, th, tl, 9byte **/ /** gxh, gxl, gyh, gyl, gzh, gzl, cxh, cxl, cyh, cyl, czh, czl,12byte**/ /** motorStatus 1byte**/ /** --Total 22byte **/ serString[16] = xSpeed & 0xFF; // Temporary sending additional telemetry serString[15] = xSpeed >> 8; serString[18] = ySpeed & 0xFF; serString[17] = ySpeed >> 8; // serString[18] = iax & 0xFF; // serString[17] = iax >> 8; // serString[20] = igy & 0xFF; // serString[19] = igy >> 8; serString[21] = enableSteppers; SpeedFilterCounter++; if (SpeedFilterCounter> SpeedFilterLength) SpeedFilterCounter = 0; /** Receive RF commands if any ********************************************/ // LDByteWriteSPI(0x20, 0b00001010); nRFstatus = LDBytePollnRF(); for(iii=0; iii<10;iii++) // For debug { nRFregisters[iii] = readnRFbyte(iii); } if (nRFstatus & 0b01000000) // Data Ready RX FIFO { // _LATB4 = 1; //// Delay10TCYx(100); LDByteReadSPI(0x61, rfCommands, 21); // Read RX FIFO LDByteWriteSPI(0x27 , 0b01000000); // Clear RX FIFO interrupt bit //ByteWriteSPI(0xE2 , 0xFF); //Flush RX FIFO if(rfCommands[0] == 0x82) { if(enableSteppers) { enableSteppers = 0; buttonState = 0; //_LATB4 = 0; } else { enableSteppers = 1; buttonState = 2; //_LATB4 = 1; } } else if (rfCommands[0] == 0x83) { _LATB4 = 1; // Set the received values to variables for(iii=1;iii<19;iii += 3){ if(rfCommands[iii]<9) setValueCode[rfCommands[iii]] = (rfCommands[iii+1]<<8) + rfCommands[iii+2]; } pGAIN = setValueCode[1]; dGAIN = setValueCode[2]; compFilterGyro = (float)setValueCode[4]/(float)1000; compFilterAccel = 1.0 - compFilterGyro; iGAIN = (float)setValueCode[3]/100.0; //angle = (setValueCode[6]-180)* PI /180.0; //SpeedFilterLength = setValueCode[5]; LDByteWriteI2C(0x00D0, 0x1A, setValueCode[5]); // 0.0 ms and disable FSYNC sGAIN = (float)(setValueCode[6]-1000)/100.0; } // //LATDbits.LATD3 = 0; // } // LDByteWriteSPI(0x20, 0b00001010); // // LDByteWriteSPI(0x26, 0b00000010); // Set RC_CH to reset retransmit counter LDCommandWriteSPI(0xE2); // Flush RX buffer (not during ACK) /** Send telemetry ********************************************************/ // _LATB15 = 0; // Set CE low to stop receiving data // LDByteWriteSPI(0x20 , 0b00001010); // Set to send mode // __delay32(100); LDByteWriteSPI(0x27, 0b00010000); // clear MAX_RT (max retries) LDCommandWriteSPI(0xE1); // Flush TX buffer (tx buffer contains last failed transmission) LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received) sendnRFstring( serString, 22); /** Set nRF to recive mode ************************************************/ // __delay32(1000); // LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received) // LDByteWriteSPI(0x20 , 0b00001011); // Set to receive mode // _LATB15 = 1; // Set CE high to start receiving data /* Done with interrupt ****************************************************/ _LATA4 = 0; _T1IF = 0; // Clear Timer 1 interrupt flag }
int16_t main(void) { unsigned int buttonCounter = 20000; int i; /* Configure the oscillator for the device */ ConfigureOscillator(); /* Initialize IO ports and peripherals */ InitApp(); //initSerial(); initSPI1(); initSPI2(); InitI2C(); __delay32(1600000); // allow for POR for all devices initnRF(); ControlByte = 0x00D0; // mpu6050 address InitMPU6050(ControlByte); // InitHMC5883L(); __delay_ms(500); /** Init control loop *****************************************************/ readSensorData(); accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG); accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG); gyroXangle = accXangle; gyroYangle = accYangle; compAngleX = accXangle; compAngleY = accYangle; AngleOffset[0] = accXangle; AngleOffset[1] = accYangle; CalibrateGyro(); // Finding the gyro zero-offset SetupInterrupts(); ////// int serStringN = 14; ////// char nRFstatus = 0; //////// int i; ////// delaytime = 1; ////// bool mode = 0; while(1) { /** Read Button RB7 for enable steppers *******************************/ if (buttonState == 0 && PORTBbits.RB7 && !buttonCounter) { buttonState = 1; buttonCounter = 20000; enableSteppers = 0; } if (buttonState == 1 && !PORTBbits.RB7 && !buttonCounter) { enableSteppers = 2; __delay32(40000000); buttonState = 2; buttonCounter = 20000; enableSteppers = 1; } if (buttonState == 2 && PORTBbits.RB7 && !buttonCounter) { buttonState = 3; buttonCounter = 20000; enableSteppers = 0; } if (buttonState == 3 && !PORTBbits.RB7 && !buttonCounter) { buttonState = 0; buttonCounter = 20000; enableSteppers = 0; } if (buttonCounter) { buttonCounter--; } for (i=0;i<100;i++) { i=i; } // __delay32(100); } }
int ADCCalibrate(void) // Calculate mean value of ADC_CAL_N_SAMPLES samples for iA, iB and Vdc and store in // MeasCurrParm.Offseta, Offsetb // DMA AND ADC isr are not enabled here! // An error in estimation of current offset results in a cogging torque when // torque set point is 0; the value can be adjusted looking at the 3 phase ia, ib, and ic // and centering all values around 0 { int i,TOT; long ADCVdcZero = 0; long ADCOffs1 = 0, ADCOffs2 = 0; // Put an 'accettable' value in case of error in the calibration process MeasCurrParm.Offseta = 0x0; MeasCurrParm.Offsetbc = 0x0; // Turn on ADC module AD1CON1bits.ADON = 1; for(i=0;i<ADC_CAL_N_SAMPLES;i++) { // samples AN0 AD1CHS0bits.CH0SA = 0; AD1CON1bits.DONE = 0; // amplifiers holding AD1CON1bits.SAMP = 1; // TODO: TRIM ME (eventually:) __delay32(100); // convert! AD1CON1bits.SAMP = 0; // init time out timer TOT=0; // poll for end of conversion while( 0 == AD1CON1bits.DONE) { // Avoids polling lock if (ADC_CAL_TIMEOUT == ++TOT) { // offset correction error signaling return -1; } } // accumulate values ADCOffs1 += (int)ADC1BUF0; // Samples AN1 AD1CHS0bits.CH0SA = 1; AD1CON1bits.DONE = 0; // amplifiers holding AD1CON1bits.SAMP = 1; // TODO TRIM ME __delay32(100); // convert! AD1CON1bits.SAMP = 0; // init time out timer TOT=0; // poll for end of conversion while( 0 == AD1CON1bits.DONE ) { // Avoids polling lock if (ADC_CAL_TIMEOUT == ++TOT) { // offset correction error signaling return -1; } } // accumuulate values ADCOffs2 += (int)ADC1BUF0; // Samples AN2 (Vdc) AD1CHS0bits.CH0SA = 2; AD1CON1bits.DONE = 0; // amplifiers holding AD1CON1bits.SAMP = 1; // TODO TRIM ME */ __delay32(100); // convert! AD1CON1bits.SAMP = 0; // don't mind about int generated // init time out timer TOT=0; // poll for end of conversion while( 0 == AD1CON1bits.DONE ) { // Avoids polling lock if (ADC_CAL_TIMEOUT == ++TOT) { // offset correction error signaling return -1; } } // accumuulate values ADCVdcZero += (int)ADC1BUF0; /* if the measured VDCLINK is lower than this threshold * then we cannot be sure the Allegro are properly powered. * For this reason the ADC calibration is not reliable. */ if(ADCVDCLinkTo100mV(ADC1BUF0) < ADC_VDCLINK_ALLEGRO_MIN_THRESHOLD) return -2; } // divide for number of samples in order to get mean ADCOffs1 /= ADC_CAL_N_SAMPLES; ADCOffs2 /= ADC_CAL_N_SAMPLES; ADCVdcZero /= ADC_CAL_N_SAMPLES; // associate the calculated offset MeasCurrParm.Offseta = ADCOffs1; MeasCurrParm.Offsetbc = ADCOffs2; // TODO: mettere a posto la correzione dell'offset // e quindi togliere questi bypass! //MeasCurrParm.Offseta = 0x200; //MeasCurrParm.Offsetbc = 0x200; // TODO: un commentino, no? TargetDCbus = ADCVdcZero; // Turn off ADC module AD1CON1bits.ADON = 0; return 0; }