//using FB_TX //TODO: ready pin #define NUM_OF_SAMPLES_Y 800 HAL_COMP(yaskawa); HAL_PIN(pos) = 0.0; HAL_PIN(error) = 0.0; HAL_PIN(dump) = 0; HAL_PIN(len) = 13; // SigmaII servomotor ( SGMAH serie ) 13bits Incremental encoder HAL_PIN(off) = 67; HAL_PIN(decode_len) = 99.0; //####################### HAL_PIN(position_compensation)=0.0; HAL_PIN(compensation_is_valide)=0.0; HAL_PIN(hal_sensor_state)=0.0; HAL_PIN(start_offset)=1000; // any value other than used in HAL-offset calculation !!! MEM(volatile uint32_t rxbuf[NUM_OF_SAMPLES_Y + 1]); MEM(volatile uint32_t txbuf[128]); MEM(int pos1) = 0; MEM(char data[150]); MEM(DMA_InitTypeDef DMA_InitStructurerx); RT_INIT( GPIO_InitTypeDef GPIO_InitStruct; // enable clocks RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); // RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_DMA2, ENABLE); //############# Don't use, this causes crash !!!!! //TX enable FB0_45E GPIO_InitStruct.GPIO_Pin = GPIO_Pin_15; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_25MHz; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOD, &GPIO_InitStruct); GPIO_ResetBits(GPIOD,GPIO_Pin_15); //TX FB0_45 GPIO_InitStruct.GPIO_Pin = GPIO_Pin_14; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOD, &GPIO_InitStruct); GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_TIM4); // TIM4_CH3 //TIM4 is used to toggle tx pin TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset; TIM_OC3Init(TIM4, &TIM_OCInitStructure); TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable); TIM_CtrlPWMOutputs(TIM4, ENABLE); //RX FB_45 data on PC12 input ( or PC6 input ) GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz; GPIO_Init(GPIOC, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz; GPIO_Init(GPIOC, &GPIO_InitStruct); //TIM8 triggers DMA to sample reply RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_Period = 20; // 168 / (20 + 1) = 8MHz TIM_TimeBaseStructure.TIM_Prescaler = 0; TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure); TIM_ARRPreloadConfig(TIM8,ENABLE); TIM_DMACmd(TIM8, TIM_DMA_Update, ENABLE); TIM_Cmd(TIM8, ENABLE); DMA_InitStructurerx.DMA_Channel = DMA_Channel_7; //TIM8 ch1 DMA_InitStructurerx.DMA_PeripheralBaseAddr = (uint32_t)&GPIOC->IDR; DMA_InitStructurerx.DMA_Memory0BaseAddr = (uint32_t)&rxbuf; DMA_InitStructurerx.DMA_DIR = DMA_DIR_PeripheralToMemory; DMA_InitStructurerx.DMA_BufferSize = NUM_OF_SAMPLES_Y; DMA_InitStructurerx.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructurerx.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructurerx.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructurerx.DMA_MemoryDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructurerx.DMA_Mode = DMA_Mode_Normal; DMA_InitStructurerx.DMA_Priority = DMA_Priority_Medium;//High; DMA_InitStructurerx.DMA_FIFOMode = DMA_FIFOMode_Disable; DMA_InitStructurerx.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; DMA_InitStructurerx.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructurerx.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; //manchaster //0 -> 01 //1 -> 10 //SYNC framing framing //0101010101010101010 01111110 111110 111110 111110 1 01111110; //010101... are to sync receiver //01111110 is hdlc start and end flag for framing //0 is appended after 5 ones => transmitted data is 0xffff //reply has the same format //"0101010101010101010 01111110 111110 111110 111110 1 01111110"; /*const */char request[] = "0101010101010101010 01111110 111110 111110 111110 1 01111110"; char request_m[sizeof(request)*2 +1]; uint32_t tim_a = TIM4->CCER; uint32_t tim_b = tim_a; tim_a &=~TIM_CCER_CC3P ; //~(1<<9); //0 tim_b |= TIM_CCER_CC3P ; // (1<<9); //1 //encode in manchaster int j = 0; for(int i = 0;request[i];i++){ if(request[i] == '0'){ request_m[j++] = '0'; request_m[j++] = '1'; }else if(request[i] == '1'){ request_m[j++] = '1'; request_m[j++] = '0'; } } request_m[j] = '\0'; pos1=0; //build txbuf for dma for(int i = 0;request_m[i];i++){ if(request_m[i] == '0'){ txbuf[pos1++] = tim_a; }else if(request_m[i] == '1'){ txbuf[pos1++] = tim_b; } } txbuf[pos1++] = tim_a; txbuf[pos1++] = tim_a; ); RT( DMA_InitTypeDef DMA_InitStructuretx; // DMA2-Config DMA_Cmd(DMA2_Stream1, DISABLE); DMA_DeInit(DMA2_Stream1); DMA_InitStructuretx.DMA_Channel = DMA_Channel_7; //TIM8 ch1 DMA_InitStructuretx.DMA_PeripheralBaseAddr = (uint32_t)&TIM4->CCER; DMA_InitStructuretx.DMA_Memory0BaseAddr = (uint32_t)&txbuf; DMA_InitStructuretx.DMA_DIR = DMA_DIR_MemoryToPeripheral; DMA_InitStructuretx.DMA_BufferSize = pos1; DMA_InitStructuretx.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructuretx.DMA_MemoryInc = DMA_MemoryInc_Enable; DMA_InitStructuretx.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructuretx.DMA_MemoryDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructuretx.DMA_Mode = DMA_Mode_Normal; DMA_InitStructuretx.DMA_Priority = DMA_Priority_Medium; //High; ////#################### VeryHigh; DMA_InitStructuretx.DMA_FIFOMode = DMA_FIFOMode_Disable; DMA_InitStructuretx.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull; DMA_InitStructuretx.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructuretx.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; DMA_Init(DMA2_Stream1, &DMA_InitStructuretx); TIM8->ARR = 20 ; // 168 / (20 + 1) = 8MHz TIM4->CCER |= (uint16_t)TIM_CCER_CC3E; // Re-enable OC3 //############################ GPIOD->BSRRL = GPIO_Pin_15; //TX enable DMA_Cmd(DMA2_Stream1, DISABLE); DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF1); DMA_Cmd(DMA2_Stream1, ENABLE);//transmit request while (!(DMA2->LISR & DMA_FLAG_TCIF1));//wait for request GPIOD->BSRRH = GPIO_Pin_15; //TX disable TIM4->CCER &= ~(uint16_t)TIM_CCER_CC3E; // Disable OC3 //############################ //DMA2-Config DMA_Cmd(DMA2_Stream1, DISABLE); DMA_DeInit(DMA2_Stream1); TIM8->ARR = 6; // 168 / (6 + 1) = 24MHz DMA_Init(DMA2_Stream1, &DMA_InitStructurerx); uint32_t error = 0; GPIOE->BSRRH = GPIO_Pin_1;//messpin //start DMA DMA_Cmd(DMA2_Stream1, ENABLE); //wait for DMA transfer complete while (DMA_GetFlagStatus(DMA2_Stream1, DMA_FLAG_TCIF1) == RESET); GPIOE->BSRRL = GPIO_Pin_1;//messpin uint32_t current; uint32_t last = (rxbuf[0] & GPIO_Pin_12); uint32_t first_edge = 0; //find any edge for(int j = 1; j < 20; j++){ current = (rxbuf[j] & GPIO_Pin_12); if(current != last){ first_edge = j; break; } last = current; } last = (rxbuf[first_edge] & GPIO_Pin_12); //align edge for(int j = first_edge+3; j < 20; j+=3){ current = (rxbuf[j] & GPIO_Pin_12); if(current == last && last != 0){ first_edge = j; break; } last = current; } if(first_edge == 0 || first_edge == 19){ error++; } first_edge %= 6; uint32_t bit = 0;// 2 manchaster bits = 1 bit uint32_t ones = 0;// received ones, needed for framing and unstuffing uint32_t j = 0;// unstuffing and start offset int pos_length = PIN(len);// position length int pos_offset = PIN(off);// position offset //int decode_length = pos_length + pos_offset + 1; int decode_length = PIN(decode_len); //##################### //decode manchaster and unstuff for(int i = j; i < (decode_length+j); i++){ bit = 0; if(rxbuf[i*6+first_edge+1]&GPIO_Pin_12){ bit += 2; } if(rxbuf[i*6+first_edge+4]&GPIO_Pin_12){ bit += 1; } switch(bit){ case 1://01 -> manchaster encoded 0 if(ones == 5){// 0 after 5 ones => stuffed data j++; }else{ data[i-j] = '0'; } ones = 0; break; case 2://10 -> manchaster encoded 1 if(ones == 5){// 1 after 5 ones => start of packet j = ++i; ones = 0; }else{ ones++; data[i-j] = '1'; } break; case 0://00 -> manchaster alignment error case 3://11 -> manchaster alignment error default://obsolete data[i-j] = 'X'; error++; break; } } data[100] = 0; // terminate string uint32_t ypos = 0; //extract position data for(int i = 0; i < pos_length; i++){ ypos += (data[i + pos_offset]=='1') << i; } if(error){ PIN(error) = 1.0; }else{ PIN(error) = 0.0; //extract compensation data uint32_t ycomp = 0; for(int j = 0; j < 11; j++){ ycomp += (data[j + 81]=='1') << j; } PIN(position_compensation) = (float) ycomp; //(1<<(11))); //extract compensation is valide if(data[7]=='0')PIN(compensation_is_valide)=1.0; else PIN(compensation_is_valide)=0.0; //extract hal sensor state uint32_t yHal = 0; for(int h = 0; h < 3; h++){ yHal += (data[h + 58]=='1') << h;} PIN(hal_sensor_state)=(float)yHal; uint32_t hal_off=0; hal_off=PIN(start_offset); if(hal_off==1000){ // do this only once switch(yHal) { case 2: hal_off=0; break; case 3: hal_off=456; break; case 1: hal_off=911; break; case 5: hal_off=1366; break; case 4: hal_off=1821; break; case 6: hal_off=2276; break; }; PIN(start_offset)=hal_off; } PIN(pos) = ( (float)((ypos + hal_off)%8192) /8192.0) * 2.0 * M_PI - M_PI; //############## } ); NRT( if(RISING_EDGE(PIN(dump))){ printf("%s\n",data); } ); ENDCOMP;