void dxl_serial_init(volatile struct dxl_device *device, int index) { ASSERT(index >= 1 && index <= 3); // Initializing device struct serial *serial = (struct serial*)malloc(sizeof(struct serial)); dxl_device_init(device); device->data = (void *)serial; device->tick = dxl_serial_tick; device->process = process; serial->index = index; serial->txComplete = true; serial->dmaEvent = false; if (index == 1) { serial->port = &Serial1; serial->direction = DIRECTION1; serial->channel = DMA_CH4; } else if (index == 2) { serial->port = &Serial2; serial->direction = DIRECTION2; serial->channel = DMA_CH7; } else { serial->port = &Serial3; serial->direction = DIRECTION3; serial->channel = DMA_CH2; } serial->syncReadCurrent = 0xff; serial->syncReadCount = 0; initSerial(serial); if (!serialInitialized) { serialInitialized = true; // Initializing DMA dma_init(DMA1); usart1_tc_handler = usart1_tc; usart2_tc_handler = usart2_tc; usart3_tc_handler = usart3_tc; // Reset allocation for (unsigned int k=0; k<sizeof(devicePorts); k++) { devicePorts[k] = 0; } // Initialize sync read timer syncReadTimer.pause(); syncReadTimer.setPrescaleFactor(CYCLES_PER_MICROSECOND*10); syncReadTimer.setOverflow(0xffff); syncReadTimer.refresh(); syncReadTimer.resume(); } }
void setup() { // Setup our pins pinMode(BOARD_LED_PIN, OUTPUT); pinMode(VGA_R, OUTPUT); pinMode(VGA_G, OUTPUT); pinMode(VGA_B, OUTPUT); pinMode(VGA_V, OUTPUT); pinMode(VGA_H, OUTPUT); digitalWrite(VGA_R, LOW); digitalWrite(VGA_G, LOW); digitalWrite(VGA_B, LOW); digitalWrite(VGA_H, HIGH); digitalWrite(VGA_V, HIGH); // Fill the logo array with color patterns corresponding to its // truth value. Note that we could get more tricky here, since // there are 3 bits of color. for (int y = 0; y < y_max; y++) { for (int x = 0; x < x_max; x++) { logo[y][x] = logo[y][x] ? ON_COLOR : OFF_COLOR; } } // This gets rid of the majority of the interrupt artifacts; // there's still a glitch for low values of y, but let's not worry // about that. (Probably due to the hackish way vsync is done). SerialUSB.end(); systick_disable(); // Configure timer.pause(); // while we configure timer.setPrescaleFactor(1); // Full speed timer.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); timer.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE); timer.setMode(TIMER_CH3, TIMER_OUTPUT_COMPARE); timer.setMode(TIMER_CH4, TIMER_OUTPUT_COMPARE); timer.setOverflow(2287); // Total line time timer.setCompare(TIMER_CH1, 200); timer.attachInterrupt(TIMER_CH1, isr_porch); timer.setCompare(TIMER_CH2, 300); timer.attachInterrupt(TIMER_CH2, isr_start); timer.setCompare(TIMER_CH3, 2170); timer.attachInterrupt(TIMER_CH3, isr_stop); timer.setCompare(TIMER_CH4, 1); // Could be zero, I guess timer.attachInterrupt(TIMER_CH4, isr_update); timer.setCount(0); // Ready... timer.resume(); // Go! }
//Timer control routines void baud_timer_init() { #ifdef ___MAPLE timer4.pause(); // Pause the timer while configuring it timer4.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); // Set up interrupt on channel 1 timer4.setCount(0); // Reset count to zero timer4.setPrescaleFactor(72); // Timer counts at 72MHz/72 = 1MHz 1 count = 1uS timer4.setOverflow(0xFFFF); // reset occurs at 15.259Hz timer4.refresh(); // Refresh the timer's count, prescale, and overflow timer4.resume(); // Start the timer counting #endif #ifdef ___ARDUINO cli(); // stop interrupts during configuration TCCR1A = 0; // Clear TCCR1A register TCCR1B = 0; // Clear TCCR1B register TCNT1 = 0; // Initialize counter value OCR1A = 0xFFFF; // Set compare match register to maximum value TCCR1B |= (1 << WGM12); // CTC mode // We want 1uS ticks, for 16MHz CPU, we use prescaler of 16 // as 1MHz = 1uS period, but Arduino is lame and only has // 3 bit multiplier, we can have 8 (overflows too quickly) // or 64, which operates at 1/4 the desired resolution TCCR1B |= (1 << CS11); // Configure for 8 prescaler TIMSK1 |= (1 << OCIE1A); // enable compare interrupt sei(); // re-enable interrupts #endif #ifdef ___TEENSY FTM0_MODE |= FTM_MODE_WPDIS; FTM0_CNT = 0; FTM0_CNTIN = 0; FTM0_SC |= FTM_SC_PS(7); FTM0_SC |= FTM_SC_CLKS(1); FTM0_MOD = 0xFFFF; FTM0_MODE |= FTM_MODE_FTMEN; /* PIT_LDVAL1 = 0x500000; PIT_TCTRL1 = TIE; PIT_TCTRL1 |= TEN; PIT_TFLG1 |= 1; */ #endif }
void motor_init(encoder *pEnc) { // Ensuring the shut down is active (inversed logic on this one) digitalWrite(SHUT_DOWN_PIN, LOW); pinMode(SHUT_DOWN_PIN, OUTPUT); digitalWrite(SHUT_DOWN_PIN, LOW); // Preparing the first PWM signal digitalWrite(PWM_1_PIN, LOW); pinMode(PWM_1_PIN, PWM); pwmWrite(PWM_1_PIN, 0x0000); // Preparing the second PWM signal digitalWrite(PWM_2_PIN, LOW); pinMode(PWM_2_PIN, PWM); pwmWrite(PWM_2_PIN, 0x0000); if (HAS_CURRENT_SENSING) { // ADC pin init pinMode(CURRENT_ADC_PIN, INPUT_ANALOG); } // Releasing the shutdown digitalWrite(SHUT_DOWN_PIN, HIGH); // Reading the magic offset in the flash mot.offset = dxl_read_magic_offset(); mot.command = 0; mot.previousCommand = 0; mot.predictiveCommand = 0; mot.predictiveCommandTorque = 0; mot.angle = pEnc->angle + mot.offset; mot.previousAngle = pEnc->angle + mot.offset; mot.angleBuffer = *buffer_creation(dxl_regs.ram.speedCalculationDelay + 1, mot.angle); // Works because a tick is 1 ms. mot.speedBuffer = *buffer_creation(NB_TICKS_BEFORE_UPDATING_ACCELERATION, 0); mot.targetAngle = pEnc->angle; mot.speed = 0; mot.averageSpeed = 0; mot.previousSpeed = 0; mot.signOfSpeed = 0; mot.targetSpeed = 0; mot.acceleration = 0; mot.targetAcceleration = 0; mot.state = COMPLIANT; mot.current = 0; mot.averageCurrent = 0; mot.targetCurrent = 0; mot.posAngleLimit = 0; mot.negAngleLimit = 0; mot.multiTurnOn = false; mot.multiTurnAngle = mot.angle; mot.electricalTorque = 0.0; mot.outputTorque = 0.0; mot.targetTorque = 0.0; mot.temperatureIsCritic = false; // filtre k.p /(1+2*z* p/wn+p2/wn2) float k = dxl_regs.ram.speedCalculationDelay + 1; float tau = (dxl_regs.ram.speedCalculationDelay + 1) * 0.5; float wn = 1 / tau; float csi = 0.7; float te = 1; motor_init_filter_speed(&mot, 0, k, 0, 1, 2 * csi / wn, 1 / (wn * wn), te); init_filter_state(&mot.filt_speed, mot.angle); timer3.setPrescaleFactor( 7200); // 1 for current debug, 7200 => 10 tick per ms timer3.setOverflow(65535); }
void init_ppm_timer() { timer4.pause(); timer4.setPrescaleFactor(TIMER_PRESCALE); timer4.setOverflow(65535); timer4.setCount(0); // use channel 2 to detect when we stop receiving // a ppm signal from the encoder. timer4.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE); timer4.setCompare(TIMER_CH2, 65535); timer4.attachCompare2Interrupt(ppm_timeout_isr); timer4.refresh(); //capture compare regs TIMx_CCRx used to hold val after a transition on corresponding ICx //when cap occurs, flag CCXIF (TIMx_SR register) is set, //and interrupt, or dma req can be sent if they are enabled. //if cap occurs while flag is already high CCxOF (overcapture) flag is set.. //CCIX can be cleared by writing 0, or by reading the capped data from TIMx_CCRx //CCxOF is cleared by writing 0 to it. //Clear the CC1E bit to disable capture from the counter as we set it up. //CC1S bits aren't writeable when CC1E is set. //CC1E is bit 0 of CCER (page 401) bitClear(r.gen->CCER,0); //Capture/Compare 1 Selection // set CC1S bits to 01 in the capture compare mode register 1. // 01 selects TI1 as the input to use. (page 399 stm32 reference) // (assuming here that TI1 is D16, according to maple master pin map) //CC1S bits are bits 1,0 bitClear(r.gen->CCMR1, 1); bitSet(r.gen->CCMR1, 0); //Input Capture 1 Filter. // need to set IC1F bits according to a table saying how long // we should wait for a signal to be 'stable' to validate a transition // on the input. // (page 401 stm32 reference) //IC1F bits are bits 7,6,5,4 bitClear(r.gen->CCMR1, 7); bitClear(r.gen->CCMR1, 6); bitSet(r.gen->CCMR1, 5); bitSet(r.gen->CCMR1, 4); //sort out the input capture prescaler IC1PSC.. //00 no prescaler.. capture is done at every edge detected bitClear(r.gen->CCMR1, 3); bitClear(r.gen->CCMR1, 2); //select the edge for the transition on TI1 channel using CC1P in CCER //CC1P is bit 1 of CCER (page 401) // 0 = rising (non-inverted. capture is done on a rising edge of IC1) // 1 = falling (inverted. capture is done on a falling edge of IC1) bitClear(r.gen->CCER,1); //set the CC1E bit to enable capture from the counter. //CC1E is bit 0 of CCER (page 401) bitSet(r.gen->CCER,0); //enable dma for this timer.. //sets the Capture/Compare 1 DMA request enable bit on the DMA/interrupt enable register. //bit 9 is CC1DE as defined on page 393. bitSet(r.gen->DIER,9); }