project 2 pasted in to repo

2026-09-24 10:15:22 +00:00
parent fcc0f82dc6
commit 46c761dd25
24 changed files with 1038 additions and 0 deletions
@@ -0,0 +1,14 @@
+#include <avr/io.h>
+#include "digital_in.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(); }
+
+
@@ -0,0 +1,9 @@
+#include "digital_out.h"
+#include <avr/io.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; }
@@ -0,0 +1,87 @@
+#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;
+}
@@ -0,0 +1,110 @@
+#include "motor_driver.h"
+
+#include <avr/io.h>
+
+
+MotorDriver::MotorDriver()
+{
+}
+
+
+void MotorDriver::begin()
+{
+    // Arduino D3  = PD3 = AIN1
+    // Arduino D11 = PB3 = AIN2
+
+    DDRD |= (1 << DDD3);
+    DDRB |= (1 << DDB3);
+
+    // Timer2 Fast PWM
+    TCCR2A = 0;
+    TCCR2B = 0;
+
+    // Fast PWM mode
+    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();
+}
+
+
+void MotorDriver::forward()
+{
+    // AIN1 / D3  = HIGH
+    // AIN2 / D11 = LOW
+
+    //PORTD |= (1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 255;
+}
+
+
+void MotorDriver::reverse()
+{
+    // AIN1 / D3  = LOW
+    // AIN2 / D11 = HIGH
+
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB |= (1 << PORTB3);
+
+    OCR2B = 0;
+    OCR2A = 255;
+}
+
+
+void MotorDriver::stop()
+{
+    // Both LOW
+
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 0;
+}
+
+void MotorDriver::set(double pwm)
+{
+    // Limit PWM command
+    if (pwm > 255.0)
+    {
+        pwm = 255.0;
+    }
+
+    if (pwm < -255.0)
+    {
+        pwm = -255.0;
+    }
+
+
+    if (pwm > 0)
+    {
+        // Forward
+        OCR2A = 0;
+        OCR2B = (uint8_t)pwm;
+    }
+    else if (pwm < 0)
+    {
+        // Reverse
+        OCR2B = 0;
+        OCR2A = (uint8_t)(-pwm);
+    }
+    else
+    {
+        stop();
+    }
+}
@@ -0,0 +1,13 @@
+#include "p_controller.h"
+
+P_controller::P_controller(double Kp)
+    : Kp(Kp)
+{
+}
+
+double P_controller::update(double ref, double actual)
+{
+    double error = ref - actual;
+
+    return Kp * error;
+}
@@ -0,0 +1,136 @@
+#include "rotary_encoder_driver.h"
+#include <avr/interrupt.h>
+
+RotaryEncoderDriver* driver = nullptr;
+
+RotaryEncoderDriver::RotaryEncoderDriver()
+    : cycle(0)
+    , count(0)
+    , prev_count(0)
+    , speed_ready(false)
+    , hist_idx(0)
+    , led(5)
+    , eA(2)
+    , eB(5)
+{
+    for (uint8_t i = 0; i < HIST_LEN; i++) {
+        history[i] = 0;
+    }
+}
+
+
+void RotaryEncoderDriver::begin() {
+    driver = this;
+
+    led.init();
+    eA.init();
+    eB.init();
+
+    EICRA |= (1<<ISC01) | (1<<ISC00);
+    EIMSK |= (1<<INT0);
+
+    timer.init(4000);
+}
+
+void RotaryEncoderDriver::update() {
+    count += eB.is_hi() ? 1 : -1;
+    led.set_hi();
+}
+
+int RotaryEncoderDriver::position() {
+
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    int16_t local_count = count;
+
+    SREG = oldSREG;
+
+    led.set_lo();
+
+    return local_count;
+} 
+
+void RotaryEncoderDriver::update_delta() {
+
+    int16_t oldest = history[hist_idx];
+    history[hist_idx] = count;
+    hist_idx = (hist_idx + 1) % HIST_LEN;
+
+    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (40 ms) glugga
+
+    prev_count = count;
+
+    speed_ready = true;
+
+    led.set_lo();
+}
+
+float RotaryEncoderDriver::count_per_sec() {
+
+    uint8_t oldSREG = SREG;
+
+    cli();
+
+    int16_t local_delta = delta;
+
+    SREG = oldSREG;
+
+    return local_delta / (HIST_LEN * 0.004f);
+}
+
+
+float RotaryEncoderDriver::rpm()
+{
+    const float COUNTS_PER_REV = 1050.0f;
+
+    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();
+
+    cycle = 0;
+    prev_count = count;
+    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();    
+}
+
+
@@ -0,0 +1,61 @@
+/* #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();    
+}
+
+ */
@@ -0,0 +1,53 @@
+/* Ó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);
+    }
+}
+*/
@@ -0,0 +1,14 @@
+#include "timer_usec.h"
+
+Timer_usec::Timer_usec() = default;
+
+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);
+}