From c2d878047880e9e59a2af99ca6f2c76661f58a0c Mon Sep 17 00:00:00 2001 From: isakg22 Date: Thu, 8 Oct 2026 15:18:32 +0000 Subject: [PATCH] =?UTF-8?q?N=C3=BDjir=20k=C3=B3=C3=B0ar=20og=20uppf=C3=A6r?= =?UTF-8?q?=C3=B0ir=20gamlir=20k=C3=B3=C3=B0ar=20fyrir=20arduino=20yfir=20?= =?UTF-8?q?=C3=AD=20zephyr?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/digital_in.h | 16 ++- include/digital_out.h | 16 ++- include/initialization_state.h | 11 +- include/motor_controller.h | 29 ++-- include/motor_driver.h | 3 + include/pre_operational_state.h | 7 +- include/rotary_encoder_driver.h | 53 +++++-- include/stopped_state.h | 5 +- include/timer_usec.h | 26 +++- src/digital_in.cpp | 64 ++++++++- src/digital_out.cpp | 53 ++++++- src/gamla_m_ain.cpp | 125 +++++++++++++++++ src/initialization_state.cpp | 44 ++++-- src/main.c | 7 + src/main.cpp | 84 ----------- src/motor_controller.cpp | 26 ++-- src/motor_driver.cpp | 209 ++++++++++++++++++++-------- src/operational_state.cpp | 40 ++++-- src/pre_operational_state.cpp | 56 +++++--- src/rotary_encoder_driver.cpp | 173 ++++++++++++++--------- src/rotary_encoder_driver_gamli.cpp | 132 ++++++++++++++++++ src/stopped_state.cpp | 24 +++- src/timer_usec.cpp | 47 +++++-- zephyr/prj.conf | 10 ++ 24 files changed, 922 insertions(+), 338 deletions(-) create mode 100644 src/gamla_m_ain.cpp delete mode 100644 src/main.cpp create mode 100644 src/rotary_encoder_driver_gamli.cpp diff --git a/include/digital_in.h b/include/digital_in.h index 0f62f9c..9ac64f5 100644 --- a/include/digital_in.h +++ b/include/digital_in.h @@ -1,14 +1,22 @@ #ifndef DIGITAL_IN_H #define DIGITAL_IN_H + #include +#include struct Digital_in { - uint8_t pin_mask; + enum : uint8_t { + ENCODER_C1 = 0, + ENCODER_C2 = 1 + }; + + const struct gpio_dt_spec *gpio; + + explicit Digital_in(uint8_t pin_id); - Digital_in(uint8_t pin); - void init(); bool is_hi(); bool is_lo(); }; -#endif + +#endif \ No newline at end of file diff --git a/include/digital_out.h b/include/digital_out.h index 7ed8229..4b08482 100644 --- a/include/digital_out.h +++ b/include/digital_out.h @@ -1,15 +1,27 @@ #ifndef DIGITAL_OUT_H #define DIGITAL_OUT_H + #include +#include + struct Digital_out { - uint8_t pin_mask; + // uint8_t pin_mask; - Digital_out(uint8_t pin); + enum : uint8_t { + STATUS_LED = 0 + }; + + explicit Digital_out(uint8_t pin_id); void init(); void toggle(); void set_hi(); void set_lo(); + +private: + const struct gpio_dt_spec *gpio; + }; + #endif diff --git a/include/initialization_state.h b/include/initialization_state.h index 01cc28c..7e2352a 100644 --- a/include/initialization_state.h +++ b/include/initialization_state.h @@ -1,15 +1,22 @@ #ifndef INITIALIZATION_STATE_H #define INITIALIZATION_STATE_H +#include + #include "state.h" class InitializationState : public State { private: - static const unsigned long WAIT_MS = 2000; + // static const unsigned long WAIT_MS = 2000; + + // unsigned long enter_time_ms = 0; + + + static constexpr int64_t WAIT_MS = 2000; + int64_t enter_time_ms = 0; - unsigned long enter_time_ms = 0; public: void enter() override; diff --git a/include/motor_controller.h b/include/motor_controller.h index f81ec22..4923ec9 100644 --- a/include/motor_controller.h +++ b/include/motor_controller.h @@ -1,6 +1,8 @@ #ifndef MOTOR_CONTROLLER_H #define MOTOR_CONTROLLER_H +#include + #include "rotary_encoder_driver.h" #include "motor_driver.h" #include "p_controller.h" @@ -14,7 +16,8 @@ class MotorController private: static constexpr double DEFAULT_REFERENCE_RPM = 40.0; static constexpr double PWM_PER_PERCENT = 255.0 / 100.0; - static const uint8_t LED_PIN = 5; // PB5 = D13 = LED_BUILTIN + //static const uint8_t LED_PIN = 5; // PB5 = D13 = LED_BUILTIN + static constexpr uint8_t LED_PIN = Digital_out::STATUS_LED; State* current_state; @@ -23,9 +26,9 @@ private: State& operational_state; State& stopped_state; - RotaryEncoderDriver& encoder; - MotorDriver& motor; - P_controller* controller; + RotaryEncoderDriver &encoder; + MotorDriver &motor; + P_controller *controller; Digital_out led; @@ -38,13 +41,13 @@ private: public: MotorController( - RotaryEncoderDriver& encoder, - MotorDriver& motor, - P_controller& controller, - State& initialization, - State& pre_operational, - State& operational, - State& stopped + RotaryEncoderDriver &encoder, + MotorDriver &motor, + P_controller &controller, + State &initialization, + State &pre_operational, + State &operational, + State &stopped ); @@ -54,7 +57,7 @@ public: void command(char c); - void transition_to(State& new_state); + void transition_to(State &new_state); void to_initialization(); @@ -85,7 +88,7 @@ public: void set_Ti(double value); - void set_controller(P_controller& new_controller); + void set_controller(P_controller &new_controller); void led_on(); diff --git a/include/motor_driver.h b/include/motor_driver.h index 9b4e54b..15711b2 100644 --- a/include/motor_driver.h +++ b/include/motor_driver.h @@ -17,6 +17,9 @@ public: void set(double pwm); bool fault_active(); // Fault detection for part 2 + +private: + bool initialized; // Flag that ignores motor commands if setup fails }; #endif \ No newline at end of file diff --git a/include/pre_operational_state.h b/include/pre_operational_state.h index 7ad5cc6..3de0737 100644 --- a/include/pre_operational_state.h +++ b/include/pre_operational_state.h @@ -9,11 +9,12 @@ class P_controller; class PreOperationalState : public State { private: - P_controller& p_controller; - P_controller& pi_controller; + P_controller &p_controller; + P_controller &pi_controller; bool pi_selected; - unsigned long previous_time; + //unsigned long previous_time; + int64_t previous_time; bool led_state; //bool waiting_for_kp; diff --git a/include/rotary_encoder_driver.h b/include/rotary_encoder_driver.h index ef1ed44..ce1a5f3 100644 --- a/include/rotary_encoder_driver.h +++ b/include/rotary_encoder_driver.h @@ -2,8 +2,9 @@ #define ROTARY_ENCODER_DRIVER_H #include +#include -#include "digital_in.h" +//#include "digital_in.h" #include "timer_usec.h" class RotaryEncoderDriver { @@ -11,6 +12,19 @@ public: static constexpr uint16_t SAMPLE_PERIOD_US = 4000; // Ts = 4 ms static constexpr float COUNTS_PER_REV = 1050.0f; + RotaryEncoderDriver(); + + // Returns 0 on success, or a negative Zephyr error code. + int begin(); + + void update(); + int32_t position(); + void update_delta(); + float count_per_sec(); + float rpm(); + bool new_speed_available(); + void reset_speed_measurement(); + private: static const uint8_t HIST_LEN = 4; // 4 x Ts = 16 ms gluggi fyrir rpm-utreikning @@ -21,29 +35,42 @@ private: volatile int32_t history[HIST_LEN]; volatile uint8_t hist_idx; - Digital_in eA; - Digital_in eB; + + //Digital_in eA; + //Digital_in eB; + + struct gpio_callback encoder_c1_callback; Timer_usec timer; -public: - RotaryEncoderDriver(); + static void gpio_edge_handler( + const structdevice *dev, + struct gpio_callback *callback, + uint32_t pins); - void begin(); + static void timer_expired(void *context); + - void update(); +// public: +// RotaryEncoderDriver(); - int32_t position(); +// void begin(); - void update_delta(); +// void update(); - float count_per_sec(); +// int32_t position(); - float rpm(); +// void update_delta(); + +// float count_per_sec(); + +// float rpm(); + +// bool new_speed_available(); + +// void reset_speed_measurement(); - bool new_speed_available(); - void reset_speed_measurement(); }; #endif diff --git a/include/stopped_state.h b/include/stopped_state.h index b42c686..af081a0 100644 --- a/include/stopped_state.h +++ b/include/stopped_state.h @@ -1,12 +1,15 @@ #ifndef STOPPED_STATE_H #define STOPPED_STATE_H +#include + #include "state.h" class StoppedState : public State { private: - unsigned long previous_time; // Used for blinking LED + //unsigned long previous_time; // Used for blinking LED + int64_t previous_time; bool led_state; // Ditto public: diff --git a/include/timer_usec.h b/include/timer_usec.h index 660e9b0..ee3aebd 100644 --- a/include/timer_usec.h +++ b/include/timer_usec.h @@ -1,9 +1,31 @@ #ifndef TIMER_USEC_H #define TIMER_USEC_H -#include -struct Timer_usec { +//#include + +#include +#include + +// struct Timer_usec { +// Timer_usec(); +// void init(uint16_t period_us); +//}; + +class Timer_usec { +public: + using callback_t = void (*)(void *context); + Timer_usec(); + + void set_callback(callback_t callback, void *context); void init(uint16_t period_us); + +private: + struct k_timer timer; + callback_t callback; + void *callback_context; + + static void expiry_handler(struct k_timer *timer); }; + #endif diff --git a/src/digital_in.cpp b/src/digital_in.cpp index 5735eaf..e8e1b08 100644 --- a/src/digital_in.cpp +++ b/src/digital_in.cpp @@ -1,14 +1,64 @@ -#include +//#include #include "digital_in.h" -Digital_in::Digital_in(uint8_t pin) : pin_mask(1< -void Digital_in::init() { - DDRD &= ~pin_mask; - PORTD |= pin_mask; +// Digital_in::Digital_in(uint8_t pin) : pin_mask(1<port, gpio->pin) > 0; +} + +bool Digital_in::is_lo() +{ + return !is_hi(); +} diff --git a/src/digital_out.cpp b/src/digital_out.cpp index 2ce527c..c5989d6 100644 --- a/src/digital_out.cpp +++ b/src/digital_out.cpp @@ -1,9 +1,50 @@ #include "digital_out.h" -#include -Digital_out::Digital_out(uint8_t pin) : pin_mask(1< -void Digital_out::init() { DDRB |= pin_mask; } -void Digital_out::toggle() { PORTB ^= pin_mask; } -void Digital_out::set_hi() { PORTB |= pin_mask; } -void Digital_out::set_lo() { PORTB &= ~pin_mask; } +#include + +// Digital_out::Digital_out(uint8_t pin) : pin_mask(1< +//#include + +#include +#include +#include +#include +#include + +#include "rotary_encoder_driver.h" +#include "motor_driver.h" +#include "pi_controller.h" +#include "p_controller.h" +#include "motor_controller.h" + +#include "initialization_state.h" + +#include "pre_operational_state.h" + +#include "operational_state.h" +#include "stopped_state.h" + + +static RotaryEncoderDriver encoder; + +static MotorDriver motor; + +const double Kp_P = 10.0; // P-controller from Project 2 + +const double Kp = 10.125; // Ziegler-Nichols PI +const double Ti = 0.0334; +const double T = 0.004; + +static P_controller p_controller(Kp_P); +static PI_controller pi_controller(Kp, Ti, T); + + +static InitializationState initialization_state; +// Pre-operational velur á milli stýringanna með 'c' +static PreOperationalState pre_operational_state(p_controller, pi_controller); +static OperationalState operational_state; +static StoppedState stopped_state; + +// Stöðurnar verða að vera skilgreindar á undan motor_controller +static MotorController motor_controller( + encoder, + motor, + pi_controller, // Sjálfgefin stýring + initialization_state, + pre_operational_state, + operational_state, + stopped_state +); + + +// int main() +// { +// init(); +// Serial.begin(115200); +// cli(); + +// encoder.begin(); +// motor.begin(); + +// sei(); + +// motor_controller.begin(); + +// while (true) +// { +// if (Serial.available() > 0) +// { +// char c = Serial.read(); + +// motor_controller.command(c); + +// } + + +// motor_controller.update(); +// } + + +// return 0; +// } + +// #endif + +static const struct device *const console_dev = + DEVICE_DT_GET(DT_CHOSEN(zephyr_console)); + +extern "C" void motor_application_main(void) +{ + if (!device_is_ready(console_dev)) { + printk("Console UART is not ready\n"); + return; + } + + const int encoder_result = encoder.begin(); + if (encoder_result < 0) { + printk("Encoder setup failed: %d\n", encoder_result); + return; + } + + motor.begin(); + motor_controller.begin(); + + while (true) { + unsigned char received_char; + + // Read a command if a character is waiting. + if (uart_poll_in(console_dev, &received_char) == 0) { + motor_controller.command( + static_cast(received_char)); + } + + motor_controller.update(); + + // Give Zephyr's scheduler time to run other threads. + k_msleep(1); + } +} diff --git a/src/initialization_state.cpp b/src/initialization_state.cpp index 70d5d5f..5dd6cb2 100644 --- a/src/initialization_state.cpp +++ b/src/initialization_state.cpp @@ -1,41 +1,61 @@ #include "initialization_state.h" #include "motor_controller.h" -#include +//#include + +#include +#include void InitializationState::enter() { - Serial.println(); - Serial.println("State: Initialization"); + // Serial.println(); + // Serial.println("State: Initialization"); + + // context->led_off(); + + // context->stop_motor(); + + // context->reset_speed_measurement(); + + // enter_time_ms = millis(); + + printk("\nState: Initialization\n"); context->led_off(); - context->stop_motor(); - context->reset_speed_measurement(); - enter_time_ms = millis(); + enter_time_ms = k_uptime_get(); } void InitializationState::update() { - // Non-blocking bið svo aðallykkjan haldi áfram að keyra - if (millis() - enter_time_ms >= WAIT_MS) - { + // // Non-blocking bið svo aðallykkjan haldi áfram að keyra + // if (millis() - enter_time_ms >= WAIT_MS) + // { + // context->to_pre_operational(); + // // context->to_operational(); + // } + + + // Check the time without blocking the application. + if ((k_uptime_get() - enter_time_ms) >= WAIT_MS) { context->to_pre_operational(); - // context->to_operational(); - } + } + } void InitializationState::command(char c) { + (void)c; } void InitializationState::exit() { - Serial.println("Boot-up"); + //Serial.println("Boot-up"); + printk("Boot-up\n"); } diff --git a/src/main.c b/src/main.c index fd354fc..a54dbc0 100644 --- a/src/main.c +++ b/src/main.c @@ -1,5 +1,12 @@ #include +#ifndef PIO_UNIT_TESTING + +void gamla_m_ain(void); + void main(void) { + gamla_m_ain(); } + +#endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 4dc9246..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// main.cpp er sleppt i test-build (pio test) svo setup()/loop() rekist ekki a prof -#ifndef PIO_UNIT_TESTING - -#include -#include - -#include "rotary_encoder_driver.h" -#include "motor_driver.h" -#include "pi_controller.h" - -#include "motor_controller.h" - -#include "initialization_state.h" - -#include "pre_operational_state.h" - -#include "operational_state.h" -#include "stopped_state.h" - - -RotaryEncoderDriver encoder; - -MotorDriver motor; - -const double Kp_P = 10.0; // P-controller from Project 2 - -const double Kp = 10.125; // Ziegler-Nichols PI -const double Ti = 0.0334; -const double T = 0.004; - -P_controller p_controller(Kp_P); -PI_controller pi_controller(Kp, Ti, T); - - -InitializationState initialization_state; -// Pre-operational velur á milli stýringanna með 'c' -PreOperationalState pre_operational_state(p_controller, pi_controller); -OperationalState operational_state; -StoppedState stopped_state; - -// Stöðurnar verða að vera skilgreindar á undan motor_controller -MotorController motor_controller( - encoder, - motor, - pi_controller, // Sjálfgefin stýring - initialization_state, - pre_operational_state, - operational_state, - stopped_state -); - - -int main() -{ - init(); - Serial.begin(115200); - cli(); - - encoder.begin(); - motor.begin(); - - sei(); - - motor_controller.begin(); - - while (true) - { - if (Serial.available() > 0) - { - char c = Serial.read(); - - motor_controller.command(c); - - } - - - motor_controller.update(); - } - - - return 0; -} - -#endif diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp index 9d31ea1..7dc6452 100644 --- a/src/motor_controller.cpp +++ b/src/motor_controller.cpp @@ -1,17 +1,17 @@ #include "motor_controller.h" #include "state.h" -#include +//#include MotorController::MotorController( - RotaryEncoderDriver& encoder, - MotorDriver& motor, - P_controller& controller, - State& initialization, - State& pre_operational, - State& operational, - State& stopped + RotaryEncoderDriver &encoder, + MotorDriver &motor, + P_controller &controller, + State &initialization, + State &pre_operational, + State &operational, + State &stopped ) : current_state(nullptr), initialization_state(initialization), @@ -54,15 +54,13 @@ void MotorController::begin() } -void MotorController::transition_to(State& new_state) +void MotorController::transition_to(State &new_state) { - if (current_state != nullptr) - { + if (current_state != nullptr) { current_state->exit(); } current_state = &new_state; - current_state->enter(); } @@ -109,9 +107,9 @@ bool MotorController::run_speed_controller() return false; } - double actual = encoder.rpm(); + const double actual = encoder.rpm(); - double pwm_percent = + const double pwm_percent = controller->update(reference_rpm, actual); double pwm = diff --git a/src/motor_driver.cpp b/src/motor_driver.cpp index 6b841a7..5d3fd50 100644 --- a/src/motor_driver.cpp +++ b/src/motor_driver.cpp @@ -1,40 +1,57 @@ #include "motor_driver.h" -#include +//#include +#include +#include +#include // Í Fast PWM gefur OCR = 0 samt einn stuttan púls í hverri lotu. // Til að pinni sé alveg LOW er hann aftengdur frá timernum (COM-bitar) // og keyrður LOW gegnum PORT-registerið. -static void ain1_off() // D3 = PD3 = OC2B -{ - TCCR2A &= ~(1 << COM2B1); - PORTD &= ~(1 << PORTD3); - OCR2B = 0; -} +// static void ain1_off() // D3 = PD3 = OC2B +// { +// TCCR2A &= ~(1 << COM2B1); +// PORTD &= ~(1 << PORTD3); +// OCR2B = 0; +// } -static void ain2_off() // D11 = PB3 = OC2A -{ - TCCR2A &= ~(1 << COM2A1); - PORTB &= ~(1 << PORTB3); - OCR2A = 0; -} +// static void ain2_off() // D11 = PB3 = OC2A +// { +// TCCR2A &= ~(1 << COM2A1); +// PORTB &= ~(1 << PORTB3); +// OCR2A = 0; +// } -static void ain1_pwm(uint8_t duty) -{ - OCR2B = duty; - TCCR2A |= (1 << COM2B1); -} +// static void ain1_pwm(uint8_t duty) +// { +// OCR2B = duty; +// TCCR2A |= (1 << COM2B1); +// } + +// static void ain2_pwm(uint8_t duty) +// { +// OCR2A = duty; +// TCCR2A |= (1 << COM2A1); +// } + +static const struct pwm_dt_spec bin1_pwm = + PWM_DT_SPEC_GET(DT_ALIAS(motor_bin1_pwm)); + +static const struct gpio_dt_spec bin2 = + GPIO_DT_SPEC_GET(DT_ALIAS(motor_bin2), gpios); + +static const struct gpio_dt_spec motor_flt = + GPIO_DT_SPEC_GET(DT_ALIAS(motor_flt), gpios); + +static const struct gpio_dt_spec motor_slp = + GPIO_DT_SPEC_GET(DT_ALIAS(motor_slp), gpios); -static void ain2_pwm(uint8_t duty) -{ - OCR2A = duty; - TCCR2A |= (1 << COM2A1); -} MotorDriver::MotorDriver() + : initialized(false) { } @@ -45,27 +62,51 @@ void MotorDriver::begin() // Arduino D11 = PB3 = AIN2 // Arduino D4 = PD4 = FLT - DDRD |= (1 << DDD3); - DDRB |= (1 << DDB3); + // DDRD |= (1 << DDD3); + // DDRB |= (1 << DDB3); - DDRD &= ~(1 << DDD4); + // DDRD &= ~(1 << DDD4); - PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2 + // PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2 - // Timer2 Fast PWM - TCCR2A = 0; - TCCR2B = 0; + // // Timer2 Fast PWM + // TCCR2A = 0; + // TCCR2B = 0; - // Fast PWM mode - TCCR2A |= (1 << WGM20); - TCCR2A |= (1 << WGM21); + // // Fast PWM mode + // TCCR2A |= (1 << WGM20); + // TCCR2A |= (1 << WGM21); - // Timer2 prescaler = 32 - TCCR2B |= (1 << CS21); - TCCR2B |= (1 << CS20); + // // Timer2 prescaler = 32 + // TCCR2B |= (1 << CS21); + // TCCR2B |= (1 << CS20); + initialized = false; + + if (!pwm_is_ready_dt(&bin1_pwm) || + !gpio_is_ready_dt(&bin2) || + !gpio_is_ready_dt(&motor_flt) || + !gpio_is_ready_dt(&motor_slp)) { + return; + } + + if (gpio_pin_configure_dt(&bin2, GPIO_OUTPUT_INACTIVE) < 0 || + gpio_pin_configure_dt(&motor_flt, GPIO_INPUT) < 0 || + gpio_pin_configure_dt(&motor_slp, GPIO_OUTPUT_INACTIVE) < 0) { + return; + } + + // SLP is active high, so drive it high to enable the DRV8833. + if (gpio_pin_set_dt(&motor_slp, 1) < 0) { + return; + } + + if (pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0) < 0) { + return; + } // PWM-úttök eru tengd í set() eftir þörfum + initialized = true; stop(); } @@ -85,46 +126,96 @@ void MotorDriver::reverse() void MotorDriver::stop() { // Both LOW - ain1_off(); - ain2_off(); + // ain1_off(); + // ain2_off(); + + if (!initialized) { + return; + } + + // BIN1 low and BIN2 low puts the DRV8833 in coast mode. + pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0); + gpio_pin_set_dt(&bin2, 0); } void MotorDriver::set(double pwm) { + + if (!initialized) { + return; + } + // Limit PWM command if (pwm > 255.0) { pwm = 255.0; - } - - if (pwm < -255.0) - { + } else if (pwm < -255.0) { pwm = -255.0; } - - uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm); - - if (duty == 0) - { + if (pwm == 0.0) { stop(); + return; } - else if (pwm > 0) - { - // Forward - ain2_off(); - ain1_pwm(duty); - } - else - { - // Reverse - ain1_off(); - ain2_pwm(duty); + + + // uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm); + + // if (duty == 0) + // { + // stop(); + // } + // else if (pwm > 0) + // { + // // Forward + // ain2_off(); + // ain1_pwm(duty); + // } + // else + // { + // // Reverse + // ain1_off(); + // ain2_pwm(duty); + // } + + const uint32_t magnitude = + static_cast(pwm > 0.0 ? pwm : -pwm); + + // PWM pulse width in nanoseconds. The overlay sets a 50 us period. + const uint32_t requested_pulse = + (bin1_pwm.period * magnitude) / 255U; + + // First put BIN1 low so changing BIN2 does not switch direction + // while the previous PWM signal is still active. + pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0); + + uint32_t bin1_pulse; + + if (pwm > 0.0) { + // Forward: BIN2 low; BIN1 PWM duty equals the command magnitude. + gpio_pin_set_dt(&bin2, 0); + bin1_pulse = requested_pulse; + } else { + // Reverse: BIN2 high; BIN1 pulse is inverted for reverse PWM. + gpio_pin_set_dt(&bin2, 1); + bin1_pulse = bin1_pwm.period - requested_pulse; } + + pwm_set_dt(&bin1_pwm, bin1_pwm.period, bin1_pulse); + } bool MotorDriver::fault_active() { - return !(PIND & (1 << PIND4)); + //return !(PIND & (1 << PIND4)); + + if (!initialized) { + return true; // Fail safe if the driver did not initialize. + } + + // The overlay marks FLT as active low. gpio_pin_get_dt() returns + // the logical active state, so 1 means a fault is active. + const int fault = gpio_pin_get_dt(&motor_flt); + return fault < 0 || fault != 0; } diff --git a/src/operational_state.cpp b/src/operational_state.cpp index 59c6673..2190551 100644 --- a/src/operational_state.cpp +++ b/src/operational_state.cpp @@ -1,12 +1,15 @@ #include "operational_state.h" #include "motor_controller.h" -#include +//#include + +#include void OperationalState::enter() { - Serial.println("State: Operational"); + //Serial.println("State: Operational"); + printk("State: Operational\n"); context->reset_controller(); @@ -15,7 +18,8 @@ void OperationalState::enter() sample_count = 0; #if LOG_CSV - Serial.println("time_ms,ref_rpm,actual_rpm,pwm"); + //Serial.println("time_ms,ref_rpm,actual_rpm,pwm"); + printk("time_ms,ref_rpm,actual_rpm,pwm\n"); #endif } @@ -32,7 +36,8 @@ void OperationalState::update() { if (context->fault_active()) { - Serial.println("Fault detected"); + //Serial.println("Fault detected"); + printk("Fault detected\n"); context->to_stopped(); @@ -41,17 +46,22 @@ void OperationalState::update() if (context->run_speed_controller()) { - sample_count++; + ++sample_count; #if LOG_CSV // 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational - Serial.print(sample_count * 4UL); - Serial.print(','); - Serial.print(context->get_reference_rpm(), 1); - Serial.print(','); - Serial.print(context->get_actual_rpm(), 2); - Serial.print(','); - Serial.println(context->get_pwm(), 1); + // Serial.print(sample_count * 4UL); + // Serial.print(','); + // Serial.print(context->get_reference_rpm(), 1); + // Serial.print(','); + // Serial.print(context->get_actual_rpm(), 2); + // Serial.print(','); + // Serial.println(context->get_pwm(), 1); + printk("%u,%.1f,%.2f,%.1f\n", + static_cast(sample_count * 4U), + context->get_reference_rpm(), + context->get_actual_rpm(), + context->get_pwm()); #endif } } @@ -61,14 +71,16 @@ void OperationalState::command(char c) { if (c == 'r' || c == 'R') { - Serial.println("Command: reset"); + //Serial.println("Command: reset"); + printk("Command: reset\n"); context->to_initialization(); } else if (c == 'p' || c == 'P') { - Serial.println("Command: set pre-operational"); + //Serial.println("Command: set pre-operational"); + printk("Command: set pre-operational\n"); context->to_pre_operational(); } diff --git a/src/pre_operational_state.cpp b/src/pre_operational_state.cpp index 3bb4342..a1ed94d 100644 --- a/src/pre_operational_state.cpp +++ b/src/pre_operational_state.cpp @@ -1,10 +1,17 @@ #include "pre_operational_state.h" #include "motor_controller.h" -#include -#include +#include "p_controller.h" -PreOperationalState::PreOperationalState(P_controller& p_controller, P_controller& pi_controller) +//#include + +#include +#include +#include + + + +PreOperationalState::PreOperationalState(P_controller &p_controller, P_controller &pi_controller) : p_controller(p_controller), pi_controller(pi_controller), pi_selected(true), @@ -19,7 +26,8 @@ PreOperationalState::PreOperationalState(P_controller& p_controller, P_controlle void PreOperationalState::enter() { - Serial.println("State: Pre-operational"); + //Serial.println("State: Pre-operational"); + printk("State: Pre-operational\n"); context->stop_motor(); @@ -27,12 +35,14 @@ void PreOperationalState::enter() led_state = false; - previous_time = millis(); + //previous_time = millis(); + previous_time = k_uptime_get(); } void PreOperationalState::update() { - unsigned long current_time = millis(); + //unsigned long current_time = millis(); + const int64_t current_time = k_uptime_get(); if (current_time - previous_time >= 500) { @@ -65,21 +75,23 @@ void PreOperationalState::command(char c) input_buffer[input_index] = '\0'; // Convert text to a number - double value = atof(input_buffer); + const double value = atof(input_buffer); if (input_mode == ENTER_KP) { context->set_Kp(value); - Serial.print("Kp set to: "); - Serial.println(value); + // Serial.print("Kp set to: "); + // Serial.println(value); + printk("Kp updated\n"); } else if (input_mode == ENTER_TI) { context->set_Ti(value); - Serial.print("Ti set to: "); - Serial.println(value); + // Serial.print("Ti set to: "); + // Serial.println(value); + printk("Ti updated\n"); } } @@ -97,8 +109,8 @@ void PreOperationalState::command(char c) { if (input_index < sizeof(input_buffer) - 1) { - input_buffer[input_index] = c; - input_index++; + input_buffer[input_index++] = c; + //input_index++; } } @@ -109,14 +121,16 @@ void PreOperationalState::command(char c) // Normal state commands if (c == 'o' || c == 'O') { - Serial.println("Command: set operational"); + //Serial.println("Command: set operational"); + printk("Command: set operational\n"); context->to_operational(); } else if (c == 'r' || c == 'R') { - Serial.println("Command: reset"); + //Serial.println("Command: reset"); + printk("Command: reset\n"); context->to_initialization(); } @@ -126,7 +140,8 @@ void PreOperationalState::command(char c) input_mode = ENTER_KP; input_index = 0; - Serial.println("Enter Kp and press Enter:"); + //Serial.println("Enter Kp and press Enter:"); + printk("Enter Kp and press Enter:\n"); } else if (c == 't' || c == 'T') @@ -134,7 +149,8 @@ void PreOperationalState::command(char c) input_mode = ENTER_TI; input_index = 0; - Serial.println("Enter Ti and press Enter:"); + //Serial.println("Enter Ti and press Enter:"); + printk("Enter Ti and press Enter:\n"); } // Select controller algorithm, the controller is passed to the motor controller @@ -145,12 +161,14 @@ void PreOperationalState::command(char c) if (pi_selected) { context->set_controller(pi_controller); - Serial.println("Controller: PI"); + //Serial.println("Controller: PI"); + printk("Controller: PI\n"); } else { context->set_controller(p_controller); - Serial.println("Controller: P"); + //Serial.println("Controller: P"); + printk("Controller: P\n"); } } } diff --git a/src/rotary_encoder_driver.cpp b/src/rotary_encoder_driver.cpp index 2c75a4b..b7b75d0 100644 --- a/src/rotary_encoder_driver.cpp +++ b/src/rotary_encoder_driver.cpp @@ -1,84 +1,141 @@ #include "rotary_encoder_driver.h" -#include -static RotaryEncoderDriver* driver = nullptr; +#include +#include +#include + +static const struct gpio_dt_spec encoder_c1 = + GPIO_DT_SPEC_GET(DT_ALIAS(encoder_c1), gpios); + +static const struct gpio_dt_spec encoder_c2 = + GPIO_DT_SPEC_GET(DT_ALIAS(encoder_c2), gpios); RotaryEncoderDriver::RotaryEncoderDriver() - : count(0) - , delta(0) - , speed_ready(false) - , hist_idx(0) - , eA(2) - , eB(5) + : count(0), + delta(0), + speed_ready(false), + hist_idx(0) { - for (uint8_t i = 0; i < HIST_LEN; i++) - { + for (uint8_t i = 0; i < HIST_LEN; ++i) { history[i] = 0; } } - -void RotaryEncoderDriver::begin() +int RotaryEncoderDriver::begin() { - driver = this; + if (!gpio_is_ready_dt(&encoder_c1) || + !gpio_is_ready_dt(&encoder_c2)) { + return -ENODEV; + } - eA.init(); - eB.init(); + int ret = gpio_pin_configure_dt(&encoder_c1, GPIO_INPUT); + if (ret < 0) { + return ret; + } - EICRA |= (1<update(); + } +} + +void RotaryEncoderDriver::timer_expired(void *context) +{ + auto *encoder = static_cast(context); + encoder->update_delta(); } void RotaryEncoderDriver::update() { - count += eB.is_hi() ? 1 : -1; + // C2's level at the C1 edge tells us the direction. + const int direction = gpio_pin_get_dt(&encoder_c2); + if (direction < 0) { + return; + } + + const unsigned int key = irq_lock(); + count += direction ? 1 : -1; + irq_unlock(key); } int32_t RotaryEncoderDriver::position() { + const unsigned int key = irq_lock(); + const int32_t result = count; + irq_unlock(key); - uint8_t oldSREG = SREG; - - cli(); - - int32_t local_count = count; - - SREG = oldSREG; - - return local_count; + return result; } void RotaryEncoderDriver::update_delta() { + const unsigned int key = irq_lock(); - int32_t oldest = history[hist_idx]; - - history[hist_idx] = count; + const int32_t current_count = count; + const int32_t oldest_count = history[hist_idx]; + history[hist_idx] = current_count; hist_idx = (hist_idx + 1) % HIST_LEN; - delta = (int16_t)(count - oldest); // breyting yfir HIST_LEN * Ts (16 ms) glugga - + delta = static_cast(current_count - oldest_count); speed_ready = true; + + irq_unlock(key); } float RotaryEncoderDriver::count_per_sec() { + const unsigned int key = irq_lock(); + const int16_t local_delta = delta; + irq_unlock(key); - uint8_t oldSREG = SREG; + constexpr float window_seconds = + HIST_LEN * SAMPLE_PERIOD_US * 1e-6f; - cli(); - - int16_t local_delta = delta; - - SREG = oldSREG; - - return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f); + return local_delta / window_seconds; } - float RotaryEncoderDriver::rpm() { return count_per_sec() * 60.0f / COUNTS_PER_REV; @@ -86,47 +143,29 @@ float RotaryEncoderDriver::rpm() bool RotaryEncoderDriver::new_speed_available() { - uint8_t oldSREG = SREG; + const unsigned int key = irq_lock(); + const bool ready = speed_ready; - cli(); - - bool ready = speed_ready; - - if (ready) - { + if (ready) { speed_ready = false; } - SREG = oldSREG; - + irq_unlock(key); return ready; } - void RotaryEncoderDriver::reset_speed_measurement() { - uint8_t oldSREG = SREG; - - cli(); + const unsigned int key = irq_lock(); delta = 0; speed_ready = false; - for (uint8_t i = 0; i < HIST_LEN; i++) - { + for (uint8_t i = 0; i < HIST_LEN; ++i) { history[i] = count; } hist_idx = 0; - SREG = oldSREG; -} - -ISR(INT0_vect) -{ - driver->update(); -} -ISR(TIMER1_COMPA_vect) -{ - driver->update_delta(); -} + irq_unlock(key); +} \ No newline at end of file diff --git a/src/rotary_encoder_driver_gamli.cpp b/src/rotary_encoder_driver_gamli.cpp new file mode 100644 index 0000000..dc17219 --- /dev/null +++ b/src/rotary_encoder_driver_gamli.cpp @@ -0,0 +1,132 @@ +// #include "rotary_encoder_driver.h" +// #include + +// static RotaryEncoderDriver* driver = nullptr; + +// RotaryEncoderDriver::RotaryEncoderDriver() +// : count(0) +// , delta(0) +// , speed_ready(false) +// , hist_idx(0) +// , eA(2) +// , eB(5) +// { +// for (uint8_t i = 0; i < HIST_LEN; i++) +// { +// history[i] = 0; +// } +// } + + +// void RotaryEncoderDriver::begin() +// { +// driver = this; + +// eA.init(); +// eB.init(); + +// EICRA |= (1<update(); +// } +// ISR(TIMER1_COMPA_vect) +// { +// driver->update_delta(); +// } diff --git a/src/stopped_state.cpp b/src/stopped_state.cpp index 421d3bd..56a30e4 100644 --- a/src/stopped_state.cpp +++ b/src/stopped_state.cpp @@ -1,7 +1,10 @@ #include "stopped_state.h" #include "motor_controller.h" -#include +//#include + +#include +#include StoppedState::StoppedState() @@ -13,7 +16,9 @@ StoppedState::StoppedState() void StoppedState::enter() { - Serial.println("State: Stopped"); + //Serial.println("State: Stopped"); + + printk("State: Stopped\n"); context->stop_motor(); @@ -21,14 +26,16 @@ void StoppedState::enter() context->led_off(); - previous_time = millis(); + //previous_time = millis(); + previous_time = k_uptime_get(); } // Controlling the blinking of the LED @ 2Hz void StoppedState::update() { - unsigned long current_time = millis(); + //unsigned long current_time = millis(); + const int64_t current_time = k_uptime_get(); if (current_time - previous_time >= 250) { @@ -54,21 +61,24 @@ void StoppedState::command(char c) { if (c == 'r' || c == 'R') { - Serial.println("Command: reset"); + //Serial.println("Command: reset"); + printk("Command: reset\n"); context->to_initialization(); } else if (c == 'o' || c == 'O') { - Serial.println("Command: set operational"); + //Serial.println("Command: set operational"); + printk("Command: set operational\n"); context->to_operational(); } else if (c == 'p' || c == 'P') { - Serial.println("Command: set pre-operational"); + //Serial.println("Command: set pre-operational"); + printk("Command: set pre-operational\n"); context->to_pre_operational(); } diff --git a/src/timer_usec.cpp b/src/timer_usec.cpp index 762359f..cd63e59 100644 --- a/src/timer_usec.cpp +++ b/src/timer_usec.cpp @@ -1,14 +1,43 @@ #include "timer_usec.h" -Timer_usec::Timer_usec() = default; +Timer_usec::Timer_usec() //= default; + : callback(nullptr), + callback_context(nullptr) -void Timer_usec::init(uint16_t period_us) { // for custom duty cycle duration in ms in part 1 - TCCR1A = 0; - TCCR1B = 0; - TCNT1 = 0; +{ - OCR1A = period_us * 2 - 1; - TCCR1B |= (1<(k_timer_user_data_get(expired_timer)); + + if (self != nullptr && self->callback != nullptr) { + self->callback(self->callback_context); + } +} \ No newline at end of file diff --git a/zephyr/prj.conf b/zephyr/prj.conf index e69de29..d18c3e9 100644 --- a/zephyr/prj.conf +++ b/zephyr/prj.conf @@ -0,0 +1,10 @@ +CONFIG_CPP=y + +CONFIG_GPIO=y +CONFIG_PWM=y + +CONFIG_SERIAL=y +CONFIG_CONSOLE=y +CONFIG_UART_CONSOLE=y +CONFIG_PRINTK=y +CONFIG_CBPRINTF_FP_SUPPORT=y \ No newline at end of file