__INLINE void PhaseChangedRoutine(void)
{
FLAG_PHASE_CHANGED = RESET;
tMotor.structMotor.unPhaseChangeCNT++;
if (TRUE == tMotor.structMotor.MSR.bZeroCrossDetecting)
{
// iPhaseChangeTime = TIMER_GetCounter(TIMER1);
// Miss ZXD or ZXD success filter
// If continuously detected more than MIN_SUCC_ZXD_THRESHOLD ZX, OK! GOOD!!
if (TRUE == tMotor.structMotor.MSR.bThisPhaseDetectedZX)
{
tMotor.structMotor.MSR.unMissedZXD_CNT = 0;
if (tMotor.structMotor.MSR.unSuccessZXD_CNT > MIN_SUCC_ZXD_THRESHOLD)
{
tMotor.structMotor.MSR.bLocked = TRUE;
}
else
{
tMotor.structMotor.MSR.unSuccessZXD_CNT++;
}
}
else // If continuously missing detected more than MAX_MISS_ZXD_THRESHOLD ZX, loss lock
{
tMotor.structMotor.MSR.unSuccessZXD_CNT = 0;
// If ZX was not detected in last phase, unLastZXDetectedTime was also not updated
// Guess one value
unLastZXDetectedTime = GET_TIMER_DIFF((tMotor.structMotor.unActualPeriod >> 2), TIMER_GetCounter(TIMER1));
if (tMotor.structMotor.MSR.unMissedZXD_CNT > MAX_MISS_ZXD_THRESHOLD)
{
if (TRUE == tMotor.structMotor.MSR.bLocked)
{
tMotor.structMotor.MSR.bLocked = FALSE;
MOTOR_SHUT_DOWN;
setError(ERR_INTERNAL);
}
}
else
{
tMotor.structMotor.MSR.unMissedZXD_CNT++;
}
}
}
if (TRUE == tMotor.structMotor.MSR.bLocked)
{
// Set a rough next phase change time as the same with last phase
// After detected ZX in TIM1 interrupt, next phase change time will be re-configured
TIMER_SET_CMP_VALUE(TIMER0, tMotor.structMotor.unActualPeriod << 1);
}
tMotor.structMotor.MSR.bThisPhaseDetectedZX = FALSE;
// For debug
GPIO_TOGGLE(P50);
}
int main(void)
{
static unsigned int led_state = 0;
RCC_ClocksTypeDef clockinfo;
RCC_GetClocksFreq(&clockinfo);
// regardless of clock speed this gives us 1000 ticks per second
SysTick_Config(clockinfo.SYSCLK_Frequency / 1000);
int blink_speed_ms = 400;
setup_gpios();
setup_adc();
setup_usart();
setup_button_irqs();
kkputs("hello karl...\n");
uint64_t lasttime = millis();
while (1) {
if (millis() - blink_speed_ms > lasttime) {
if (led_state & 1) {
switch_leds_on();
kkputs("O");
} else {
switch_leds_off();
kkputs("o");
}
led_state ^= 1;
GPIO_TOGGLE(GPIOC, GPIO_Pin_3);
lasttime = millis();
}
if (button_pressed) {
button_pressed = 0;
kkputs("button was pressed!\n");
blink_speed_ms >>= 1;
if (blink_speed_ms <= 50) {
blink_speed_ms = 1000;
}
}
// start and wait for adc to convert...
ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 1, ADC_SampleTime_192Cycles);
ADC_SoftwareStartConv(ADC1);
while (ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC) == 0)
;
uint16_t pot_val = ADC_GetConversionValue(ADC1);
if (pot_val > 0x700) {
GPIO_HIGH(GPIOA, GPIO_Pin_4);
} else {
GPIO_LOW(GPIOA, GPIO_Pin_4);
}
}
}
static void prvCheckTimerCallback( TimerHandle_t xTimer )
{
static long lChangedTimerPeriodAlready = pdFALSE;
unsigned long ulErrorFound = pdFALSE;
/* Check all the demo tasks to ensure they are all still running, and that
none have detected an error. */
if( xAreDynamicPriorityTasksStillRunning() != pdTRUE )
{
ulErrorFound = pdTRUE;
}
if( xAreBlockingQueuesStillRunning() != pdTRUE )
{
ulErrorFound = pdTRUE;
}
if ( xAreBlockTimeTestTasksStillRunning() != pdTRUE )
{
ulErrorFound = pdTRUE;
}
if ( xAreGenericQueueTasksStillRunning() != pdTRUE )
{
ulErrorFound = pdTRUE;
}
if ( xAreRecursiveMutexTasksStillRunning() != pdTRUE )
{
ulErrorFound = pdTRUE;
}
if( xArePollingQueuesStillRunning() != pdTRUE )
{
ulErrorFound = pdTRUE;
}
if( xAreSemaphoreTasksStillRunning() != pdTRUE )
{
ulErrorFound = pdTRUE;
}
/* Toggle the check LED to give an indication of the system status. If
the LED toggles every mainCHECK_TIMER_PERIOD_MS milliseconds then
everything is ok. A faster toggle indicates an error. */
GPIO_TOGGLE( LD_GPIO_PORT, LD_GREEN_GPIO_PIN );
/* Have any errors been latch in ulErrorFound? If so, shorten the
period of the check timer to mainERROR_CHECK_TIMER_PERIOD_MS milliseconds.
This will result in an increase in the rate at which mainCHECK_LED
toggles. */
if( ulErrorFound != pdFALSE )
{
if( lChangedTimerPeriodAlready == pdFALSE )
{
lChangedTimerPeriodAlready = pdTRUE;
/* This call to xTimerChangePeriod() uses a zero block time.
Functions called from inside of a timer callback function must
*never* attempt to block. */
xTimerChangePeriod( xTimer, ( mainERROR_CHECK_TIMER_PERIOD_MS ), mainDONT_BLOCK );
}
}
}
void main(void)
{
init();
PSC_Init(0x00, MAX_PWM);
uart_init(UART_BAUD_SELECT_DOUBLE_SPEED(UART_BAUDRATE, F_CPU));
while(1)
{
/* Communicate with Big Brother :-) */
communication();
/* Sampling */
if (Flag_IT_timer0)
{
if ( !(timer % 100) )
{
/* This led shows that program is running */
GPIO_TOGGLE(WD_LED);
}
if ( !(timer % 10) )
{
GPIO_TOGGLE(LED);
/* Read meashured speed from Timer2 */
Omega_meas = TCNT1 * 10;
TCNT1 = 0;
/* PID controller */
pid_output = update_pid(reference_val.omega - Omega_meas, Omega_meas);
Command = ((int32_t)pid_output * 131) >> 10;
/* Update immediate vals */
instant_val.time = timer;
instant_val.omega = Omega_meas;
instant_val.error = reference_val.omega - Omega_meas;
instant_val.pid_output = pid_output;
instant_val.command = Command;
}
/* direction management : extract sign and absolute value */
if (Command > (int16_t)(0))
{
direction = 0;
OmegaTe = Command;
} else {
direction = 1;
OmegaTe = (~Command) + 1;
}
if (OmegaTe > K_scal)
{
// ------------------------ V/f law --------------------------
amplitude = controlVF(OmegaTe);
// ------------------ natural PWN algorithm ------------------
PWM0 = duty_cycle(theta1,amplitude);
PWM1 = duty_cycle(theta2,amplitude);
PWM2 = duty_cycle(theta3,amplitude);
} else {
PWM0 = 0;
PWM1 = 0;
PWM2 = 0;
}
// -------- load the PSCs with the new duty cycles -----------
PSC0_Load(PWM0, PWM0 + DeadTime);
if (!direction)
{
PSC1_Load(PWM1, PWM1 + DeadTime);
PSC2_Load(PWM2, PWM2 + DeadTime);
} else {
PSC2_Load(PWM1, PWM1 + DeadTime);
PSC1_Load(PWM2, PWM2 + DeadTime);
}
// 3 integrators for the evolution of the angles
theta1 = (K_scal * theta1 + OmegaTe) / K_scal;
theta2 = theta1 + 160;
theta3 = theta1 + 320;
if (theta1>=MAX_THETAx4) theta1 -= MAX_THETAx4;
if (theta2>=MAX_THETAx4) theta2 -= MAX_THETAx4;
if (theta3>=MAX_THETAx4) theta3 -= MAX_THETAx4;
Flag_IT_timer0 = 0;
}
}
/////////////////
// main routine
/////////////////
void main (void) {
int8_t ret1, ret2;
int16_t accX1, accY1, accZ1;
int16_t accX2, accY2, accZ2;
char buf[100];
uint8_t count=0, len=0;
/////////////////
// init peripherals
/////////////////
// disable interrupts
DISABLE_INTERRUPTS;
// switch to 16MHz (default is 2MHz)
CLK.CKDIVR.byte = 0x00;
// set default option bytes to assert bootloader is running
flash_OPT_default();
// init timer TIM3 for sleep and timeout (required by I2C)
tim3_init();
// init timer TIM4 for 1ms clock with interrupts
tim4_init();
// init I2C bus
i2c_init();
// init and reset LCD display
lcd_init();
// init pins for UART1 Rx(=PA4) and Tx(=PA5)
gpio_init(&PORT_A, PIN_4, INPUT_PULLUP_NOEXINT);
gpio_init(&PORT_A, PIN_5, OUTPUT_PUSHPULL_FAST);
// init UART1 to high speed (connected to PC on muBoard)
uart1_init(230400L);
// init LEDs on muBoard for visual feedback
GPIO_SET(PORT_H,PIN_2|PIN_3, 1);
gpio_init(&PORT_H, PIN_2|PIN_3, OUTPUT_PUSHPULL_FAST);
// enable interrupts
ENABLE_INTERRUPTS;
// init I2C routine pointers
I2C_routine();
// initialize sensors
do {
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_init(&bno055);
ret1 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret1 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret1 && USE_I2C_ADDR1);
do {
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_init(&bno055);
ret2 |= bno055_set_power_mode(POWER_MODE_NORMAL);
ret2 |= bno055_set_operation_mode(OPERATION_MODE_AMG);
} while (ret2 && USE_I2C_ADDR2);
/////////////////
// main loop
/////////////////
while (1) {
// every 1ms do
if (g_flagClock) {
g_flagClock = 0;
// every 10ms do
if (g_clock > 10) {
g_clock = 0;
// just to be sure
accX1 = accY1 = accZ1 = ret1 = 0;
accX2 = accY2 = accZ2 = ret2 = 0;
// read data from sensor 1
#if USE_I2C_ADDR1
bno055.dev_addr = BNO055_I2C_ADDR1;
ret1 = bno055_read_accel_x(&accX1);
ret1 |= bno055_read_accel_y(&accY1);
ret1 |= bno055_read_accel_z(&accZ1);
if (ret1 != 0) {
accX1 = accY1 = accZ1 = 0;
}
#endif // USE_I2C_ADDR1
// read data from sensor 2
#if USE_I2C_ADDR2
bno055.dev_addr = BNO055_I2C_ADDR2;
ret2 = bno055_read_accel_x(&accX2);
ret2 |= bno055_read_accel_y(&accY2);
ret2 |= bno055_read_accel_z(&accZ2);
if (ret2 != 0) {
accX2 = accY2 = accZ2 = 0;
}
#endif // USE_I2C_ADDR2
// send data to PC via UART1. Use SW FIFO for background operation
len = 0;
buf[len++] = (uint8_t)(accX1 >> 8); // x1-acc (MSB first)
buf[len++] = (uint8_t) accX1;
buf[len++] = (uint8_t)(accY1 >> 8); // y1-acc (MSB first)
buf[len++] = (uint8_t) accY1;
buf[len++] = (uint8_t)(accZ1 >> 8); // z1-acc (MSB first)
buf[len++] = (uint8_t) accZ1;
buf[len++] = (uint8_t)(accX2 >> 8); // x2-acc (MSB first)
buf[len++] = (uint8_t) accX2;
buf[len++] = (uint8_t)(accY2 >> 8); // y2-acc (MSB first)
buf[len++] = (uint8_t) accY2;
buf[len++] = (uint8_t)(accZ2 >> 8); // z2-acc (MSB first)
buf[len++] = (uint8_t) accZ2;
uart1_send_buf(len, buf);
// indicate I2C status via red LED (on=ok)
GPIO_SET(PORT_H,PIN_3, ret1|ret2);
// show life beat via green LED
if (++count > 20) {
count = 0;
GPIO_TOGGLE(PORT_H,PIN_2);
// print to LCD
sprintf(buf, "%02d %03d %03d %03d", (int) ret1, (int) accX1, (int) accY1, (int) accZ1);
lcd_print(1, 1, buf);
sprintf(buf, "%02d %03d %03d %03d", (int) ret2, (int) accX2, (int) accY2, (int) accZ2);
lcd_print(2, 1, buf);
}
} // loop 10ms
} // loop 1ms
} // main loop
/*******************************************************************************
* Function Name : Green_Led_TOGGLE
* Description : Toggles Green LED.
* Return : None.
*******************************************************************************/
void Green_Led_TOGGLE(void)
{
GPIO_TOGGLE(LED_GREEN_PORT, LED_GREEN_PIN);
}
/*******************************************************************************
* Function Name : Red_Led_TOGGLE
* Description : Toggles Red LED.
* Return : None.
*******************************************************************************/
void Red_Led_TOGGLE(void)
{
GPIO_TOGGLE(LED_RED_PORT, LED_RED_PIN);
}
void test2(){
GPIO_TOGGLE(LED2_PORT, LED2_PIN);
}
//unsigned char inverse_led1_flag = 0;
//unsigned char inverse_led2_flag = 0;
//unsigned char oneSec_flag = 0;
//unsigned char testarray[10];
//unsigned int testWord[5];
void test1(){
GPIO_TOGGLE(LED1_PORT, LED1_PIN);
}
int main(void)
{
uint8_t ch;
/* Enable GPIO clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
/* Enable USART3 clock */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_6, Bit_RESET);
GPIO_WriteBit(GPIOB, GPIO_Pin_7, Bit_RESET);
/* Configure USART3 Rx & Tx as alternate function */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_40MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOC, &GPIO_InitStructure);
/* GPIOC --> USART3 Rx & Tx */
GPIO_PinAFConfig(GPIOC, GPIO_PinSource10, GPIO_AF_USART3);
GPIO_PinAFConfig(GPIOC, GPIO_PinSource11, GPIO_AF_USART3);
USART_InitStructure.USART_BaudRate = 9600;
USART_InitStructure.USART_WordLength = USART_WordLength_9b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
/* USART configuration */
USART_Init(USART3, &USART_InitStructure);
/* Enable USART */
USART_Cmd(USART3, ENABLE);
do {
USART_ReceiveData(USART3);
} while(USART_GetFlagStatus(USART3, USART_FLAG_RXNE) != RESET);
while(1) {
/* wait for character to arrive */
while(USART_GetFlagStatus(USART3, USART_FLAG_RXNE) == RESET)
;
GPIO_TOGGLE(GPIOB,GPIO_Pin_6);
/* read the available data */
ch = USART_ReceiveData(USART3);
ch++;
/* Wait while TX is full */
while(USART_GetFlagStatus(USART3, USART_FLAG_TXE) == RESET)
;
/*transmit data only when TX is empty */
USART_SendData(USART3, ch);
GPIO_TOGGLE(GPIOB, GPIO_Pin_7);
}
}
