stm32-robot-arm-controller/main.cpp at main · RegexCore/stm32-robot-arm-controller · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
/*********************************************************************
*                    SEGGER Microcontroller GmbH                     *
*                        The Embedded Experts                        *
**********************************************************************

File        : main.cpp
Purpose     : Generic application start

Project     : Robot Arm Controller
Description : Main application entry for robotic arm control firmware

* SPDX-License-Identifier: MIT
* Copyright (c) 2026 Manuel Wiesinger
*********************************************************************/

#include <stdio.h>
#include <stm32f4xx.h>
#include "hardware/i2c/hw_i2c.h"
#include "hardware/adc/hw_adc.h"
#include "hardware/gpio/hw_gpio.h"
#include "hardware/timer/systicktimer.h"
#include "hardware/timer/timer_interrupt.h"
#include "libraries/pca9685/pca9685.h"
#include "libraries/diagnostic/logger.hpp"
#include "libraries/joystick/joystick.hpp"
#include "libraries/servo/servo.hpp"
#include "libraries/kinematics/kinematics.hpp"
#include "controller/robot_controller.hpp"
#include "config/robot_config.hpp"

using namespace diagnostic;
using namespace robotarm;

volatile bool control_tick = true;

int main(void)
{
    printf("[Init] Steps:\n");
    SystemCoreClockUpdate();
    Logger::init();
    TIM7_Init_10ms();
    systick_init();
    HW_I2C_Init();
    HW_ADC_Init();
    HW_PC10_PC11_Init(PC_NO_PULL);
    HW_PC12_Init(PC_NO_PULL);
    HW_PA15_Init(PC_NO_PULL);
    PCA9685_Init(PCA9685_DEFAULT_ADDR, PCA9685_DEFAULT_FREQUENCY);

    uint32_t mhz   = SystemCoreClock / 1000000;
    uint32_t deci = (SystemCoreClock % 1000000) / 100000;

    Logger::printf("****** SYSTEM ******\n");
    Logger::printf("[Clock] System Core Clock: %d Hz (%d.%d MHz)\n",
         SystemCoreClock, mhz, deci);

    Logger::printf("[Init] Peripheral setup completed\n");
    Logger::printf("[Main] Program start\n");

    Joystick joystick;
    Kinematics kinematics(config::activeConfig.upperArmLength, config::activeConfig.lowerArmLength);  // Upper arm = 100, forearm = 100 (lengths in mm)
    ServoController servocontroller(joystick);
    RobotController robotcontroller(joystick, kinematics, servocontroller);

    robotcontroller.init();

    while (1)
    {
        if(control_tick)
        {
            control_tick = false;
            robotcontroller.periodicUpdate();
        }
    }
}

void TIM7_IRQHandler(void)
{
    if (TIM7->SR & TIM_SR_UIF)   // Update interrupt?
    {
        control_tick = true;
        TIM7->SR &= ~TIM_SR_UIF; // Clear flag
    }
}