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