unsigned long loop()

in BalancingRobot/Software/RTOS/rtos_app/rtos_app.c [212:408]

• 153 lines of code
• 41 McCabe index (conditional complexity)

unsigned long loop(void)
{
	unsigned long startPeriod = millis();


	// TODO: Read IMU, calculate Yaw, Pitch, Roll
	// ICM-20948 oritentation gives Roll as lean angle.

	float _Roll = 0.0;	// 
	float g_heading = 0.0; //
	if (g_heading < 0)
		g_heading += 360;

	loopCounter++;		// counter to ensure we have 200 items in the rollHistory array.


	// roll history is used to determine how stable the IMU readings are before unlocking the motors/telemetry
	rollHistory[rollHistoryPointer] = _Roll;
	rollHistoryPointer = (rollHistoryPointer + 1) % 200;

	float min = 360.0;
	float max = 0;
	int rollHistoryCount = 0;
	for (int x = 0; x < 200; x++)
	{
		if (rollHistory[x] != -1)
		{
			rollHistoryCount++;
			if (rollHistory[x] < min)
				min = rollHistory[x];
			if (rollHistory[x] > max)
				max = rollHistory[x];
		}
	}

	// determine whether the IMU is stable.
	if (!imuStable && rollHistoryCount == 200 && fabs(max - min) < 0.02)
	{
		imuStable = true;
	}

	// if the IMU is not stable, or the system is updating then return (motor control code after this point)
	if (!imuStable || updating)
	{
		return 0;
	}

	// see if we're close to pointing north...
	if (TurnRobotFlag)
	{
		// get a value from 0 to 15 (0 == north)
		int OrientPos=GetCompassDirection(g_heading);
		int TurnPos = GetCompassDirection(turnHeading);
		if (OrientPos == TurnPos)
		{
			details.id = MSG_TURN_DETAILS;
			details.startHeading = turnStartHeading;
			details.endHeading = g_heading;
			EnqueueIntercoreMessage(&details, sizeof(details));
			TurnRobotFlag = false;
		}
	}

	// set the PID input
	input = _Roll;

	float delta = fabs(_Roll - CalibratedSetpoint);

	static unsigned long calibrationCounter = 0;

	float setpointMotorAdjust = 0.0;

	calibrationCounter++;
	if (calibrationCounter == 10)
	{
		calibrationCounter = 0;
		if (!AutoCalibrateSetpoint && !ObstacleDetected && _Roll > 80)
		{
			setpointMotorAdjust = PIDController_Update(&pid, 0, output/100.0);
			SpeedSetpointAdjust = setpointMotorAdjust * -1;
		}
	}

	setpoint = CalibratedSetpoint + ToFSetpointAdjust + SpeedSetpointAdjust + RemoteFwdBackAdjust;

	Compute();

	if (!TurnRobotFlag && !ObstacleDetected && _Roll > 80 && RemoteFwdBackAdjust == 0 && RemoteRotateCommand == 0)
	{
		if (output > 0)
		{
			CalibratedSetpoint += 0.005;
		}
		if (output < 0)
		{
			CalibratedSetpoint -= 0.005;
		}
	}


#ifdef SHOW_LOG
		printf("Calibrated Setpoint %3.2f | Setpoint %3.2f (Tweak %3.2f) [AutoCalib %c | calibStarted %c | Tick %d] | Delta %3.2f | Output %3.2f\n", CalibratedSetpoint, setpoint, setpointTweak, AutoCalibrateSetpoint == true ? 'Y' : 'N', autoCalibrateStarted == true ? 'Y' : 'N', AutoCalibrationTickCounter, delta, output);
#endif

	if (output == 0 || delta > CUT_OFF_ANGLE)
	{
		Tof_Active = false;
		// turn off 'GPIO6 LED'
		mtk_os_hal_pwm_config_freq_duty_normal(OS_HAL_PWM_GROUP1, PWM_CHANNEL2, PWM_PERIOD, 0);
		ToF_Settle_Counter = 0;

		// turn off motors.
		mtk_os_hal_gpio_set_output(OS_HAL_GPIO_0, 0);
		mtk_os_hal_gpio_set_output(OS_HAL_GPIO_1, 0);
		mtk_os_hal_gpio_set_output(OS_HAL_GPIO_2, 0);
		mtk_os_hal_gpio_set_output(OS_HAL_GPIO_12, 0);
		mtk_os_hal_gpio_set_output(OS_HAL_GPIO_13, 0);

		mtk_os_hal_pwm_config_freq_duty_normal(OS_HAL_PWM_GROUP1, PWM_CHANNEL0, PWM_PERIOD, 0);
		mtk_os_hal_pwm_config_freq_duty_normal(OS_HAL_PWM_GROUP1, PWM_CHANNEL1, PWM_PERIOD, 0);
	}
	else
	{
		if (Tof_Active == false)
		{
			ToF_Settle_Counter++;
			if (ToF_Settle_Counter == TOF_STABILIZE_PERIOD)
			{
				ToF_Settle_Counter = 0;
				Tof_Active = true;
				// turn on 'GPIO6 LED'
				// mtk_os_hal_pwm_config_freq_duty_normal(OS_HAL_PWM_GROUP1, PWM_CHANNEL2, PWM_PERIOD, 1000);
			}
		}

		unsigned long duty = abs(output)*PWM_MULTIPLIER;
		unsigned long dutyLeft = duty;
		unsigned long dutyRight = duty;

		static bool Tick = false;

		// add turning value.
		if ((TurnRobotFlag || RemoteRotateCommand != 0) && Tick && !ObstacleDetected)
		{
			if (TurnRobotFlag || RemoteRotateCommand)
			{
				if (output > 0)
				{
					if (!RotateClockwise)
					{
						dutyLeft = (duty + SPIN_OFFSET) % 1000;
					}
					else
					{
						dutyRight = (duty + SPIN_OFFSET) % 1000;
					}
				}
				if (output < 0)
				{
					if (!RotateClockwise)
					{
						dutyRight = (duty + SPIN_OFFSET) % 1000;
					}
					else
					{
						dutyLeft = (duty + SPIN_OFFSET) % 1000;
					}
				}
			}
		}

		if (output > 0)
		{
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_0, 0);
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_1, 1);
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_2, 1);
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_12, 0);
		}
		else
		{
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_0, 1);
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_1, 0);
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_2, 0);
			mtk_os_hal_gpio_set_output(OS_HAL_GPIO_12, 1);
		}

		Tick = !Tick;

		mtk_os_hal_gpio_set_output(OS_HAL_GPIO_13, 1);
		mtk_os_hal_pwm_config_freq_duty_normal(OS_HAL_PWM_GROUP1, PWM_CHANNEL0, PWM_PERIOD, dutyLeft);
		mtk_os_hal_pwm_config_freq_duty_normal(OS_HAL_PWM_GROUP1, PWM_CHANNEL1, PWM_PERIOD, dutyRight);
	}

	unsigned long timePeriod = millis() - startPeriod;

	return timePeriod;
}