dig:mic-servo
This is an old revision of the document!
Here are the main.c and mic-servo.py code for a working project for lab_3.
main.c
////////////////////////////////////////////////////////////////////////////// // RGG LED main.c ////////////////////////////////////////////////////////////////////////////// #include "compiler.h" #include "preprocessor.h" #include "pm.h" #include "gpio.h" #include "pwm.h" #include "adc.h" #include "dsp.h" #include "tc.h" //#include "print_funcs.h" #include "intc.h" #include "power_clocks_lib.h" #include "conf_usb.h" #include "usb_task.h" #if USB_DEVICE_FEATURE == ENABLED #include "usb_drv.h" #include "usb_descriptors.h" #include "usb_standard_request.h" #include "device_task.h" #endif // RGB LED and servo PWM defines #define cR 0 #define cG 1 #define cB 2 #define cServo 3 // indexes into channel arrays #define R_PWM_PIN AVR32_PWM_1_0_PIN // PA08 #define R_PWM_FUNCTION AVR32_PWM_1_0_FUNCTION #define R_PWM_CHANNEL_ID 1 // note we skip PWM[2]=pin 25 because it is used by default fuse settings and DFU bootloader code as DFU launcher input pin #define G_PWM_PIN AVR32_PWM_3_0_PIN // PA14 #define G_PWM_FUNCTION AVR32_PWM_3_0_FUNCTION #define G_PWM_CHANNEL_ID 3 #define B_PWM_PIN AVR32_PWM_4_0_PIN // PA15 #define B_PWM_FUNCTION AVR32_PWM_4_0_FUNCTION #define B_PWM_CHANNEL_ID 4 static pwm_opt_t pwm_opt; static avr32_pwm_channel_t pwm_channel[4]; static unsigned int channel_id[4]; #define TTT 4 // defines how to multiply PWM period and duty cycle values for the RGB LED PWM channels. 4 gives 4kHz frequency. // Timer / Counter, used to generate interrupt for ADC sampling # define EXAMPLE_TC (&AVR32_TC) # define EXAMPLE_TC_IRQ_GROUP AVR32_TC_IRQ_GROUP # define EXAMPLE_TC_IRQ AVR32_TC_IRQ0 # define FPBA FOSC0 // chosen PBA clock frequency - set up to be same as cystral freq=12MHz #define FADC 10000 // desired ADC sample rate in Hz #define TC_CHANNEL 0 volatile U32 tc_tick = 0; // counts samples volatile bool takeSampleNow=TRUE; // ISR sets to tell main loop to sample ADC int dspCounter=0; // used to count signal processing steps for audMean, so that meanSq power estimate filtering can occur at intervals of TAU2 // adc #define ADC_CHANNEL 0 #define ADC_PIN AVR32_ADC_AD_0_PIN // PA03, used by LDR on bronze board or hacked copper board #define ADC_FUNCTION AVR32_ADC_AD_0_FUNCTION // Signal processing: long audMean=0; // holds mean mic signal, for high pass filtering long meanSq=0; // holds mean square audio signal long maxSq=0; // holds max, for clipping detection unsigned int servo; // the computed servo signal, ranging from 1000-2000 us bool initialized=0; // flags if filters have been initialized #define NTAU1 6 // 'time constant' in samples is 2^TAU1 of high pass filter for mean audio level. Actual RC time constant is deltaT*(2^TAU1) e.g. 25ms for TAU1=8 and fsample=10kHz. #define TAU2 64 // 'time constant' in samples of lowpass second filter as multiple of time constant of highpass filter // servo #define FSERVOUPDATE 100 // update rate for servo pulse width in main loop in Hz #define SERVO_PIN AVR32_PWM_6_0_PIN // PWM_6_0 is PA19 #define SERVO_PWM_FUNCTION AVR32_PWM_6_0_FUNCTION #define SERVO_PWM_CHANNEL_ID 6 #define SERVO_MID 773 // counts to get 1500us pulse width #define SERVO_MULT 0.5156 // for static calculations; multiply us by this to get cdty count // USB static U32 sof_cnt; // start-of-frame counts, get one every ms static U8 in_data_length; static U8 in_buf[EP_SIZE_TEMP1]; // 64 byte data buffer for in packet (to host) static U8 out_data_length; static U8 out_buf[EP_SIZE_TEMP2]; // data buffer for out packet, from host ////////////////////////////////////////////////////////////////////////////// // clock setup ////////////////////////////////////////////////////////////////////////////// #define FOSC0 12000000 //!< Osc0 frequency: Hz. #define OSC0_STARTUP AVR32_PM_OSCCTRL0_STARTUP_2048_RCOSC //!< Osc0 startup time: RCOsc periods. void init_clock() { // source: gpio_local_bus_example.c // Initialize domain clocks (CPU, HSB, PBA and PBB) to the max frequency available // without flash wait states. // Some of the registers in the GPIO module are mapped onto the CPU local bus. // To ensure maximum transfer speed and cycle determinism, any slaves being // addressed by the CPU on the local bus must be able to receive and transmit // data on the bus at CPU clock speeds. The consequences of this is that the // GPIO module has to run at the CPU clock frequency when local bus transfers // are being performed => we want fPBA = fCPU. // Switch the main clock source to Osc0. pm_switch_to_osc0(&AVR32_PM, FOSC0, OSC0_STARTUP); // Setup PLL0 on Osc0, mul=10 ,no divisor, lockcount=16: 12Mhzx11 = 132MHz output pm_pll_setup(&AVR32_PM, 0, // pll. 10, // mul. 1, // div. 0, // osc. 16); // lockcount. // PLL output VCO frequency is 132MHz. // We divide it by 2 with the pll_div2=1 to get a main clock at 66MHz. pm_pll_set_option(&AVR32_PM, 0, // pll. 1, // pll_freq. 1, // pll_div2. 0); // pll_wbwdisable. // Enable the PLL. pm_pll_enable(&AVR32_PM, 0); // Wait until the PLL output is stable. pm_wait_for_pll0_locked(&AVR32_PM); // Configure each clock domain to use the main clock divided by 2 // => fCPU = fPBA = fPBB = 33MHz. pm_cksel(&AVR32_PM, 1, // pbadiv. 0, // pbasel. 1, // pbbdiv. 0, // pbbsel. 1, // hsbdiv=cpudiv 0); // hsbsel=cpusel // Switch the main clock source to PLL0. pm_switch_to_clock(&AVR32_PM, AVR32_PM_MCCTRL_MCSEL_PLL0); } ////////////////////////////////////////////////////////////////////////////// // PWM setup ////////////////////////////////////////////////////////////////////////////// // PWM setup void init_pwm() { // set PWM GPIOs gpio_enable_module_pin(R_PWM_PIN, R_PWM_FUNCTION); gpio_enable_module_pin(G_PWM_PIN, G_PWM_FUNCTION); gpio_enable_module_pin(B_PWM_PIN, B_PWM_FUNCTION); gpio_enable_module_pin(SERVO_PIN, SERVO_PWM_FUNCTION); // PWM controller configuration. pwm_opt.diva = AVR32_PWM_DIVA_CLK_OFF; // no divider pwm_opt.divb = AVR32_PWM_DIVB_CLK_OFF; pwm_opt.prea = AVR32_PWM_PREA_MCK; // no prescaler for div a or b pwm_opt.preb = AVR32_PWM_PREB_MCK; // init pwm globals pwm_init(&pwm_opt); channel_id[cR] = R_PWM_CHANNEL_ID; channel_id[cG] = G_PWM_CHANNEL_ID; channel_id[cB] = B_PWM_CHANNEL_ID; channel_id[cServo] = SERVO_PWM_CHANNEL_ID; // the core runs at 33MHz (see init_clock()). Therefore we get PWM base clock of 33Mhz. // Then we divide this down by powers of 2 and supply 20 bit count values for the period and duty cycle. // We observe that the LED frequency is 4kHz. // The LED channels are set up to use 33 MHz/2 clock source, and count to 256<<4=4096 for the period. // Therefore we expect that the frequency is 33M/2/4k=4.028kHz. This is exactly what we observe. // To get a reasonable servo pulse frequency of 200Hz, we use for the servo channel a further division by 32 to get // a frequency of 4.028kHz/32=125.9Hz. Therefore we use a prescaler of 64 instead of 2. unsigned int c; for (c = cR; c <= cB; c++) { pwm_channel[c].CMR.calg = PWM_MODE_LEFT_ALIGNED; // Channel mode. pwm_channel[c].CMR.cpol = PWM_POLARITY_HIGH; // Channel polarity. pwm_channel[c].CMR.cpd = PWM_UPDATE_DUTY; // Not used the first time. pwm_channel[c].CMR.cpre = AVR32_PWM_CPRE_MCK_DIV_2; // Channel prescaler. pwm_channel[c].cdty = 0; // Channel duty cycle, should be < CPRD. pwm_channel[c].cprd = (256 << TTT); // Channel period. pwm_channel[c].cupd = 0; // Channel update is not used here. pwm_channel_init(channel_id[c], &pwm_channel[c]); } // servo channel // the duty cycle units here are in units of the prescaled clock period which is 64/33MHz=1.939us. // Therefore to get X us we need to supply X/1.939 counts to cdty, or approximately X/2 counts. // Therefore to get 1500us we need to supply 773 counts or about 750 counts. pwm_channel[cServo].CMR.calg = PWM_MODE_LEFT_ALIGNED; // Channel mode. pwm_channel[cServo].CMR.cpol = PWM_POLARITY_HIGH; // Channel polarity. pwm_channel[cServo].CMR.cpd = PWM_UPDATE_DUTY; // Not used the first time. pwm_channel[cServo].CMR.cpre = AVR32_PWM_CPRE_MCK_DIV_64; // Channel prescaler - should give 125.9Hz pwm_channel[cServo].cdty = 0; // start at zero to leave servo disabled if it was disabled and is digital servo with annoying whine. // SERVO_MID; // Channel duty cycle, should be < CPRD. pwm_channel[cServo].cprd = (256 << TTT); // Channel period. Use same as LED so that we get 4kHz/32=125.9Hz frequency. pwm_channel[cServo].cupd = 0; // Channel update is not used here. pwm_channel_init(channel_id[cServo], &pwm_channel[cServo]); pwm_start_channels((1 << channel_id[cR]) | (1 << channel_id[cG]) | (1<< channel_id[cB])|(1<<channel_id[cServo])); } // sets RGB LED brightnesses, r,g,b range from 0-255 void set_rgb(U8 r, U8 g, U8 b) { U8 c; for (c = cR; c <= cB; c++) { // Channel duty cycle, should be < CPRD. switch (c) { case cR: pwm_channel[c].cdty = ((U32) r) << TTT; break; case cG: pwm_channel[c].cdty = ((U32) g) << TTT; break; case cB: pwm_channel[c].cdty = ((U32) b) << TTT; break; } pwm_channel_init(channel_id[c], &pwm_channel[c]); } } /** * Sets the pulse width for the servo output. 1000 to 2000 is servo range. * Argument, pulse width in us from 1000 to 2000 * */ void setServoPWUs(U16 us) { pwm_channel[cServo].cdty = ((U32) us>>1); // TODO almost correct, double quantity to give almost exactly us, see SERVO_MULT and pwm_init() pwm_channel_init(channel_id[cServo], &pwm_channel[cServo]); } ////////////////////////////////////////////////////////////////////////////// // ADC ////////////////////////////////////////////////////////////////////////////// // initializes ADC for one channel void init_adc() { // GPIO pin/adc-function map. static const gpio_map_t ADC_GPIO_MAP = { { ADC_PIN, ADC_FUNCTION } }; volatile avr32_adc_t *adc = &AVR32_ADC; // ADC IP registers address // Assign and enable GPIO pins to the ADC function. gpio_enable_module(ADC_GPIO_MAP, sizeof(ADC_GPIO_MAP) / sizeof(ADC_GPIO_MAP[0])); // configure ADC // Lower the ADC clock to match the ADC characteristics (because we configured // the CPU clock to 33MHz, and the ADC clock requires less than 5 MHz for 10 bit ADC, // therefore prescale by ; // cf. the ADC Characteristic section in the datasheet). // TODO These ADC config numbers are wrong currently! AVR32_ADC.mr |= 0x3 << AVR32_ADC_MR_PRESCAL_OFFSET; adc_configure(adc); // Assign the on-board sensors to their ADC channel. unsigned short adc_channel = ADC_CHANNEL; // Enable the ADC channels. adc_enable(adc, adc_channel); } U16 get_adc_value() { // launch conversion on all enabled channels volatile avr32_adc_t *adc = &AVR32_ADC; // ADC IP registers address adc_start(adc); // get value for the adc channel U16 adc_value = adc_get_value(adc, ADC_CHANNEL); return adc_value; } ////////////////////////////////////////////////////////////////////////////// // USB ////////////////////////////////////////////////////////////////////////////// //! //! @brief This function initializes the hardware/software resources required for device applicative task. //! void device_task_init(void) { sof_cnt = 0; in_data_length = 0; out_data_length = 0; Usb_enable_sof_interrupt();// enables interrupt every ms for start of frame } //! //! @brief Entry point of the device applicative task management //! //! This function links the device application to the USB bus. //! void device_task(void) { if (takeSampleNow) { // flag set in timer ISR gpio_local_tgl_gpio_pin(AVR32_PIN_PA11); // debug takeSampleNow=FALSE; // signal processing S16 adcval = (S16)get_adc_value(); // 0-1023=3.3V if (initialized) audMean = ((adcval-audMean)>>NTAU1)+audMean; // TODO mix old and new value else audMean = adcval; // init filter with first reading if(dspCounter--==0){ // only update meanSq at this interval, so to produce effective time constant that is TAU2 times tau of audMean filtering dspCounter=TAU2; long diff = adcval - audMean; // signed diff of sample from mean long sq = diff * diff; // square diff if (initialized) meanSq = ((sq-meanSq)>>NTAU1)+meanSq; // low pass square diff else meanSq = sq; if (meanSq > maxSq) maxSq = meanSq; // not used now } initialized = 1; // we update the servo at FSERVOUPDATE frequency. Since the FADC is higher, we can count ADC cycles // and update the servo only every FADC/FSERVOUPDATE samples if (tc_tick % (FADC / FSERVOUPDATE) == 0) { servo = (unsigned int) (meanSq >> 3); // send meanSq back to host if (servo > 2000) servo = 2000; else if (servo < 1000) servo = 1000; setServoPWUs(servo); // sets 1000 to 2000 us // only communicate if we are enumerated if (!Is_device_enumerated()) return; // HERE STARTS THE USB DEVICE APPLICATIVE CODE // Load the IN endpoint (to the host PC) with the desired measurement to show on host if (Is_usb_in_ready(EP_TEMP_IN)) { // store bytes of 32 bit value to buffer...: in_buf[0] = 0xFF & (meanSq>>24); // big endian order, MSB goes first in array in_buf[1] = 0xff & (meanSq>>16); in_buf[2] = 0xFF & (meanSq >> 8); in_buf[3] = 0xFF & (meanSq >> 0); in_data_length = 4; // do magic to send the packet Usb_reset_endpoint_fifo_access(EP_TEMP_IN); usb_write_ep_txpacket(EP_TEMP_IN, in_buf, in_data_length, NULL); in_data_length = 0; Usb_ack_in_ready_send(EP_TEMP_IN); } // If we receive something in the OUT endpoint (from the host), use it to set the RGB color if (Is_usb_out_received(EP_TEMP_OUT)) { gpio_local_tgl_gpio_pin(AVR32_PIN_PA12); // debug data from host Usb_reset_endpoint_fifo_access(EP_TEMP_OUT); out_data_length = Usb_byte_count(EP_TEMP_OUT); usb_read_ep_rxpacket(EP_TEMP_OUT, out_buf, out_data_length, NULL); // store the received data in out_buf Usb_ack_out_received_free(EP_TEMP_OUT); // update PWM: set_rgb(out_buf[1], out_buf[2], out_buf[3]); } } } } //! //! @brief usb_sof_action //! //! This function increments the sof_cnt counter each time //! the USB Start-of-Frame interrupt subroutine is executed (1 ms). //! Useful to manage time delays //! void usb_sof_action(void) { // gpio_local_tgl_gpio_pin(AVR32_PIN_PA10); // debug, should toggle every ms sof_cnt++; } /*! \brief TC interrupt - used for AD conversion. */ #if defined (__GNUC__) __attribute__((__interrupt__)) #elif defined (__ICCAVR32__) #pragma handler = EXAMPLE_TC_IRQ_GROUP, 1 __interrupt #endif static void tc_irq(void) { // Increment the counter, which is also used to determine servo updates tc_tick++; // set a flag to tell main loop to take a sample takeSampleNow = TRUE; // Clear the interrupt flag. This is a side effect of reading the TC SR. tc_read_sr(EXAMPLE_TC, TC_CHANNEL); // Toggle a GPIO pin (this pin is used as a regular GPIO pin). gpio_local_tgl_gpio_pin(AVR32_PIN_PA10); // debug, should toggle at desired sample rate } void init_tc(){ // Timer/Counter Options for waveform generation. static const tc_waveform_opt_t WAVEFORM_OPT = { .channel = TC_CHANNEL, // Channel selection. .bswtrg = TC_EVT_EFFECT_NOOP, // Software trigger effect on TIOB. .beevt = TC_EVT_EFFECT_NOOP, // External event effect on TIOB. .bcpc = TC_EVT_EFFECT_NOOP, // RC compare effect on TIOB. .bcpb = TC_EVT_EFFECT_NOOP, // RB compare effect on TIOB. .aswtrg = TC_EVT_EFFECT_NOOP, // Software trigger effect on TIOA. .aeevt = TC_EVT_EFFECT_NOOP, // External event effect on TIOA. .acpc = TC_EVT_EFFECT_NOOP, // RC compare effect on TIOA: toggle. .acpa = TC_EVT_EFFECT_NOOP, // RA compare effect on TIOA: toggle (other possibilities are none, set and clear). .wavsel = TC_WAVEFORM_SEL_UP_MODE_RC_TRIGGER,// Waveform selection: Up mode with automatic trigger(reset) on RC compare. .enetrg = FALSE, // External event trigger enable. .eevt = 0, // External event selection. .eevtedg = TC_SEL_NO_EDGE, // External event edge selection. .cpcdis = FALSE, // Counter disable when RC compare. .cpcstop = FALSE, // Counter clock stopped with RC compare. .burst = FALSE, // Burst signal selection. .clki = FALSE, // Clock inversion. .tcclks = TC_CLOCK_SOURCE_TC2 // Internal source clock 2, connected to fPBA/2=6MHz. }; //! Timer/counter interrupts. static const tc_interrupt_t TC_INTERRUPT = { .etrgs = 0, //! External trigger interrupt. .ldrbs = 0, //! RB load interrupt. .ldras = 0, //! RA load interrupt. .cpcs = 1, //! RC compare interrupt. - generate interrupt with counter reaching Reset Count value (RC) .cpbs = 0, //! RB compare interrupt. .cpas = 0, //! RA compare interrupt. .lovrs = 0, //! Load overrun interrupt. .covfs = 0 //! Counter overflow interrupt. }; volatile avr32_tc_t *tc = EXAMPLE_TC; Disable_global_interrupt(); // Register the RTC interrupt handler to the interrupt controller. INTC_register_interrupt(&tc_irq, EXAMPLE_TC_IRQ, AVR32_INTC_INT0); Enable_global_interrupt(); // Initialize the timer/counter. tc_init_waveform(tc, &WAVEFORM_OPT); // Initialize the timer/counter waveform. // Set the compare triggers for timer/counter (TC). // TC counter is 16-bits, with secondary main clock fPBA = 12 MHz. // Lowest possible freq is 12e6/2^16=183.1Hz. // We want ADC sample rate of FADC Hz. To get this, we load RC (Reset Counter) value so that // TC reaches RC value every 1/FADC ms. Therefore we configure TC so that RC=FPBA/FADC. // E.g., to get FADC=10kHz, we need RC=12e6/10000=1200. tc_write_rc(tc, TC_CHANNEL, (FPBA /2) / FADC); // Set RC value. tc_configure_interrupts(tc, TC_CHANNEL, &TC_INTERRUPT); // Start the timer/counter. tc_start(tc, TC_CHANNEL); } ////////////////////////////////////////////////////////////////////////////// // main ////////////////////////////////////////////////////////////////////////////// int main() { Enable_global_exception(); Disable_global_interrupt(); INTC_init_interrupts(); pcl_switch_to_osc(PCL_OSC0, FOSC0, OSC0_STARTUP); init_clock(); init_pwm(); gpio_local_init(); init_adc(); pcl_configure_usb_clock(); usb_task_init(); init_tc(); #if USB_DEVICE_FEATURE == ENABLED device_task_init(); #endif gpio_local_enable_pin_output_driver(AVR32_PIN_PA10); // we bit bang these for debugging on scope gpio_local_enable_pin_output_driver(AVR32_PIN_PA11); gpio_local_enable_pin_output_driver(AVR32_PIN_PA12); gpio_local_clr_gpio_pin(AVR32_PIN_PA10); gpio_local_clr_gpio_pin(AVR32_PIN_PA11); gpio_local_clr_gpio_pin(AVR32_PIN_PA12); while (TRUE) { usb_task(); #if USB_DEVICE_FEATURE == ENABLED device_task(); #endif } return 0; } ////////////////////////////////////////////////////////////////////////////// // EOF //////////////////////////////////////////////////////////////////////////////
mic-servo.py
- mic-servo.py
#!/usr/bin/env python import sys, os import array import usb import colorsys import time import math busses = usb.busses() VENDOR = 0x03eb PRODUCT = 0x2300 IFACE = 0 EP_IN = 0x81 EP_OUT = 0x02 deltah = 0.0003 T = 0.00 PWMperADC = 100 BAR = 80 tstart = time.time() def get_device(): for bus in busses: devices = bus.devices for dev in devices: if dev.idVendor == VENDOR and dev.idProduct == PRODUCT: return dev return None vmax = 1 vmin = 2**16 vavg=0.0 vms=0.0 m=.02 # mixing factor for filter vavg=0.0 initialized=False vol=0 def adc(v): global vmax, vmin, m, vavg, vms, initialized, vol vol=v if vol > vmax: vmax = vol if vol < vmin: vmin = vol if vmax <= vmin: vmin -= 1 t = time.time() - tstart hz = usbiocnt / t # vol=int(255*(vol - vmin) / (vmax - vmin) ) o = '% 4d Hz ' % hz o+= '% 4d ' % vol o += '#' * int( BAR * (vol - vmin) / (vmax - vmin) ) print o usbiocnt = 0L def usbio(dh): global usbiocnt, vol usbiocnt += 1 dout = array.array('B', [0]*4) dout[0] = 0xFF & 0x00 dout[1] = 0xFF & (vol) # red dout[2] = 0xFF & (vol) # green dout[3] = 0xFF & (vol) # blue dh.bulkWrite(EP_OUT, dout.tostring()) #if usbiocnt % PWMperADC == 0: if 1: din = dh.bulkRead(EP_IN, 4) # read a packet l = len(din) if l != 4: print "unexpected bulk read length: %d" % l else: if usbiocnt % PWMperADC == 0: value=(din[0]<<24)+ (din[1]<<16)+ (din[2] << 8) + din[3] adc(value) def intcol(v): v = int(v*256) if v > 255: v = 255 if v < 0: v = 0 return v def micservo(dh): global vol while 1: usbio(dh) time.sleep(T) def main(): dev = get_device() dh = dev.open() dh.claimInterface(IFACE) micservo(dh) dh.releaseInterface() del dh return 0 if __name__ == '__main__': sys.exit( main() )
dig/mic-servo.1300804892.txt.gz · Last modified: 2024/02/29 07:28 (external edit)