Encoder fyrir position í int32, stöðuvél án null-pointera, nefndir fastar, LED gegnum context

src/initialization_state.cpp

@@ -9,7 +9,7 @@ void InitializationState::enter()
     Serial.println();
     Serial.println("State: Initialization");
 
-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 
     context->stop_motor();
 

src/main.cpp

@@ -15,14 +15,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;
@@ -30,6 +25,16 @@ InitializationState initialization_state;
 OperationalState operational_state;
 
 
+// Stöðurnar verða að vera skilgreindar á undan motor_controller
+MotorController motor_controller(
+    encoder,
+    motor,
+    controller,
+    initialization_state,
+    operational_state
+);
+
+
 int main()
 {
     init();
@@ -37,11 +42,6 @@ int main()
     Serial.begin(115200);
 
 
-    pinMode(LED_BUILTIN, OUTPUT);
-
-    digitalWrite(LED_BUILTIN, LOW);
-
-
     cli();
 
     encoder.begin();
@@ -51,12 +51,6 @@ int main()
     sei();
 
 
-    motor_controller.set_states(
-        &initialization_state,
-        &operational_state
-    );
-
-
     motor_controller.begin();
 
 

src/motor_controller.cpp

@@ -7,45 +7,41 @@
 MotorController::MotorController(
     RotaryEncoderDriver& encoder,
     MotorDriver& motor,
-    P_controller& controller
+    P_controller& controller,
+    State& initialization,
+    State& operational
 )
     : current_state(nullptr),
-      initialization_state(nullptr),
-      operational_state(nullptr),
+      initialization_state(initialization),
+      operational_state(operational),
       encoder(encoder),
       motor(motor),
       controller(controller),
-      reference_rpm(40.0)
+      led(LED_PIN),
+      reference_rpm(DEFAULT_REFERENCE_RPM)
 {
-}
-
-
-void MotorController::set_states(
-    State* initialization,
-    State* operational
-)
-{
-    initialization_state = initialization;
-    operational_state = operational;
+    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();
 }
@@ -91,7 +87,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);
     }
@@ -107,4 +103,16 @@ void MotorController::stop_motor()
 void MotorController::reset_speed_measurement()
 {
     encoder.reset_speed_measurement();
-}
+}
+
+
+void MotorController::led_on()
+{
+    led.set_hi();
+}
+
+
+void MotorController::led_off()
+{
+    led.set_lo();
+}

src/operational_state.cpp

@@ -8,7 +8,7 @@ void OperationalState::enter()
 {
     Serial.println("State: Operational");
 
-    digitalWrite(LED_BUILTIN, HIGH);
+    context->led_on();
 }
 
 
@@ -33,5 +33,5 @@ void OperationalState::exit()
 {
     context->stop_motor();
 
-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 }

src/rotary_encoder_driver.cpp

@@ -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();
 }
-
-