fixed unstability and refactored to separate file (b43c3bfd) · Commits · Ján Labuda / NXP_CUP

src/firmware/board/peripherals.c

+9 −8

Original line number	Diff line number	Diff line
		@@ -535,11 +535,11 @@ instance:
		- enable_irq: 'true'
		- ftm_interrupt:
		- IRQn: 'FTM3_IRQn'
		- enable_interrrupt: 'noInit'
		- enable_interrrupt: 'enabled'
		- enable_priority: 'false'
		- priority: '0'
		- enable_custom_name: 'false'
		- EnableTimerInInit: 'false'
		- EnableTimerInInit: 'true'
		- ftm_edge_aligned_mode:
		- ftm_edge_aligned_channels_config:
		- 0:
		@@ -578,12 +578,13 @@ const ftm_dual_edge_capture_param_t FTM3_CONTROLLER_STATUS_channel1 = {


		static void FTM3_CONTROLLER_STATUS_init(void) {
		//FTM_Init(FTM3_CONTROLLER_STATUS_PERIPHERAL, &FTM3_CONTROLLER_STATUS_config);
		//FTM_SetTimerPeriod(FTM3_CONTROLLER_STATUS_PERIPHERAL, FTM3_CONTROLLER_STATUS_TIMER_MODULO_VALUE);
		//FTM_SetupDualEdgeCapture(FTM3_CONTROLLER_STATUS_PERIPHERAL, kFTM_Chnl_1, &FTM3_CONTROLLER_STATUS_channel1, 0);
		//FTM_EnableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL, kFTM_Chnl2InterruptEnable\|kFTM_Chnl3InterruptEnable \| kFTM_TimeOverflowInterruptEnable);
		/* Interrupt FTM3_IRQn request in the NVIC is not initialized (disabled by default). */
		/* It can be enabled later by EnableIRQ(FTM3_CONTROLLER_STATUS_IRQN); function call. */
		FTM_Init(FTM3_CONTROLLER_STATUS_PERIPHERAL, &FTM3_CONTROLLER_STATUS_config);
		FTM_SetTimerPeriod(FTM3_CONTROLLER_STATUS_PERIPHERAL, FTM3_CONTROLLER_STATUS_TIMER_MODULO_VALUE);
		FTM_SetupDualEdgeCapture(FTM3_CONTROLLER_STATUS_PERIPHERAL, kFTM_Chnl_1, &FTM3_CONTROLLER_STATUS_channel1, 0);
		FTM_EnableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL, kFTM_Chnl2InterruptEnable\|kFTM_Chnl3InterruptEnable \| kFTM_TimeOverflowInterruptEnable);
		/* Enable interrupt FTM3_IRQn request in the NVIC. */
		EnableIRQ(FTM3_CONTROLLER_STATUS_IRQN);
		FTM_StartTimer(FTM3_CONTROLLER_STATUS_PERIPHERAL, kFTM_SystemClock);
		}

		/***********************************************************************************************************************

src/firmware/firmware.mex

+2 −2

Original line number	Diff line number	Diff line
		@@ -1275,12 +1275,12 @@
		<setting name="enable_irq" value="true"/>
		<struct name="ftm_interrupt">
		<setting name="IRQn" value="FTM3_IRQn"/>
		<setting name="enable_interrrupt" value="noInit"/>
		<setting name="enable_interrrupt" value="enabled"/>
		<setting name="enable_priority" value="false"/>
		<setting name="priority" value="0"/>
		<setting name="enable_custom_name" value="false"/>
		</struct>
		<setting name="EnableTimerInInit" value="false"/>
		<setting name="EnableTimerInInit" value="true"/>
		</config_set>
		<config_set name="ftm_edge_aligned_mode">
		<array name="ftm_edge_aligned_channels_config">

src/firmware/source/controls/Controller.cpp

0 → 100644

+128 −0

Original line number	Diff line number	Diff line
		#include "Controller.hpp"
		#include "fsl_ftm.h"
		#include "peripherals.h"

		volatile bool ftmFirstChannelInterruptFlag = false;
		volatile bool ftmSecondChannelInterruptFlag = false;
		volatile uint32_t g_timerOverflowInterruptCount = 0u;
		volatile uint32_t g_firstChannelOverflowCount = 0u;
		volatile uint32_t g_secondChannelOverflowCount = 0u;

		extern "C" {
		void FTM3_CONTROLLER_STATUS_IRQHANDLER(void) {
		if ((FTM_GetStatusFlags(FTM3_CONTROLLER_STATUS_PERIPHERAL)
		& kFTM_TimeOverflowFlag) == kFTM_TimeOverflowFlag) {
		/* Clear overflow interrupt flag.*/
		FTM_ClearStatusFlags(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		kFTM_TimeOverflowFlag);
		g_timerOverflowInterruptCount++;
		} else if (((FTM_GetStatusFlags(FTM3_CONTROLLER_STATUS_PERIPHERAL)
		& FTM_FIRST_CHANNEL_FLAG) == FTM_FIRST_CHANNEL_FLAG)
		&& (ftmFirstChannelInterruptFlag == false)) {
		/* Disable first channel interrupt.*/
		FTM_DisableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		FTM_FIRST_CHANNEL_INTERRUPT_ENABLE);
		g_firstChannelOverflowCount = g_timerOverflowInterruptCount;
		ftmFirstChannelInterruptFlag = true;
		} else if ((FTM_GetStatusFlags(FTM3_CONTROLLER_STATUS_PERIPHERAL)
		& FTM_SECOND_CHANNEL_FLAG) == FTM_SECOND_CHANNEL_FLAG) {
		/* Clear second channel interrupt flag.*/
		FTM_ClearStatusFlags(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		FTM_SECOND_CHANNEL_FLAG);
		/* Disable second channel interrupt.*/
		FTM_DisableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		FTM_SECOND_CHANNEL_INTERRUPT_ENABLE);
		g_secondChannelOverflowCount = g_timerOverflowInterruptCount;
		ftmSecondChannelInterruptFlag = true;
		} else {
		}
		__DSB();
		}
		}

		float FTM_PwmPulseWithCaculate(FTM_Type *base, ftm_chnl_t chnlPair,
		uint32_t firstChannelOverflowCount,
		uint32_t secondChannelOverflowCount) {
		uint32_t capture1Val = base->CONTROLS[chnlPair * 2].CnV;
		uint32_t capture2Val = base->CONTROLS[(chnlPair * 2) + 1].CnV;

		if (capture2Val > capture1Val) {
		return (float) (((secondChannelOverflowCount - firstChannelOverflowCount)
		* 65536 + capture2Val - capture1Val) + 1)
		/ ((float) FTM3_CONTROLLER_STATUS_CLOCK_SOURCE / 1000000);
		}
		return 0;
		}

		float fabs(float x) {
		return (x < 0) ? -x : x;
		}

		bool values_are_homogenous(float values[], int size, float tolerance) {
		if (size <= 1)
		return true;

		for (int i = 1; i < size; i++) {
		if (fabs(values[i] - values[i - 1]) > tolerance)
		return false;
		}
		return true;
		}

		controller_status_t get_controller_status() {
		uint8_t i = 0;
		float pulseWidth[PULSE_WIDTH_SAMPLING_WINDOW_SIZE] = { 0 };

		while (pulseWidth[0] == 0
		\|\| !values_are_homogenous(pulseWidth,
		PULSE_WIDTH_SAMPLING_WINDOW_SIZE, 100)) {
		while (ftmFirstChannelInterruptFlag != true) {
		}

		while (ftmSecondChannelInterruptFlag != true) {
		}
		ftmFirstChannelInterruptFlag = false;
		ftmSecondChannelInterruptFlag = false;

		/* Clear first channel interrupt flag after the second edge is detected.*/
		FTM_ClearStatusFlags(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		FTM_FIRST_CHANNEL_FLAG);

		/* Clear overflow interrupt flag.*/
		FTM_ClearStatusFlags(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		kFTM_TimeOverflowFlag);
		/* Disable overflow interrupt.*/
		FTM_DisableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		kFTM_TimeOverflowInterruptEnable);

		float currentPulseWidth = FTM_PwmPulseWithCaculate(
		FTM3_CONTROLLER_STATUS_PERIPHERAL,
		FTM3_CONTROLLER_STATUS_DUAL_CAPTURE_PAIR,
		g_firstChannelOverflowCount, g_secondChannelOverflowCount);

		if (currentPulseWidth != 0) {
		pulseWidth[i] = currentPulseWidth;
		i = (i + 1) % PULSE_WIDTH_SAMPLING_WINDOW_SIZE;
		}

		g_secondChannelOverflowCount = 0;
		g_firstChannelOverflowCount = 0;
		/* Enable first channel interrupt */
		FTM_EnableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		FTM_FIRST_CHANNEL_INTERRUPT_ENABLE);

		/* Enable second channel interrupt when the second edge is detected */
		FTM_EnableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		FTM_SECOND_CHANNEL_INTERRUPT_ENABLE);

		/* Enable overflow interrupt */
		FTM_EnableInterrupts(FTM3_CONTROLLER_STATUS_PERIPHERAL,
		kFTM_TimeOverflowInterruptEnable);
		}

		if (pulseWidth[0] > 1700 && pulseWidth[0] < 2000)
		return CONTROLLER_OFF;
		if (pulseWidth[0] > 700 && pulseWidth[0] < 1200)
		return CONTROLLER_HAS_CONTROL;
		return CONTROLLER_NAN;
		}

src/firmware/source/controls/Controller.hpp

0 → 100644

+22 −0

Original line number	Diff line number	Diff line
		#ifndef CONTROLS_CONTROLLER_HPP_
		#define CONTROLS_CONTROLLER_HPP_

		#define PULSE_WIDTH_SAMPLING_WINDOW_SIZE 5

		/* Interrupt to enable and flag to read; depends on the FTM channel used for dual-edge capture */
		#define FTM_FIRST_CHANNEL_INTERRUPT_ENABLE kFTM_Chnl2InterruptEnable
		#define FTM_FIRST_CHANNEL_FLAG kFTM_Chnl2Flag
		#define FTM_SECOND_CHANNEL_INTERRUPT_ENABLE kFTM_Chnl3InterruptEnable
		#define FTM_SECOND_CHANNEL_FLAG kFTM_Chnl3Flag

		typedef enum controller_status {
		CONTROLLER_HAS_CONTROL, CONTROLLER_OFF, CONTROLLER_NAN,
		} controller_status_t;

		controller_status_t get_controller_status();

		extern "C" {
		void FTM3_CONTROLLER_STATUS_IRQHANDLER(void);
		}

		#endif /* CONTROLS_CONTROLLER_HPP_ */

src/firmware/source/firmware.cpp

+194 −386

Original line number	Diff line number	Diff line
		@@ -29,17 +29,6 @@
		*/

		#include <stdio.h>
		#include <Config.hpp>
		#include "board.h"
		#include <algorithm>
		#include "peripherals.h"
		#include "pin_mux.h"
		#include "clock_config.h"
		#include "MK66F18.h"
		#include "fsl_debug_console.h"
		#include <controls/Controls.hpp>

		// TODO: prefiltruj
		#include "board.h"
		#include "peripherals.h"
		#include "pin_mux.h"
		@@ -48,20 +37,20 @@
		#include "fsl_debug_console.h"
		#include "fsl_gpio.h"

		#include <Config.hpp>
		#include <algorithm>
		#include <array>
		#include <controls/LinearCamera.hpp>
		#include <Utils/Delay.hpp>
		#include <Utils/Print.hpp>
		#include <Utils/FrameProcessing.hpp>
		#include <Utils/Algorithms.hpp>
		#include <controls/Controller.hpp>
		#include <controls/Controls.hpp>
		#include <controls/SpeedSensor.hpp>
		#include <controls/Led.hpp>
		#include <Config.hpp>
		#include "Schumacher.hpp"
		#include <algorithm>

		// TODO: prefiltruj
		/*
		void print_negative(const char *format, ...)
		{
		@@ -77,8 +66,7 @@
		using frame = LinearCamera::frame;

		void load_data(frame &raw, frame &filtered, frame &derived,
		uint32_t exposition)
		{
		uint32_t exposition) {
		LinearCamera::readFrameWithExpositionUs(raw, exposition);
		median_filter<uint8_t, 128, 5>(raw, filtered);
		compute_derivative(filtered, derived);
		@@ -101,162 +89,19 @@ void load_data(frame &raw, frame &filtered, frame &derived,
		}
		*/

		#define FTM_SOURCE_CLOCK CLOCK_GetFreq(kCLOCK_BusClk)

		typedef enum controller_status
		{
		CONTROLLER_ON,
		CONTROLLER_OFF,
		CONTROLLER_NAN,
		} controller_status_t;
		#define USE_CONTROLLER

		bool ftm_running = true;

		/* The Flextimer instance/channel used for board */
		#define DEMO_FTM_BASEADDR FTM3
		/* FTM channel pair used for the dual-edge capture, channel pair 1 uses channels 2 and 3 */
		#define BOARD_FTM_INPUT_CAPTURE_CHANNEL_PAIR kFTM_Chnl_1
		/* Interrupt number and interrupt handler for the FTM instance used */
		#define FTM_INTERRUPT_NUMBER FTM3_IRQn
		#define FTM_INPUT_CAPTURE_HANDLER FTM3_IRQHandler
		/* Interrupt to enable and flag to read; depends on the FTM channel used for dual-edge capture */
		#define FTM_FIRST_CHANNEL_INTERRUPT_ENABLE kFTM_Chnl2InterruptEnable
		#define FTM_FIRST_CHANNEL_FLAG kFTM_Chnl2Flag
		#define FTM_SECOND_CHANNEL_INTERRUPT_ENABLE kFTM_Chnl3InterruptEnable
		#define FTM_SECOND_CHANNEL_FLAG kFTM_Chnl3Flag

		/* Get source clock for FTM driver */
		#define FTM_SOURCE_CLOCK CLOCK_GetFreq(kCLOCK_BusClk)

		/*******************************************************************************
		* Variables
		******************************************************************************/
		volatile bool ftmFirstChannelInterruptFlag = false;
		volatile bool ftmSecondChannelInterruptFlag = false;
		/* Record FTM TOF interrupt times */
		volatile uint32_t g_timerOverflowInterruptCount = 0u;
		volatile uint32_t g_firstChannelOverflowCount = 0u;
		volatile uint32_t g_secondChannelOverflowCount = 0u;
		bool ftm_running = true;

		/*******************************************************************************
		* Code
		******************************************************************************/
		extern "C"
		{
		void FTM3_CONTROLLER_STATUS_IRQHANDLER(void)
		{
		if ((FTM_GetStatusFlags(DEMO_FTM_BASEADDR) & kFTM_TimeOverflowFlag) == kFTM_TimeOverflowFlag)
		{
		/* Clear overflow interrupt flag.*/
		FTM_ClearStatusFlags(DEMO_FTM_BASEADDR, kFTM_TimeOverflowFlag);
		g_timerOverflowInterruptCount++;
		}
		else if (((FTM_GetStatusFlags(DEMO_FTM_BASEADDR) & FTM_FIRST_CHANNEL_FLAG) == FTM_FIRST_CHANNEL_FLAG) &&
		(ftmFirstChannelInterruptFlag == false))
		{
		/* Disable first channel interrupt.*/
		FTM_DisableInterrupts(DEMO_FTM_BASEADDR, FTM_FIRST_CHANNEL_INTERRUPT_ENABLE);
		g_firstChannelOverflowCount = g_timerOverflowInterruptCount;
		ftmFirstChannelInterruptFlag = true;
		}
		else if ((FTM_GetStatusFlags(DEMO_FTM_BASEADDR) & FTM_SECOND_CHANNEL_FLAG) == FTM_SECOND_CHANNEL_FLAG)
		{
		/* Clear second channel interrupt flag.*/
		FTM_ClearStatusFlags(DEMO_FTM_BASEADDR, FTM_SECOND_CHANNEL_FLAG);
		/* Disable second channel interrupt.*/
		FTM_DisableInterrupts(DEMO_FTM_BASEADDR, FTM_SECOND_CHANNEL_INTERRUPT_ENABLE);
		g_secondChannelOverflowCount = g_timerOverflowInterruptCount;
		ftmSecondChannelInterruptFlag = true;
		}
		else
		{
		}
		__DSB();
		}
		}

		controller_status_t get_controller_status()
		{
		ftm_config_t ftmInfo;
		ftm_dual_edge_capture_param_t edgeParam;
		uint32_t capture1Val;
		uint32_t capture2Val;
		float pulseWidth;

		const ftm_config_t FTM3_CONTROLLER_STATUS_config = {
		.prescale = kFTM_Prescale_Divide_1,
		.bdmMode = kFTM_BdmMode_0,
		.pwmSyncMode = kFTM_SoftwareTrigger,
		.reloadPoints = 0,
		.faultMode = kFTM_Fault_Disable,
		.faultFilterValue = 0,
		.deadTimePrescale = kFTM_Deadtime_Prescale_1,
		.deadTimeValue = 0,
		.extTriggers = 0,
		.chnlInitState = 0,
		.chnlPolarity = 0,
		.useGlobalTimeBase = false};

		const ftm_dual_edge_capture_param_t FTM3_CONTROLLER_STATUS_channel1 = {
		.mode = kFTM_Continuous,
		.currChanEdgeMode = kFTM_RisingEdge,
		.nextChanEdgeMode = kFTM_FallingEdge};

		FTM_Init(DEMO_FTM_BASEADDR, &FTM3_CONTROLLER_STATUS_config);
		FTM_SetTimerPeriod(DEMO_FTM_BASEADDR, FTM3_CONTROLLER_STATUS_TIMER_MODULO_VALUE);
		FTM_SetupDualEdgeCapture(DEMO_FTM_BASEADDR, kFTM_Chnl_1, &FTM3_CONTROLLER_STATUS_channel1, 0);
		FTM_EnableInterrupts(DEMO_FTM_BASEADDR, kFTM_Chnl2InterruptEnable \| kFTM_Chnl3InterruptEnable \| kFTM_TimeOverflowInterruptEnable);

		EnableIRQ(FTM_INTERRUPT_NUMBER);
		FTM_StartTimer(DEMO_FTM_BASEADDR, kFTM_SystemClock);

		while (ftmFirstChannelInterruptFlag != true)
		{
		}

		while (ftmSecondChannelInterruptFlag != true)
		{
		}

		/* Clear first channel interrupt flag after the second edge is detected.*/
		FTM_ClearStatusFlags(DEMO_FTM_BASEADDR, FTM_FIRST_CHANNEL_FLAG);

		/* Clear overflow interrupt flag.*/
		FTM_ClearStatusFlags(DEMO_FTM_BASEADDR, kFTM_TimeOverflowFlag);
		/* Disable overflow interrupt.*/
		FTM_DisableInterrupts(DEMO_FTM_BASEADDR, kFTM_TimeOverflowInterruptEnable);
		FTM_StopTimer(DEMO_FTM_BASEADDR);

		capture1Val = FTM_GetInputCaptureValue(DEMO_FTM_BASEADDR, (ftm_chnl_t)(BOARD_FTM_INPUT_CAPTURE_CHANNEL_PAIR * 2));
		capture2Val =
		FTM_GetInputCaptureValue(DEMO_FTM_BASEADDR, (ftm_chnl_t)(BOARD_FTM_INPUT_CAPTURE_CHANNEL_PAIR * 2 + 1));

		/* FTM clock source is not prescaled and is
		* divided by 1000000 as the output is printed in microseconds
		*/
		pulseWidth =
		(float)(((g_secondChannelOverflowCount - g_firstChannelOverflowCount) * 65536 + capture2Val - capture1Val) +
		1) /
		((float)FTM_SOURCE_CLOCK / 1000000);

		// reset all values for the next interrupt
		ftmFirstChannelInterruptFlag = false;
		ftmSecondChannelInterruptFlag = false;
		g_timerOverflowInterruptCount = 0u;
		g_firstChannelOverflowCount = 0u;
		g_secondChannelOverflowCount = 0u;

		FTM_Deinit(DEMO_FTM_BASEADDR);

		if (pulseWidth > 1700 && pulseWidth < 2000)
		return CONTROLLER_OFF;
		if (pulseWidth > 1000 && pulseWidth < 1200)
		return CONTROLLER_ON;
		return CONTROLLER_NAN;
		}

		void set_mode(int &min, int &max, int &curve, bool fast_enable)
		{
		void set_mode(int &min, int &max, int &curve, bool fast_enable) {
		min = fast_enable ? SPEED_MIN_FAST : SPEED_MIN_SLOW;
		max = fast_enable ? SPEED_MAX_FAST : SPEED_MAX_SLOW;
		curve = fast_enable ? SPEED_CURVE_FAST : SPEED_CURVE_SLOW;
		@@ -267,11 +112,8 @@ void set_mode(int &min, int &max, int &curve, bool fast_enable)
		GPIO_PinWrite(SHIELD_IO_LED_4_GPIO, SHIELD_IO_LED_4_PIN, fast_enable);
		}

		void norm_main(void)
		{
		start:
		for (int i = 0; i < 200; i++)
		{
		void norm_main(void) {
		start: for (int i = 0; i < 200; i++) {
		GPIO_PinWrite(SHIELD_GPIO_PTB18_GPIO, SHIELD_GPIO_PTB18_PIN, 1);

		__asm volatile("nop");
		@@ -287,8 +129,7 @@ start:
		set_servo(0);
		set_regulator(0);

		controller_status_t controller_status = get_controller_status();
		controller_status = get_controller_status();
		controller_status_t controller_status = CONTROLLER_NAN;

		int speed_min = 0;
		int speed_max = 0;
		@@ -296,8 +137,7 @@ start:

		int distance_counter = 0;

		while (!GPIO_PinRead(SHIELD_IO_SWITCH_1_GPIO, SHIELD_IO_SWITCH_1_PIN))
		{
		while (!GPIO_PinRead(SHIELD_IO_SWITCH_1_GPIO, SHIELD_IO_SWITCH_1_PIN)) {
		set_mode(speed_min, speed_max, speed_curve,
		GPIO_PinRead(SHIELD_IO_SWITCH_2_GPIO, SHIELD_IO_SWITCH_2_PIN));
		Delay::waitMs(50);
		@@ -314,7 +154,8 @@ start:

		set_led(LED_PERCENTAGE);
		uint32_t exposition =
		AUTO_EXPOSITION ? findDesiredExposition(frame_raw) : DEFAULT_EXPOSITION;
		AUTO_EXPOSITION ?
		findDesiredExposition(frame_raw) : DEFAULT_EXPOSITION;

		//print_camera(frame_raw, frame_filtered, frame_derived, exposition);
		if (PRINT_CAMERA) {
		@@ -328,10 +169,8 @@ start:
		set_regulator(speed_max * !STOP);
		Delay::waitMs(INITIALSTRAIGHT_TIME_SPEED * !INIT_FINISH);

		while (true)
		{
		if (!GPIO_PinRead(SHIELD_IO_SWITCH_1_GPIO, SHIELD_IO_SWITCH_1_PIN))
		{
		while (true) {
		if (!GPIO_PinRead(SHIELD_IO_SWITCH_1_GPIO, SHIELD_IO_SWITCH_1_PIN)) {
		goto start;
		}

		@@ -356,16 +195,12 @@ start:
		saw_box \|= saw_finish && distance_counter >= DISTANCE_COUNTER_LIMIT;
		*/
		// saw_box \|= distance_counter >= DISTANCE_COUNTER_LIMIT;

		// set_finish_leds(saw_finish);
		// set_box_leds(saw_box);
		exposition += (lighting < DESIRED_LIGHTING - DESIRED_LIGHTING_OFFSET) * EXPOSITION_INCREMENT;
		exposition -= (lighting > DESIRED_LIGHTING + DESIRED_LIGHTING_OFFSET) * EXPOSITION_INCREMENT;

		double p = saw_finish ? P_FINISH : P;
		double outcome = -(p * error);

		int speed = speed_min;
		exposition += (lighting < DESIRED_LIGHTING - DESIRED_LIGHTING_OFFSET)
		* EXPOSITION_INCREMENT;
		exposition -= (lighting > DESIRED_LIGHTING + DESIRED_LIGHTING_OFFSET)
		* EXPOSITION_INCREMENT;

		double p = saw_finish ? P_FINISH : P;

		@@ -374,28 +209,25 @@ start:

		double outcome = -(p * error);

		if (saw_box && !testing)
		{
		int speed = speed_min;

		bool testing = false;

		if (saw_box && !testing) {
		speed = IGNORE_BOX ? speed_min : -512;
		}
		else if (saw_finish && !testing)
		{
		} else if (saw_finish && !testing) {
		speed = speed_min;
		}
		else
		{
		//speed = 0;
		} else {
		// sme v zakrute, ak mame rychlost vacsiu nez pozadovana, tak brzdime, inak ideme rychlostou zakruty
		if (abs(error) > CURVE_LIMIT)
		{
		speed =
		get_speed() > SPEED_BRAKE_FAST_LIMIT ? BRAKE_FORCE : speed_curve;
		if (abs(error) > CURVE_LIMIT) {
		speed = get_speed() > SPEED_BRAKE_FAST_LIMIT ?
		BRAKE_FORCE : speed_curve;
		// speed = speed_curve;
		// set_finish_leds(false);
		// set_box_leds(true);
		// ak sme dostatocne dlho na rovinke, tak zrychlime ak nemame dostatocnu rychlost a potom ideme rychlosotu rovinky
		}
		else
		{
		} else {
		// speed = get_speed() < SPEED_LINE_FAST_MINIMUM && is_fast_mode ? SPEED_ACCELERATION : speed_max;
		speed = speed_max;

		@@ -407,81 +239,58 @@ start:
		}
		}

		#ifdef USE_CONTROLLER
		controller_status = get_controller_status();
		if (controller_status == CONTROLLER_ON)
		{
		if (ftm_running)
		{
		if (controller_status == CONTROLLER_HAS_CONTROL) {
		if (ftm_running) {
		// pokus o brzdenie pri prepnuti do manualneho rezimu - TODO nefunguje, regulator nereaguje
		// set_regulator(-128);
		// Delay::waitMs(200);
		FTM_StopTimer(FTM1);
		ftm_running = false;
		}
		}
		else
		{
		if (!ftm_running)
		{
		} else if (controller_status == CONTROLLER_OFF) {
		if (!ftm_running) {
		FTM_StartTimer(FTM1, kFTM_SystemClock);
		ftm_running = true;
		}
		}

		if (saw_box && !testing) {
		speed = IGNORE_BOX ? speed_min : -512;
		} else if (saw_finish && !testing) {
		speed = speed_min;
		//speed = 0;
		} else {
		// sme v zakrute, ak mame rychlost vacsiu nez pozadovana, tak brzdime, inak ideme rychlostou zakruty
		if (abs(error) > CURVE_LIMIT) {
		speed = get_speed() > SPEED_BRAKE_FAST_LIMIT ? BRAKE_FORCE : speed_curve;
		// speed = speed_curve;
		// set_finish_leds(false);
		// set_box_leds(true);
		// ak sme dostatocne dlho na rovinke, tak zrychlime ak nemame dostatocnu rychlost a potom ideme rychlosotu rovinky
		} else {
		// speed = get_speed() < SPEED_LINE_FAST_MINIMUM && is_fast_mode ? SPEED_ACCELERATION : speed_max;
		speed = speed_max;

		if (ftm_running)
		{
		if (!STOP)
		{
		if (ftm_running) {
		#endif
		if (!STOP) {
		set_regulator(speed);
		}
		set_servo(outcome);
		}
		#ifdef USE_CONTROLLER
		}
		#endif
		}

		void test_main(void)
		{
		void test_main(void) {
		int speed = 0;
		int angle = 0;

		while (1)
		{
		if (!GPIO_PinRead(SHIELD_IO_BUTTON_1_GPIO, SHIELD_IO_BUTTON_1_PIN))
		{
		while (1) {
		if (!GPIO_PinRead(SHIELD_IO_BUTTON_1_GPIO, SHIELD_IO_BUTTON_1_PIN)) {
		speed += 10;
		}

		if (!GPIO_PinRead(SHIELD_IO_BUTTON_2_GPIO, SHIELD_IO_BUTTON_2_PIN))
		{
		if (!GPIO_PinRead(SHIELD_IO_BUTTON_2_GPIO, SHIELD_IO_BUTTON_2_PIN)) {
		speed -= 10;
		}

		if (!GPIO_PinRead(SHIELD_IO_BUTTON_3_GPIO, SHIELD_IO_BUTTON_3_PIN))
		{
		if (!GPIO_PinRead(SHIELD_IO_BUTTON_3_GPIO, SHIELD_IO_BUTTON_3_PIN)) {
		angle += 10;
		}

		if (!GPIO_PinRead(SHIELD_IO_BUTTON_4_GPIO, SHIELD_IO_BUTTON_4_PIN))
		{
		if (!GPIO_PinRead(SHIELD_IO_BUTTON_4_GPIO, SHIELD_IO_BUTTON_4_PIN)) {
		angle -= 10;
		}

		speed =
		GPIO_PinRead(SHIELD_IO_SWITCH_1_GPIO, SHIELD_IO_SWITCH_1_PIN) ? speed : 0;
		speed = GPIO_PinRead(SHIELD_IO_SWITCH_1_GPIO, SHIELD_IO_SWITCH_1_PIN) ?
		speed : 0;

		speed = std::min(std::max(speed, 0), REGULATOR_MAX_LIMIT);
		angle = std::min(std::max(angle, SERVO_MIN_LIMIT), SERVO_MAX_LIMIT);
		@@ -491,15 +300,13 @@ void test_main(void)
		set_regulator(speed);
		set_servo(angle);

		for (int i = 0; i < 10000000; i++)
		{
		for (int i = 0; i < 10000000; i++) {
		__asm volatile("nop");
		}
		}
		}

		int main(void)
		{
		int main(void) {
		BOARD_InitBootPins();
		BOARD_InitBootClocks();
		BOARD_InitBootPeripherals();
		@@ -507,7 +314,8 @@ int main(void)
		BOARD_InitDebugConsole();
		#endif
		// disable write protection on FTM registers
		FTM1->MODE \|= FTM_MODE_PWMSYNC_MASK \| FTM_MODE_WPDIS_MASK \| FTM_MODE_FTMEN_MASK;
		FTM1->MODE \|= FTM_MODE_PWMSYNC_MASK \| FTM_MODE_WPDIS_MASK
		\| FTM_MODE_FTMEN_MASK;

		// test_main();
		norm_main();