void setupMotor(Motor *motor,
PositionTracker *currentPosition,
TIM_TypeDef* TIMx,
u16 upChannel, u16 downChannel,
portTickType pollingPeriod,
unsigned portBASE_TYPE uxPriority) {
TIM_OCInitTypeDef TIM_OCInitStruct;
portBASE_TYPE res;
motor->currentPosition = currentPosition;
motor->targetPosition = 0;
motor->stopped = 0;
motor->lock = xSemaphoreCreateMutex();
assert(motor->lock != NULL);
motor->TIMx = TIMx;
motor->upChannel = upChannel;
motor->downChannel = downChannel;
motor->pollingPeriod = pollingPeriod;
// Setup two timer channels for PWM output
TIM_OCStructInit(&TIM_OCInitStruct);
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStruct.TIM_OCPolarity = 1;
TIM_OCInitStruct.TIM_Pulse = 0;
TIM_OCInitStruct.TIM_Channel = upChannel;
TIM_OCInit(TIMx, &TIM_OCInitStruct);
TIM_OCInitStruct.TIM_Channel = downChannel;
TIM_OCInit(TIMx, &TIM_OCInitStruct);
res = xTaskCreate(motorTask, "motor", 80,
(void*)motor, uxPriority, NULL);
assert(res == pdTRUE);
}
/*******************************************************************************
* Function Name : TIM2_Configuration
* Description : Configures TIM2 output 8K as WS of DAC.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void TIM2_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = TIM2_Pre;
TIM_TimeBaseStructure.TIM_ClockDivision = 0x0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
/* Output Compare Toggle Mode configuration: Channel1 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle;
TIM_OCInitStructure.TIM_Channel = TIM_Channel_2;
TIM_OCInitStructure.TIM_Pulse = TIM2_CCR2_Val;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OCInit(TIM2, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Disable);
// TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Disable);//
}
/*******************************************************************************
* Function Name : TIM3_Configuration
* Description : Configures TIM3 as sync_times.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void TIM3_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_TimeBaseStructure.TIM_Period = FRAME_PERIOD;
TIM_TimeBaseStructure.TIM_Prescaler = TIM3_Pre;
TIM_TimeBaseStructure.TIM_ClockDivision = 0x0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
/* Output Compare Toggle Mode configuration: Channel1 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
TIM_OCInitStructure.TIM_Channel = TIM_Channel_1;
TIM_OCInitStructure.TIM_Pulse = FRAME_PERIOD-1;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInit(TIM3, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Disable);
TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE);
// TIM_ARRPreloadConfig(TIM3,DISABLE);
}
/* Configures the used Timers. ----------------------------------------------*/
void TIM_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure2;
TIM_OCInitTypeDef TIM_OCInitStructure3;
TIM_OCInitTypeDef TIM_OCInitStructure8;
/* TIM2 configuration for LED -------------------------------------------*/
TIM_TimeBaseStructure.TIM_Period = 0x12B; //299
TIM_TimeBaseStructure.TIM_Prescaler = 0xEA5F; //59999
TIM_TimeBaseStructure.TIM_ClockDivision = 0x0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
/* Outoput Compare Init */
TIM_OCStructInit(&TIM_OCInitStructure2);
/* Output Compare Timing Mode configuration: Channel1 */
TIM_OCInitStructure2.TIM_OCMode = TIM_OCMode_Timing;
TIM_OCInitStructure2.TIM_Pulse = 0x0;
TIM_OCInit(TIM2, &TIM_OCInitStructure2);
/* TIM2 enable counter */
TIM_Cmd(TIM2, ENABLE);
/* Immediate load of TIM2 Precaler value */
TIM_PrescalerConfig(TIM2, 0xEA5F, TIM_PSCReloadMode_Immediate);
/* Clear TIM2 update pending flag */
TIM_ClearFlag(TIM2, TIM_FLAG_Update);
/* Enable TIM2 Update interrupt */
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
/* TIM3 configuration for Statemachine (main control loop) --------------*/
TIM_TimeBaseStructure.TIM_Period = 1000; //alle 1ms
TIM_TimeBaseStructure.TIM_Prescaler = 0x48; //72
TIM_TimeBaseStructure.TIM_ClockDivision = 0x0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
/* Outoput Compare Init */
TIM_OCStructInit(&TIM_OCInitStructure3);
/* Output Compare Timing Mode configuration: Channel1 */
TIM_OCInitStructure3.TIM_OCMode = TIM_OCMode_Timing;
TIM_OCInitStructure3.TIM_Pulse = 0x0;
TIM_OCInit(TIM3, &TIM_OCInitStructure3);
/* TIM3 enable counter */
TIM_Cmd(TIM3, ENABLE);
/* Immediate load of TIM3 Precaler value */
TIM_PrescalerConfig(TIM3, 0x48, TIM_PSCReloadMode_Immediate);
/* Clear TIM3 update pending flag */
TIM_ClearFlag(TIM3, TIM_FLAG_Update);
/* Enable TIM3 Update interrupt */
// enable in main after powerup
//TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
/* Timer for PPM decode ---------------------------------------------------*/
TIM1_ICInitTypeDef TIM1_ICInitStructure;
//TIM_ICInitStructure.TIM_ICMode = TIM_ICMode_ICAP;
#ifndef PPM_PA11
TIM1_ICInitStructure.TIM1_Channel = TIM1_Channel_1; //Pin: PA8
#else
TIM1_ICInitStructure.TIM1_Channel = TIM1_Channel_4; //Pin: PA11
#endif
TIM1_ICInitStructure.TIM1_ICPolarity = TIM1_ICPolarity_Falling;
TIM1_ICInitStructure.TIM1_ICSelection = TIM1_ICSelection_DirectTI;
TIM1_ICInitStructure.TIM1_ICPrescaler = TIM1_ICPSC_DIV1;
TIM1_ICInitStructure.TIM1_ICFilter = 0x0;
TIM1_ICInit(&TIM1_ICInitStructure);
TIM1_SelectInputTrigger(TIM1_TS_TI2FP2);
TIM1_InternalClockConfig();
TIM1_TimeBaseInitTypeDef TIM1_TimeBaseStructure;
TIM1_TimeBaseStructure.TIM1_Period = 0xFFFF;
TIM1_TimeBaseStructure.TIM1_Prescaler = 17; // fCK_PSC / (17 + 1) 1ms = 4000
TIM1_TimeBaseStructure.TIM1_ClockDivision = TIM1_CKD_DIV1;
TIM1_TimeBaseStructure.TIM1_CounterMode = TIM1_CounterMode_Up;
TIM1_TimeBaseInit(& TIM1_TimeBaseStructure);
/* TIM enable counter */
TIM1_Cmd(ENABLE);
/* Enable the CC1 Interrupt Request */
#ifndef PPM_PA11
TIM1_ITConfig(TIM_IT_CC1, ENABLE);
#else
TIM1_ITConfig(TIM_IT_CC4, ENABLE);
#endif
/* Timer for Servo 0-3 --------------------------------------------------*/
/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = 5999;
TIM_TimeBaseStructure.TIM_Prescaler = 72;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
/* PWM1 Mode configuration: Channel1 */
TIM_OCInitStructure8.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure8.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure8.TIM_Channel = TIM_Channel_1;
TIM_OCInitStructure8.TIM_Pulse = 1500;
TIM_OCInitStructure8.TIM_OCPolarity = TIM_OCPolarity_High; // maybee it works with TIM_OCPolarity_LOW???
TIM_OCInit(TIM8, &TIM_OCInitStructure8);
TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel2 */
TIM_OCInitStructure8.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure8.TIM_Channel = TIM_Channel_2;
TIM_OCInitStructure8.TIM_Pulse = 1500;
TIM_OCInit(TIM8, &TIM_OCInitStructure8);
TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel3 */
TIM_OCInitStructure8.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure8.TIM_Channel = TIM_Channel_3;
TIM_OCInitStructure8.TIM_Pulse = 1500;
TIM_OCInit(TIM8, &TIM_OCInitStructure8);
TIM_OC3PreloadConfig(TIM8, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel4 */
TIM_OCInitStructure8.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure8.TIM_Channel = TIM_Channel_4;
TIM_OCInitStructure8.TIM_Pulse = 1500;
TIM_OCInit(TIM8, &TIM_OCInitStructure8);
TIM_OC4PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_ARRPreloadConfig(TIM8, ENABLE);
/* Enable the TIM Main Output only needed for timer 1 and 8*/
TIM8->BDTR |= ((u16)0x8000); //BDTR_MOE_Set;
/* TIM8 enable counter */
TIM_Cmd(TIM8, ENABLE);
/* Immediate load of TIM8 Precaler value */
//TIM_PrescalerConfig(TIM8, 72, TIM_PSCReloadMode_Immediate);
/* Clear TIM8 update pending flag */
//TIM_ClearFlag(TIM8, TIM_FLAG_Update);
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
char test[25];
unsigned int an1, an2, an3, an4, an5;
float Ta=0;
int i;
long msum[4];
float fm[4], T1, T2, T3, SP;
unsigned int DSState = 0;
unsigned long DSTimer=0, t0, t1, dt, HTimer, TTimer, t_sec, t_min;
unsigned char data[2];
unsigned char out=0;
unsigned int decval;
volatile float pp, pi, pd, f_error, error_old=0, pid_out, pid_out_i, pid_out_p, pid_out_d, error_i=0;
u8 Send_Buffer[25];
u8 tmp[4];
//char no_windup = 0;
RCC_Configuration();
NVIC_Configuration();
/* SysTick end of count event each 1ms with input clock equal to 9MHz (HCLK/8, default) */
SysTick_SetReload(9000);
/* Enable SysTick interrupt */
SysTick_ITConfig(ENABLE);
/* Enable the SysTick Counter */
SysTick_CounterCmd(SysTick_Counter_Enable);
GPIO_Setup();
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2, ENABLE);
/* SPI2 Config -------------------------------------------------------------*/
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge;
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256;
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_Init(SPI2, &SPI_InitStructure);
/* Enable SPI1 */
SPI_CalculateCRC(SPI2, DISABLE);
SPI_Cmd(SPI2, ENABLE);
InitADC1(); // ADC1 Init
OWInit(&OneWire, GPIOB, GPIO_Pin_8);
/* Connect Key Button EXTI Line to Key Button GPIO Pin */
//GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_Pin_7);
/* Configure Key Button EXTI Line to generate an interrupt on falling edge */
//EXTI_InitStructure.EXTI_Line = EXTI_Line7;
//EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
//EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
//EXTI_InitStructure.EXTI_LineCmd = ENABLE;
//EXTI_Init(&EXTI_InitStructure);
/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = 55000;
TIM_TimeBaseStructure.TIM_Prescaler = 12;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
/* TIM2 PWM2 Mode configuration: Channel1 */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
TIM_OCInitStructure.TIM_Channel = TIM_Channel_1;
TIM_OCInitStructure.TIM_Pulse = 1;//20000;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInit(TIM3, &TIM_OCInitStructure);
/* TIM2 configuration in Input Capture Mode */
TIM_ICStructInit(&TIM_ICInitStructure);
TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
TIM_ICInitStructure.TIM_ICFilter = 0;
TIM_ICInit(TIM3, &TIM_ICInitStructure);
/* One Pulse Mode selection */
TIM_SelectOnePulseMode(TIM3, TIM_OPMode_Single);
/* Input Trigger selection */
TIM_SelectInputTrigger(TIM3, TIM_TS_TI2FP2);
/* Slave Mode selection: Trigger Mode */
TIM_SelectSlaveMode(TIM3, TIM_SlaveMode_Trigger);
ST7565_st7565_init();
ST7565_st7565_command(CMD_SET_BIAS_9);
ST7565_st7565_command(CMD_DISPLAY_ON);
ST7565_st7565_command(CMD_SET_ALLPTS_NORMAL);
ST7565_st7565_set_brightness(0x0C);
OWSearch(&OneWire, addr);
/*sprintf(test, "%02X %02X %02X %02X %02X %02X %02X %02X",
addr[7],addr[6],
addr[5],addr[4],
addr[3],addr[2],
addr[1],addr[0]);
ST7565_drawstring(6, 6, test);
*/
USB_Init();
ST7565_display(); // show splashscreen
t0 = millis();
HTimer = millis();
TTimer = millis();
pp = 10;
pi = 0;
pd = 150;
while(1)
{
msum[0] = 0;
msum[1] = 0;
msum[2] = 0;
msum[3] = 0;
for (i = 0; i < 1000; i++) {
an1 = GetADC1Channel(ADC_Channel_1);
an2 = GetADC1Channel(ADC_Channel_2);
an3 = GetADC1Channel(ADC_Channel_3);
an4 = GetADC1Channel(ADC_Channel_4);
an5 = GetADC1Channel(ADC_Channel_5);
msum[0] += an1;
msum[1] += an3 - an2;
msum[2] += an4 - an2;
msum[3] += an5 - an2;
//DelayuS(333);
}
SP = round((msum[0] / 1000.0) * (60.0 / 4096)) * 5;
fm[1] = (msum[1] / 1000.0);
fm[2] = (msum[2] / 1000.0) + 12;
fm[3] = (msum[3] / 1000.0) - 7;
T1 = (T1 + Ta + Dac2Dt(fm[1])) / 2;
T2 = (T2 + Ta + Dac2Dt(fm[2])) / 2;
T3 = (T3 + Ta + Dac2Dt(fm[3])) / 2;
t1 = millis();
dt = t1 - t0;
t0 = t1;
if (millis() - HTimer > 1000) {
f_error = SP - T2;
//if (noerror_i += error;
pid_out_p = pp * f_error;
pid_out_i = pi * error_i;
pid_out_d = pd * (f_error - error_old);
pid_out = pid_out_p + pid_out_i + pid_out_d;
error_old = f_error;
//out = pid_out;
if (pid_out > 99) {
out = 99;
} else if (pid_out < 0) {
out = 0;
} else {
out = round(pid_out);
}
TIM_SetCompare1(TIM3, 55000-PowerValues[out]);
HTimer += 1000;
//error_old = 10;
sprintf(test, "T1 : %5.1f E %5.1f ", T1, f_error);
ST7565_drawstring(6, 0, test);
sprintf(test, "T2 : %5.1f ", T2);
ST7565_drawstring(6, 1, test);
sprintf(test, "T3 : %5.1f ", T3);
ST7565_drawstring(6, 2, test);
sprintf(test, "SP : %5.1f P %6.1f ", SP, pid_out_p);
ST7565_drawstring(6, 3, test);
sprintf(test, "Ta : %5.1f I %6.1f ", Ta, pid_out_i);
ST7565_drawstring(6, 4, test);
sprintf(test, "dt : %5lu D %6.1f ", dt, pid_out_d);
ST7565_drawstring(6, 5, test);
sprintf(test, "out: %3u %% %6.1f ", out, pid_out);
ST7565_drawstring(6, 6, test);
t_sec = (millis() - TTimer) / 1000;
t_min = floor(t_sec / 60);
t_sec %= 60;
Send_Buffer[0] = 0x07;
decval = round(T1 * 100);
memcpy(&Send_Buffer[1], &decval, 2);
decval = round(T2 * 100);
memcpy(&Send_Buffer[3], &decval, 2);
decval = round(T3 * 100);
memcpy(&Send_Buffer[5], &decval, 2);
decval = round(SP * 100);
memcpy(&Send_Buffer[7], &decval, 2);
memcpy(&Send_Buffer[9], &out, 1);
//sprintf(test, "%02X %02X %02X %02X ", Send_Buffer[1], Send_Buffer[2], Send_Buffer[3], Send_Buffer[4]);
//ST7565_drawstring(6, 7, test);
UserToPMABufferCopy(Send_Buffer, ENDP1_TXADDR, 9);
SetEPTxCount(ENDP1, 9);
SetEPTxValid(ENDP1);
ST7565_display();
}
if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == 0) {
TTimer = millis();
}
//onewire
switch (DSState){
case 0:
OWReset(&OneWire);
OWWrite(&OneWire, 0xCC); // skip ROM
OWWrite(&OneWire, 0x44); // start conversion
DSTimer = millis();
DSState++;
break;
case 1:
if((millis() - DSTimer) >= 1000){
OWReset(&OneWire);
OWSelect(&OneWire, addr);
OWWrite(&OneWire, 0xBE); // Read Scratchpad
data[0] = OWRead(&OneWire);
data[1] = OWRead(&OneWire);
Ta = ((data[1] << 8) | data[0]) / 16.0;
DSState = 0;
}
break;
}
}
}