helgie22/ESP_Project4_H26
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Heljarinnar breytingar til þess að laga nokkra hluti í part 1.

Browse Source
This commit is contained in:
helgie22
2026-10-15 12:15:50 +00:00
parent 782feb2d04
commit 6d384265eb
32 changed files with 188 additions and 696 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/gamla_m_ain.cpp → src/app.cpp
+36 -46
View File
@@ -1,8 +1,3 @@
// main.cpp er sleppt i test-build (pio test) svo setup()/loop() rekist ekki a prof
//#ifndef PIO_UNIT_TESTING
//#include <Arduino.h>
//#include <avr/interrupt.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
@@ -23,7 +18,6 @@
#include "operational_state.h"
#include "stopped_state.h"
static RotaryEncoderDriver encoder;
static MotorDriver motor;
@@ -56,42 +50,34 @@ static MotorController motor_controller(
);
// 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));
// Received characters are queued by the UART ISR, like the RX buffer that
// Arduino's Serial had. The USART has a single receive register, so
// polling it from the main loop loses characters that arrive in a burst.
K_MSGQ_DEFINE(rx_queue, sizeof(char), 64, 1);
static void uart_rx_isr(const struct device *dev, void *user_data)
{
(void)user_data;
if (!uart_irq_update(dev)) {
return;
}
while (uart_irq_rx_ready(dev)) {
unsigned char c;
if (uart_fifo_read(dev, &c, 1) != 1) {
break;
}
// Drop the character if the queue is full.
(void)k_msgq_put(&rx_queue, &c, K_NO_WAIT);
}
}
extern "C" void motor_application_main(void)
{
if (!device_is_ready(console_dev)) {
@@ -99,6 +85,9 @@ extern "C" void motor_application_main(void)
return;
}
uart_irq_callback_user_data_set(console_dev, uart_rx_isr, nullptr);
uart_irq_rx_enable(console_dev);
const int encoder_result = encoder.begin();
if (encoder_result < 0) {
printk("Encoder setup failed: %d\n", encoder_result);
@@ -109,17 +98,18 @@ extern "C" void motor_application_main(void)
motor_controller.begin();
while (true) {
unsigned char received_char;
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<char>(received_char));
// Handle every character the ISR has queued since the last pass.
while (k_msgq_get(&rx_queue, &received_char, K_NO_WAIT) == 0) {
motor_controller.command(received_char);
}
motor_controller.update();
// Give Zephyr's scheduler time to run other threads.
k_msleep(1);
// Sleep until the encoder has a new speed sample (every 4 ms), so
// the controller runs right after sampling. The timeout only
// matters if the timer stops; the thread is idle while it waits.
encoder.wait_for_sample(K_MSEC(10));
}
}
src/digital_in.cpp
+21 -11
View File
@@ -1,17 +1,8 @@
//#include <avr/io.h>
#include "digital_in.h"
#include <errno.h>
#include <zephyr/devicetree.h>
// Digital_in::Digital_in(uint8_t pin) : pin_mask(1<<pin) {}
// void Digital_in::init() {
// DDRD &= ~pin_mask;
// PORTD |= pin_mask;
// }
// bool Digital_in::is_hi() { return PIND & pin_mask; }
// bool Digital_in::is_lo() { return !is_hi(); }
#include <zephyr/sys/util.h>
static const struct gpio_dt_spec encoder_c1_gpio =
GPIO_DT_SPEC_GET(DT_ALIAS(encoder_c1), gpios);
@@ -62,3 +53,22 @@ bool Digital_in::is_lo()
return !is_hi();
}
int Digital_in::enable_rising_edge_interrupt(
struct gpio_callback *callback,
gpio_callback_handler_t handler)
{
if (gpio == nullptr || !gpio_is_ready_dt(gpio)) {
return -ENODEV;
}
gpio_init_callback(callback, handler, BIT(gpio->pin));
const int ret = gpio_add_callback(gpio->port, callback);
if (ret < 0) {
return ret;
}
// Physical rising edge, independent of the active level in the overlay.
return gpio_pin_interrupt_configure(
gpio->port, gpio->pin, GPIO_INT_EDGE_RISING);
}
src/digital_out.cpp
-9
View File
@@ -1,16 +1,7 @@
#include "digital_out.h"
//#include <avr/io.h>
#include <zephyr/devicetree.h>
// Digital_out::Digital_out(uint8_t pin) : pin_mask(1<<pin) {}
// 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; }
static const struct gpio_dt_spec status_led_gpio =
GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios);
src/initialization_state.cpp
+2 -26
View File
@@ -1,50 +1,27 @@
#include "initialization_state.h"
#include "motor_controller.h"
//#include <Arduino.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
void InitializationState::enter()
{
// 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 = k_uptime_get();
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)
// {
// 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();
}
}
}
@@ -56,6 +33,5 @@ void InitializationState::command(char c)
void InitializationState::exit()
{
//Serial.println("Boot-up");
printk("Boot-up\n");
}
src/main.c
+2 -5
View File
@@ -1,12 +1,9 @@
#include <zephyr/kernel.h>
#ifndef PIO_UNIT_TESTING
void motor_application_main(void);
void main(void)
int main(void)
{
motor_application_main();
return 0;
}
#endif
src/motor_controller.cpp
-15
View File
@@ -1,9 +1,6 @@
#include "motor_controller.h"
#include "state.h"
//#include <Arduino.h>
MotorController::MotorController(
RotaryEncoderDriver &encoder,
MotorDriver &motor,
@@ -33,18 +30,6 @@ MotorController::MotorController(
}
// void MotorController::set_states(
// State* initialization,
// State* operational,
// State* stopped
// )
// initialization_state = initialization;
// operational_state = operational;
// stopped_state = stopped;
void MotorController::begin()
{
led.init();
src/motor_driver.cpp
+34 -113
View File
@@ -1,55 +1,25 @@
#include "motor_driver.h"
//#include <avr/io.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/pwm.h>
// Í 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 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 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 pwm_dt_spec bin2_pwm =
PWM_DT_SPEC_GET(DT_ALIAS(motor_bin2_pwm));
static const struct gpio_dt_spec motor_flt =
GPIO_DT_SPEC_GET(DT_ALIAS(motor_flt), gpios);
static const struct gpio_dt_spec fault_test_button =
GPIO_DT_SPEC_GET(DT_ALIAS(fault_test_button), gpios);
static const struct gpio_dt_spec motor_slp =
GPIO_DT_SPEC_GET(DT_ALIAS(motor_slp), gpios);
MotorDriver::MotorDriver()
: initialized(false)
{
@@ -58,41 +28,18 @@ MotorDriver::MotorDriver()
void MotorDriver::begin()
{
// Arduino D3 = PD3 = AIN1
// Arduino D11 = PB3 = AIN2
// Arduino D4 = PD4 = FLT
// DDRD |= (1 << DDD3);
// DDRB |= (1 << DDB3);
// DDRD &= ~(1 << DDD4);
// PORTD |= (1 << PORTD4); // Enable pull-up resistor on D4 for part 2
// // Timer2 Fast PWM
// TCCR2A = 0;
// TCCR2B = 0;
// // Fast PWM mode
// TCCR2A |= (1 << WGM20);
// TCCR2A |= (1 << WGM21);
// // Timer2 prescaler = 32
// TCCR2B |= (1 << CS21);
// TCCR2B |= (1 << CS20);
initialized = false;
if (!pwm_is_ready_dt(&bin1_pwm) ||
!gpio_is_ready_dt(&bin2) ||
!pwm_is_ready_dt(&bin2_pwm) ||
!gpio_is_ready_dt(&motor_flt) ||
!gpio_is_ready_dt(&fault_test_button) ||
!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 ||
if (gpio_pin_configure_dt(&motor_flt, GPIO_INPUT) < 0 ||
gpio_pin_configure_dt(&fault_test_button, GPIO_INPUT) < 0 ||
gpio_pin_configure_dt(&motor_slp, GPIO_OUTPUT_INACTIVE) < 0) {
return;
}
@@ -102,10 +49,10 @@ void MotorDriver::begin()
return;
}
if (pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0) < 0) {
if (pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0) < 0 ||
pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0) < 0) {
return;
}
// PWM-úttök eru tengd í set() eftir þörfum
initialized = true;
stop();
}
@@ -125,22 +72,17 @@ void MotorDriver::reverse()
void MotorDriver::stop()
{
// Both LOW
// 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);
pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0);
}
void MotorDriver::set(double pwm)
{
if (!initialized) {
return;
}
@@ -159,63 +101,42 @@ void MotorDriver::set(double pwm)
}
// uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm);
const double magnitude = pwm > 0.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<uint32_t>(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;
// Pulse width in nanoseconds, kept as a double until the end so the
// 4000-count timer resolution is not lost to integer truncation.
const uint32_t pulse = static_cast<uint32_t>(
static_cast<double>(bin1_pwm.period) * magnitude / 255.0);
// Same PWM scheme as Project 3: one input PWM, the other input low,
// so both directions use fast decay (coast during the off time).
// The idle input is set low first; both channels share TIM1 with
// preloaded compare registers, so the change takes effect together
// at the next timer update.
if (pwm > 0.0) {
// Forward: BIN2 low; BIN1 PWM duty equals the command magnitude.
gpio_pin_set_dt(&bin2, 0);
bin1_pulse = requested_pulse;
// Forward: BIN1 PWM, BIN2 low.
pwm_set_dt(&bin2_pwm, bin2_pwm.period, 0);
pwm_set_dt(&bin1_pwm, bin1_pwm.period, 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;
// Reverse: BIN1 low, BIN2 PWM.
pwm_set_dt(&bin1_pwm, bin1_pwm.period, 0);
pwm_set_dt(&bin2_pwm, bin2_pwm.period, pulse);
}
pwm_set_dt(&bin1_pwm, bin1_pwm.period, bin1_pulse);
}
bool MotorDriver::fault_active()
{
//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;
const int driver_fault = gpio_pin_get_dt(&motor_flt);
const int button_fault = gpio_pin_get_dt(&fault_test_button);
return driver_fault < 0 ||
button_fault < 0 ||
driver_fault != 0 ||
button_fault != 0;
}
src/operational_state.cpp
+1 -24
View File
@@ -1,14 +1,10 @@
#include "operational_state.h"
#include "motor_controller.h"
//#include <Arduino.h>
#include <zephyr/sys/printk.h>
void OperationalState::enter()
{
//Serial.println("State: Operational");
printk("State: Operational\n");
context->reset_controller();
@@ -18,25 +14,15 @@ void OperationalState::enter()
sample_count = 0;
#if LOG_CSV
//Serial.println("time_ms,ref_rpm,actual_rpm,pwm");
printk("time_ms,ref_rpm,actual_rpm,pwm\n");
#endif
}
// Part 1
//void OperationalState::update()
//{
// context->run_speed_controller();
//}
// Part 2
void OperationalState::update()
{
if (context->fault_active())
{
//Serial.println("Fault detected");
printk("Fault detected\n");
context->to_stopped();
@@ -50,13 +36,6 @@ void OperationalState::update()
#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);
printk("%u,%.1f,%.2f,%.1f\n",
static_cast<unsigned int>(sample_count * 4U),
context->get_reference_rpm(),
@@ -71,7 +50,6 @@ void OperationalState::command(char c)
{
if (c == 'r' || c == 'R')
{
//Serial.println("Command: reset");
printk("Command: reset\n");
context->to_initialization();
@@ -79,7 +57,6 @@ void OperationalState::command(char c)
else if (c == 'p' || c == 'P')
{
//Serial.println("Command: set pre-operational");
printk("Command: set pre-operational\n");
context->to_pre_operational();
@@ -92,4 +69,4 @@ void OperationalState::exit()
context->stop_motor();
context->led_off();
}
}
src/p_controller.cpp
+1 -1
View File
@@ -10,4 +10,4 @@ double P_controller::update(double ref, double actual)
double error = ref - actual;
return Kp * error;
}
}
src/pi_controller.cpp
-1
View File
@@ -23,7 +23,6 @@ double PI_controller::update(double ref, double actual)
integral = new_integral;
}
return u;
// return 0.0;
}
void PI_controller::reset()
src/pre_operational_state.cpp
+6 -23
View File
@@ -3,22 +3,16 @@
#include "p_controller.h"
//#include <Arduino.h>
#include <stdlib.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
PreOperationalState::PreOperationalState(P_controller &p_controller, P_controller &pi_controller)
: p_controller(p_controller),
pi_controller(pi_controller),
pi_selected(true),
previous_time(0),
led_state(false),
//waiting_for_kp(false),
//waiting_for_ti(false),
input_mode(NONE),
input_index(0)
{
@@ -26,22 +20,22 @@ PreOperationalState::PreOperationalState(P_controller &p_controller, P_controlle
void PreOperationalState::enter()
{
//Serial.println("State: Pre-operational");
printk("State: Pre-operational\n");
// The selection survives a reset, so report it (used by capture_serial.py).
printk("Controller: %s\n", pi_selected ? "PI" : "P");
context->stop_motor();
context->led_off();
led_state = false;
//previous_time = millis();
previous_time = k_uptime_get();
}
void PreOperationalState::update()
{
//unsigned long current_time = millis();
const int64_t current_time = k_uptime_get();
if (current_time - previous_time >= 500)
@@ -81,17 +75,13 @@ void PreOperationalState::command(char c)
{
context->set_Kp(value);
// Serial.print("Kp set to: ");
// Serial.println(value);
printk("Kp updated\n");
printk("Kp set to: %.4f\n", value);
}
else if (input_mode == ENTER_TI)
{
context->set_Ti(value);
// Serial.print("Ti set to: ");
// Serial.println(value);
printk("Ti updated\n");
printk("Ti set to: %.4f\n", value);
}
}
@@ -110,7 +100,6 @@ void PreOperationalState::command(char c)
if (input_index < sizeof(input_buffer) - 1)
{
input_buffer[input_index++] = c;
//input_index++;
}
}
@@ -121,7 +110,6 @@ void PreOperationalState::command(char c)
// Normal state commands
if (c == 'o' || c == 'O')
{
//Serial.println("Command: set operational");
printk("Command: set operational\n");
context->to_operational();
@@ -129,7 +117,6 @@ void PreOperationalState::command(char c)
else if (c == 'r' || c == 'R')
{
//Serial.println("Command: reset");
printk("Command: reset\n");
context->to_initialization();
@@ -140,7 +127,6 @@ void PreOperationalState::command(char c)
input_mode = ENTER_KP;
input_index = 0;
//Serial.println("Enter Kp and press Enter:");
printk("Enter Kp and press Enter:\n");
}
@@ -149,7 +135,6 @@ void PreOperationalState::command(char c)
input_mode = ENTER_TI;
input_index = 0;
//Serial.println("Enter Ti and press Enter:");
printk("Enter Ti and press Enter:\n");
}
@@ -161,13 +146,11 @@ void PreOperationalState::command(char c)
if (pi_selected)
{
context->set_controller(pi_controller);
//Serial.println("Controller: PI");
printk("Controller: PI\n");
}
else
{
context->set_controller(p_controller);
//Serial.println("Controller: P");
printk("Controller: P\n");
}
}
@@ -176,4 +159,4 @@ void PreOperationalState::command(char c)
void PreOperationalState::exit()
{
context->led_off();
}
}
src/rotary_encoder_driver.cpp
+25 -43
View File
@@ -4,17 +4,13 @@
#include <zephyr/kernel.h>
#include <zephyr/sys/util.h>
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)
hist_idx(0),
eA(Digital_in::ENCODER_C1),
eB(Digital_in::ENCODER_C2)
{
for (uint8_t i = 0; i < HIST_LEN; ++i) {
history[i] = 0;
@@ -23,35 +19,16 @@ RotaryEncoderDriver::RotaryEncoderDriver()
int RotaryEncoderDriver::begin()
{
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;
}
// Max count 1: the main loop only needs to know that a new sample
// exists, not how many it missed.
k_sem_init(&sample_sem, 0, 1);
ret = gpio_pin_configure_dt(&encoder_c2, GPIO_INPUT);
if (ret < 0) {
return ret;
}
gpio_init_callback(
const int ret = eA.enable_rising_edge_interrupt(
&encoder_c1_callback,
gpio_edge_handler,
BIT(encoder_c1.pin));
ret = gpio_add_callback(encoder_c1.port, &encoder_c1_callback);
if (ret < 0) {
return ret;
}
// C1 is active high, so “to active” means a rising edge.
ret = gpio_pin_interrupt_configure_dt(
&encoder_c1,
GPIO_INT_EDGE_TO_ACTIVE);
gpio_edge_handler);
if (ret < 0) {
return ret;
}
@@ -68,16 +45,16 @@ void RotaryEncoderDriver::gpio_edge_handler(
struct gpio_callback *callback,
uint32_t pins)
{
(void)dev; // This handler does not need the device pointer.
// Zephyr only calls this handler for C1, the one pin in the callback.
(void)dev;
(void)pins;
auto *encoder = CONTAINER_OF(
callback,
RotaryEncoderDriver,
encoder_c1_callback);
if ((pins & BIT(encoder_c1.pin)) != 0U) {
encoder->update();
}
encoder->update();
}
void RotaryEncoderDriver::timer_expired(void *context)
@@ -89,13 +66,10 @@ void RotaryEncoderDriver::timer_expired(void *context)
void RotaryEncoderDriver::update()
{
// 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 bool forward = eB.is_hi();
const unsigned int key = irq_lock();
count += direction ? 1 : -1;
count += forward ? 1 : -1;
irq_unlock(key);
}
@@ -122,6 +96,14 @@ void RotaryEncoderDriver::update_delta()
speed_ready = true;
irq_unlock(key);
// Wake the main loop so the controller runs right after the sample.
k_sem_give(&sample_sem);
}
void RotaryEncoderDriver::wait_for_sample(k_timeout_t timeout)
{
(void)k_sem_take(&sample_sem, timeout);
}
float RotaryEncoderDriver::count_per_sec()
@@ -168,4 +150,4 @@ void RotaryEncoderDriver::reset_speed_measurement()
hist_idx = 0;
irq_unlock(key);
}
}
src/rotary_encoder_driver_gamli.cpp
-132
View File
@@ -1,132 +0,0 @@
// #include "rotary_encoder_driver.h"
// #include <avr/interrupt.h>
// 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<<ISC01) | (1<<ISC00);
// EIMSK |= (1<<INT0);
// timer.init(SAMPLE_PERIOD_US);
// }
// void RotaryEncoderDriver::update()
// {
// count += eB.is_hi() ? 1 : -1;
// }
// int32_t RotaryEncoderDriver::position()
// {
// uint8_t oldSREG = SREG;
// cli();
// int32_t local_count = count;
// SREG = oldSREG;
// return local_count;
// }
// void RotaryEncoderDriver::update_delta()
// {
// int32_t oldest = history[hist_idx];
// history[hist_idx] = count;
// hist_idx = (hist_idx + 1) % HIST_LEN;
// delta = (int16_t)(count - oldest); // breyting yfir HIST_LEN * Ts (16 ms) glugga
// speed_ready = true;
// }
// float RotaryEncoderDriver::count_per_sec()
// {
// uint8_t oldSREG = SREG;
// cli();
// int16_t local_delta = delta;
// SREG = oldSREG;
// return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
// }
// float RotaryEncoderDriver::rpm()
// {
// return count_per_sec() * 60.0f / COUNTS_PER_REV;
// }
// bool RotaryEncoderDriver::new_speed_available()
// {
// uint8_t oldSREG = SREG;
// cli();
// bool ready = speed_ready;
// if (ready)
// {
// speed_ready = false;
// }
// SREG = oldSREG;
// return ready;
// }
// void RotaryEncoderDriver::reset_speed_measurement()
// {
// uint8_t oldSREG = SREG;
// cli();
// delta = 0;
// speed_ready = false;
// 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();
// }
src/stopped_state.cpp
+1 -11
View File
@@ -1,12 +1,9 @@
#include "stopped_state.h"
#include "motor_controller.h"
//#include <Arduino.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
StoppedState::StoppedState()
: previous_time(0),
led_state(false)
@@ -16,8 +13,6 @@ StoppedState::StoppedState()
void StoppedState::enter()
{
//Serial.println("State: Stopped");
printk("State: Stopped\n");
context->stop_motor();
@@ -26,7 +21,6 @@ void StoppedState::enter()
context->led_off();
//previous_time = millis();
previous_time = k_uptime_get();
}
@@ -34,7 +28,6 @@ void StoppedState::enter()
void StoppedState::update()
{
//unsigned long current_time = millis();
const int64_t current_time = k_uptime_get();
if (current_time - previous_time >= 250)
@@ -61,7 +54,6 @@ void StoppedState::command(char c)
{
if (c == 'r' || c == 'R')
{
//Serial.println("Command: reset");
printk("Command: reset\n");
context->to_initialization();
@@ -69,7 +61,6 @@ void StoppedState::command(char c)
else if (c == 'o' || c == 'O')
{
//Serial.println("Command: set operational");
printk("Command: set operational\n");
context->to_operational();
@@ -77,7 +68,6 @@ void StoppedState::command(char c)
else if (c == 'p' || c == 'P')
{
//Serial.println("Command: set pre-operational");
printk("Command: set pre-operational\n");
context->to_pre_operational();
@@ -88,4 +78,4 @@ void StoppedState::command(char c)
void StoppedState::exit()
{
context->led_off();
}
}
src/timer_usec.cpp
+2 -15
View File
@@ -1,11 +1,9 @@
#include "timer_usec.h"
Timer_usec::Timer_usec() //= default;
Timer_usec::Timer_usec()
: callback(nullptr),
callback_context(nullptr)
{
}
void Timer_usec::set_callback(
@@ -17,17 +15,6 @@ void Timer_usec::set_callback(
}
// 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<<WGM12);
// TIMSK1 |= (1<<OCIE1A);
// TCCR1B |= (1<<CS11);
// }
void Timer_usec::init(uint16_t period_us)
{
if (period_us == 0) {
@@ -49,4 +36,4 @@ void Timer_usec::expiry_handler(struct k_timer *expired_timer)
if (self != nullptr && self->callback != nullptr) {
self->callback(self->callback_context);
}
}
}