Kom inn eftir uppfærlsu á VSc

.gitignore

@@ -1 +1,6 @@
 .pio
+venv
+*.py
+*.csv
+scripts
+*.typ

include/motor_driver.h

@@ -11,6 +11,8 @@ public:
     void forward();
     void reverse();
     void stop();
+
+    void set(double pwm);
 };
 
 #endif

include/p_controller.h

@@ -0,0 +1,15 @@
+#ifndef P_CONTROLLER_H
+#define P_CONTROLLER_H
+
+class P_controller
+{
+private:
+    double Kp;
+
+public:
+    P_controller(double Kp);
+
+    double update(double ref, double actual);
+};
+
+#endif

include/rotary_encoder_driver.h

@@ -8,11 +8,16 @@
 #include "timer_usec.h"
 
 class RotaryEncoderDriver {
+    static const uint8_t HIST_LEN = 4;   // 8 x 5 ms = 40 ms gluggi fyrir rpm-utreikning
+
     volatile uint8_t cycle;
     volatile int16_t count;
     volatile int16_t prev_count;
     volatile bool speed_ready;
 
+    volatile int16_t history[HIST_LEN];
+    volatile uint8_t hist_idx;
+
     Digital_out led;
 
     Digital_in eA;

platformio.ini

@@ -12,3 +12,5 @@
 platform = atmelavr
 board = nanoatmega328new
 framework = arduino
+
+monitor_speed = 115200

src/main.cpp

@@ -4,10 +4,21 @@
 
 #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);
@@ -15,64 +26,62 @@ int main() {
     cli();
     encoder.begin();
     motor.begin();
-
-
-
-    
     sei();
 
-
-
-    // Make sure motor is stopped before starting test
+    // 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,rpm");
-    Serial.println("0.0,0.0");
+    Serial.println("time_ms,ref_rpm,actual_rpm,pwm_requested,pwm_actual");
+    Serial.println("0.0,0.0,0.0,0.0,0.0");
 
-    // Synchronize the RPM measurement window with the step input
     encoder.reset_speed_measurement();
 
-    // Apply unit-step input
-    motor.forward();
-
     uint16_t sample = 0;
+    double ref = 0.0;
 
     while (true) {
-
-        if (encoder.new_speed_available())
-        {
+        if (encoder.new_speed_available()) {
             sample++;
 
-            float time_ms = sample * 5.0f;
-            float speed = encoder.rpm();
+            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.println(speed, 2);
+            Serial.print(ref, 1);
+            Serial.print(",");
+            Serial.print(actual, 2);
+            Serial.print(",");
+            Serial.print(pwm_requested, 1);
+            Serial.print(",");
+            Serial.println(pwm_actual, 1);
 
-
-            // Stop after about 8 seconds {2000 (<- input in sample >= xxxx) * 4 = 8 sec}
-            if (sample >= 400)
-            {
+            if (sample >= STOP_AT_SAMPLE) {
                 motor.stop();
-
-                while (true)
-                {
-
+                while (true) {
                 }
             }
         }
-        
-        //Serial.print("POSITION: ");
-        //Serial.print("RPM: ");
-        
-
-        //Serial.println(encoder.position());
-        //Serial.println(encoder.rpm());
-
-        //_delay_ms(50);
     }
     return 0;
 }

src/motor_driver.cpp

@@ -16,7 +16,27 @@ void MotorDriver::begin()
     DDRD |= (1 << DDD3);
     DDRB |= (1 << DDB3);
 
-    stop();
+    // 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();
 }
 
 
@@ -25,8 +45,11 @@ void MotorDriver::forward()
     // AIN1 / D3  = HIGH
     // AIN2 / D11 = LOW
 
-    PORTD |= (1 << PORTD3);
-    PORTB &= ~(1 << PORTB3);
+    //PORTD |= (1 << PORTD3);
+    //PORTB &= ~(1 << PORTB3);
+
+    OCR2A = 0;
+    OCR2B = 255;
 }
 
 
@@ -35,8 +58,11 @@ void MotorDriver::reverse()
     // AIN1 / D3  = LOW
     // AIN2 / D11 = HIGH
 
-    PORTD &= ~(1 << PORTD3);
-    PORTB |= (1 << PORTB3);
+    //PORTD &= ~(1 << PORTD3);
+    //PORTB |= (1 << PORTB3);
+
+    OCR2B = 0;
+    OCR2A = 255;
 }
 
 
@@ -44,6 +70,41 @@ void MotorDriver::stop()
 {
     // Both LOW
 
-    PORTD &= ~(1 << PORTD3);
-    PORTB &= ~(1 << PORTB3);
+    //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();
+    }
 }

src/p_controller.cpp

@@ -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;
+}

src/rotary_encoder_driver.cpp

@@ -8,9 +8,15 @@ RotaryEncoderDriver::RotaryEncoderDriver()
     , count(0)
     , prev_count(0)
     , speed_ready(false)
+    , hist_idx(0)
     , led(5)
     , eA(2)
-    , eB(5) {}
+    , eB(5)
+{
+    for (uint8_t i = 0; i < HIST_LEN; i++) {
+        history[i] = 0;
+    }
+}
 
 
 void RotaryEncoderDriver::begin() {
@@ -23,7 +29,7 @@ void RotaryEncoderDriver::begin() {
     EICRA |= (1<<ISC01) | (1<<ISC00);
     EIMSK |= (1<<INT0);
 
-    timer.init(5000);
+    timer.init(4000);
 }
 
 void RotaryEncoderDriver::update() {
@@ -48,7 +54,11 @@ int RotaryEncoderDriver::position() {
 
 void RotaryEncoderDriver::update_delta() {
 
-        delta = count - prev_count;
+    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;
 
@@ -67,7 +77,7 @@ float RotaryEncoderDriver::count_per_sec() {
 
     SREG = oldSREG;
 
-    return local_delta / 0.005f;
+    return local_delta / (HIST_LEN * 0.004f);
 }
 
 
@@ -108,6 +118,11 @@ void RotaryEncoderDriver::reset_speed_measurement()
     delta = 0;
     speed_ready = false;
 
+    for (uint8_t i = 0; i < HIST_LEN; i++) {
+        history[i] = count;
+    }
+    hist_idx = 0;
+
     SREG = oldSREG;
 }
 

src/temporary_main.cpp

@@ -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);
+    }
+}
+*/