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

Merge branch 'main' of ssh://gitea.cs.ru.is:2222/helgie22/ESP_Project3_H26 into Isak_part2

Browse Source
This commit is contained in:
isakg22
2026-09-30 12:21:01 +00:00
parent 484686f8d1 1e02205344
commit fa30d35401
19 changed files with 256 additions and 627 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/initialization_state.cpp
+9 -4
View File
@@ -9,19 +9,23 @@ void InitializationState::enter()
Serial.println();
Serial.println("State: Initialization");
digitalWrite(LED_BUILTIN, LOW);
context->led_off();
context->stop_motor();
context->reset_speed_measurement();
Serial.println("Boot-up");
enter_time_ms = millis();
}
void InitializationState::update()
{
context->to_operational();
// Non-blocking bið svo aðallykkjan haldi áfram að keyra
if (millis() - enter_time_ms >= WAIT_MS)
{
context->to_operational();
}
}
@@ -32,4 +36,5 @@ void InitializationState::command(char c)
void InitializationState::exit()
{
}
Serial.println("Boot-up");
}
src/main.cpp
+17 -13
View File
@@ -16,14 +16,9 @@ RotaryEncoderDriver encoder;
MotorDriver motor;
P_controller controller(12.5);
const double KP = 12.5;
MotorController motor_controller(
encoder,
motor,
controller
);
P_controller controller(KP);
InitializationState initialization_state;
@@ -32,6 +27,16 @@ OperationalState operational_state;
StoppedState stopped_state;
// Stöðurnar verða að vera skilgreindar á undan motor_controller
MotorController motor_controller(
encoder,
motor,
controller,
initialization_state,
operational_state
);
int main()
{
init();
@@ -39,11 +44,6 @@ int main()
Serial.begin(115200);
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, LOW);
cli();
encoder.begin();
@@ -81,4 +81,8 @@ int main()
return 0;
}
}
// int main(){
// return 0;
// }
src/main_P2.cpp
-88
View File
@@ -1,88 +0,0 @@
/* #include <avr/io.h>
#include <util/delay.h>
#include <Arduino.h>
#include "rotary_encoder_driver.h"
#include "motor_driver.h"
#include "p_controller.h"
RotaryEncoderDriver encoder;
MotorDriver motor;
P_controller controller(12.5);
const double REF_RPM1 = 30.0;
const double REF_RPM2 = 50.0;
const uint16_t STEP1_AT_SAMPLE = 50;
const uint16_t STEP2_AT_SAMPLE = 500;
const uint16_t STOP_AT_SAMPLE = 1000;
int main() {
init();
Serial.begin(115200);
cli();
encoder.begin();
motor.begin();
sei();
// Mótor stopp og smá bið svo kerfið sé í kyrrstöðu áður en mælingin hefst
motor.stop();
_delay_ms(2000);
Serial.println("time_ms,ref_rpm,actual_rpm,pwm_requested,pwm_actual");
Serial.println("0.0,0.0,0.0,0.0,0.0");
encoder.reset_speed_measurement();
uint16_t sample = 0;
double ref = 0.0;
while (true) {
if (encoder.new_speed_available()) {
sample++;
float time_ms = sample * 4.0f;
// Step-inntak: viðmiðið hoppar úr 0 í REF_RPM á STEP_AT_SAMPLE
if (sample == STEP1_AT_SAMPLE) {
ref = REF_RPM1;
}
if (sample == STEP2_AT_SAMPLE) {
ref = REF_RPM2;
}
double actual = encoder.rpm();
double pwm_percent = controller.update(ref, actual);
double pwm_requested = pwm_percent * 2.55;
double pwm_actual = pwm_requested;
if (pwm_actual > 255.0) pwm_actual = 255.0;
if (pwm_actual < -255.0) pwm_actual = -255.0;
motor.set(pwm_actual);
Serial.print(time_ms, 1);
Serial.print(",");
Serial.print(ref, 1);
Serial.print(",");
Serial.print(actual, 2);
Serial.print(",");
Serial.print(pwm_requested, 1);
Serial.print(",");
Serial.println(pwm_actual, 1);
if (sample >= STOP_AT_SAMPLE) {
motor.stop();
while (true) {
}
}
}
}
return 0;
}
*/
src/motor_controller.cpp
+25 -8
View File
@@ -7,7 +7,9 @@
MotorController::MotorController(
RotaryEncoderDriver& encoder,
MotorDriver& motor,
P_controller& controller
P_controller& controller,
State& initialization,
State& operational
)
: current_state(nullptr),
initialization_state(nullptr),
@@ -16,7 +18,8 @@ MotorController::MotorController(
encoder(encoder),
motor(motor),
controller(controller),
reference_rpm(30.0)
led(LED_PIN),
reference_rpm(DEFAULT_REFERENCE_RPM)
{
}
@@ -30,25 +33,28 @@ void MotorController::set_states(
initialization_state = initialization;
operational_state = operational;
stopped_state = stopped;
//initialization_state.set_context(this);
//operational_state.set_context(this);
}
void MotorController::begin()
{
led.init();
led.set_lo();
transition_to(initialization_state);
}
void MotorController::transition_to(State* new_state)
void MotorController::transition_to(State& new_state)
{
if (current_state != nullptr)
{
current_state->exit();
}
current_state = new_state;
current_state->set_context(this);
current_state = &new_state;
current_state->enter();
}
@@ -94,7 +100,7 @@ void MotorController::run_speed_controller()
controller.update(reference_rpm, actual);
double pwm =
pwm_percent * 2.55;
pwm_percent * PWM_PER_PERCENT;
motor.set(pwm);
}
@@ -120,4 +126,15 @@ void MotorController::to_stopped()
bool MotorController::fault_active()
{
return motor.fault_active();
}
}
//void MotorController::led_on()
//{
// led.set_hi();
//}
//void MotorController::led_off()
//{
// led.set_lo();
//}
src/motor_driver.cpp
+50 -42
View File
@@ -3,6 +3,37 @@
#include <avr/io.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);
}
MotorDriver::MotorDriver()
{
}
@@ -30,57 +61,32 @@ void MotorDriver::begin()
TCCR2A |= (1 << WGM20);
TCCR2A |= (1 << WGM21);
// Non-inverting PWM on OC2A and OC2B
TCCR2A |= (1 << COM2A1);
TCCR2A |= (1 << COM2B1);
// Timer2 prescaler = 32
TCCR2B |= (1 << CS21);
TCCR2B |= (1 << CS20);
OCR2A = 0;
OCR2B = 0;
//stop();
// PWM-úttök eru tengd í set() eftir þörfum
stop();
}
void MotorDriver::forward()
{
// AIN1 / D3 = HIGH
// AIN2 / D11 = LOW
//PORTD |= (1 << PORTD3);
//PORTB &= ~(1 << PORTB3);
OCR2A = 0;
OCR2B = 255;
set(255.0);
}
void MotorDriver::reverse()
{
// AIN1 / D3 = LOW
// AIN2 / D11 = HIGH
//PORTD &= ~(1 << PORTD3);
//PORTB |= (1 << PORTB3);
OCR2B = 0;
OCR2A = 255;
set(-255.0);
}
void MotorDriver::stop()
{
// Both LOW
//PORTD &= ~(1 << PORTD3);
//PORTB &= ~(1 << PORTB3);
OCR2A = 0;
OCR2B = 0;
ain1_off();
ain2_off();
}
void MotorDriver::set(double pwm)
@@ -97,26 +103,28 @@ void MotorDriver::set(double pwm)
}
if (pwm > 0)
uint8_t duty = (uint8_t)(pwm > 0 ? pwm : -pwm);
if (duty == 0)
{
stop();
}
else if (pwm > 0)
{
// Forward
OCR2A = 0;
OCR2B = (uint8_t)pwm;
}
else if (pwm < 0)
{
// Reverse
OCR2B = 0;
OCR2A = (uint8_t)(-pwm);
ain2_off();
ain1_pwm(duty);
}
else
{
stop();
// Reverse
ain1_off();
ain2_pwm(duty);
}
}
bool MotorDriver::fault_active()
{
return !(PIND & (1 << PIND4));
return !(PIND & (1 << PIND4));p
}
src/operational_state.cpp
+2 -2
View File
@@ -8,7 +8,7 @@ void OperationalState::enter()
{
Serial.println("State: Operational");
digitalWrite(LED_BUILTIN, HIGH);
context->led_on();
}
// Part 1
@@ -50,5 +50,5 @@ void OperationalState::exit()
{
context->stop_motor();
digitalWrite(LED_BUILTIN, LOW);
context->led_off();
}
src/rotary_encoder_driver.cpp
+19 -35
View File
@@ -1,80 +1,70 @@
#include "rotary_encoder_driver.h"
#include <avr/interrupt.h>
RotaryEncoderDriver* driver = nullptr;
static RotaryEncoderDriver* driver = nullptr;
RotaryEncoderDriver::RotaryEncoderDriver()
: cycle(0)
, count(0)
, prev_count(0)
: count(0)
, delta(0)
, speed_ready(false)
, hist_idx(0)
, eA(2)
, eB(5)
, delta(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()
void RotaryEncoderDriver::begin()
{
driver = this;
//led.init();
eA.init();
eB.init();
EICRA |= (1<<ISC01) | (1<<ISC00);
EIMSK |= (1<<INT0);
timer.init(4000);
timer.init(SAMPLE_PERIOD_US);
}
void RotaryEncoderDriver::update()
void RotaryEncoderDriver::update()
{
count += eB.is_hi() ? 1 : -1;
//led.set_hi();
}
int RotaryEncoderDriver::position()
int32_t RotaryEncoderDriver::position()
{
uint8_t oldSREG = SREG;
cli();
int16_t local_count = count;
int32_t local_count = count;
SREG = oldSREG;
//led.set_lo();
return local_count;
}
}
void RotaryEncoderDriver::update_delta()
void RotaryEncoderDriver::update_delta()
{
int16_t oldest = history[hist_idx];
int32_t oldest = history[hist_idx];
history[hist_idx] = count;
hist_idx = (hist_idx + 1) % HIST_LEN;
delta = count - oldest; // breyting yfir HIST_LEN * Ts (16 ms) glugga
prev_count = count;
delta = (int16_t)(count - oldest); // breyting yfir HIST_LEN * Ts (16 ms) glugga
speed_ready = true;
//led.set_lo();
}
float RotaryEncoderDriver::count_per_sec()
float RotaryEncoderDriver::count_per_sec()
{
uint8_t oldSREG = SREG;
@@ -85,14 +75,12 @@ float RotaryEncoderDriver::count_per_sec()
SREG = oldSREG;
return local_delta / (HIST_LEN * 0.004f);
return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
}
float RotaryEncoderDriver::rpm()
{
const float COUNTS_PER_REV = 1050.0f;
return count_per_sec() * 60.0f / COUNTS_PER_REV;
}
@@ -121,12 +109,10 @@ void RotaryEncoderDriver::reset_speed_measurement()
cli();
cycle = 0;
prev_count = count;
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;
}
@@ -136,13 +122,11 @@ void RotaryEncoderDriver::reset_speed_measurement()
SREG = oldSREG;
}
ISR(INT0_vect)
ISR(INT0_vect)
{
driver->update();
}
ISR(TIMER1_COMPA_vect)
ISR(TIMER1_COMPA_vect)
{
driver->update_delta();
driver->update_delta();
}
src/rotary_encoder_driver_unused.cpp
-61
View File
@@ -1,61 +0,0 @@
/* #include "rotary_encoder_driver.h"
#include <avr/interrupt.h>
RotaryEncoderDriver* driver = nullptr;
RotaryEncoderDriver::RotaryEncoderDriver()
: cycle(0)
, count(0)
, prev_count(0)
, led(5)
, eA(2)
, eB(5) {}
void RotaryEncoderDriver::begin() {
driver = this;
led.init();
eA.init();
eB.init();
EICRA |= (1<<ISC01) | (1<<ISC00);
EIMSK |= (1<<INT0);
timer.init(100);
}
void RotaryEncoderDriver::update() {
count += eB.is_hi() ? 1 : -1;
led.set_hi();
}
int RotaryEncoderDriver::position() {
led.set_lo();
return count;
}
void RotaryEncoderDriver::update_delta() {
if (++cycle == 0) {
delta = count - prev_count;
prev_count = count;
}
led.set_lo();
}
float RotaryEncoderDriver::count_per_sec() {
return delta / (256.0f * 0.0001f);
}
float RotaryEncoderDriver::rpm() {
return (count_per_sec() * 60) / 1050.0;
}
ISR(INT0_vect) {
driver->update();
}
ISR(TIMER1_COMPA_vect) {
driver->update_delta();
}
*/
src/temporary_main.cpp
-53
View File
@@ -1,53 +0,0 @@
/* Óvirkt núna, "alvöru" main() er í main.cpp. Þessi skrá var notuð í undirbúning/prófun
#include <Arduino.h>
#include "p_controller.h"
P_controller controller(2.0);
int main()
{
init();
Serial.begin(115200);
double result = controller.update(100.0, 70.0);
Serial.print("PWM = ");
Serial.println(result);
while (true)
{
}
return 0;
}
#include <Arduino.h>
#include <util/delay.h>
#include "motor_driver.h"
MotorDriver motor;
int main()
{
init();
motor.begin();
while (true)
{
motor.set(60);
_delay_ms(3000);
motor.set(120);
_delay_ms(3000);
motor.set(240);
_delay_ms(3000);
}
}
*/