HandCrankMPPT-Firmware-Rust/src/switch_control.rs

use crate::logger::Logger;
use heapless::String;

const PWR_AVG_EXPONENT: i64 = 12;

#[derive(Copy, Clone)]
pub enum ControlMode {
    MPPT,
    ConstantVoltage,
    ImpedanceControl
}

struct PID<const GAINSCALE: i32, const T0: i32>
{
    pgain: i32, // proportional gain
    igain: i32, // integral gain
    dgain: i32, // differential gain

    iaccu: i32, // accumulator for the integral part
    elast: i32, // last seen deviation value

    target: i32,

    iaccu_min: i32, // minimum allowed value in iaccu
    iaccu_max: i32, // maximum allowed value in iaccu

    dbg_p: i32,
    dbg_i: i32,
    dbg_d: i32,
}

impl<const GAINSCALE: i32, const T0: i32> PID<GAINSCALE, T0>
{
    pub fn new(pgain: i32, igain: i32, dgain: i32, target: i32, start_iaccu: i32, iaccu_min: i32,
               iaccu_max: i32) -> Self
    {
        PID::<GAINSCALE, T0> {
            pgain,
            igain,
            dgain,
            iaccu: start_iaccu,
            elast: 0,
            target,
            iaccu_min,
            iaccu_max,
            dbg_p: 0,
            dbg_i: 0,
            dbg_d: 0
        }
    }

    pub fn restart_from_output_value(&mut self, oval: i32)
    {
        self.iaccu = oval * GAINSCALE;
        self.elast = 0;
    }

    pub fn update(&mut self, meas: i32) -> i32
    {
        let err = self.target - meas;

        self.iaccu += self.igain * T0 * err;

        // limit iaccu
        if self.iaccu > self.iaccu_max {
            self.iaccu = self.iaccu_max;
        } else if self.iaccu < self.iaccu_min {
            self.iaccu = self.iaccu_min;
        }

        let pval = self.pgain * err / GAINSCALE;
        let ival = self.iaccu / GAINSCALE;
        let dval = self.dgain * (err - self.elast) / GAINSCALE / T0;

        self.elast = err;

        self.dbg_p = pval;
        self.dbg_i = ival;
        self.dbg_d = dval;

        pval + ival + dval
    }

    pub fn set_target(&mut self, new_target: i32)
    {
        self.target = new_target;
    }

    pub fn log_status(&self, logger: &mut dyn Logger)
    {
        let mut data: String<16>;

        logger.log(b"Last P: ").unwrap();
        data = String::from(self.dbg_p);
        logger.log(data.as_bytes()).unwrap();
        logger.log(b" I: ").unwrap();
        data = String::from(self.dbg_i);
        logger.log(data.as_bytes()).unwrap();
        logger.log(b" D: ").unwrap();
        data = String::from(self.dbg_d);
        logger.log(data.as_bytes()).unwrap();
    }
}

struct MPPT<const INTERVAL: u32, const PERCENT_LOSS_TOLERANCE: i32>
{
    update_delay: u32,

    pwm: i32,
    increment: i32,

    avg_power: i64, // averaged power value
    last_power: i32,

    peak_power: i32, // highest power seen so far. Decreases over time.

    min_pwm: i32,
    max_pwm: i32,
}

impl<const INTERVAL: u32, const PERCENT_LOSS_TOLERANCE: i32> MPPT<INTERVAL, PERCENT_LOSS_TOLERANCE>
{
    pub fn new(start_pwm: i32, min_pwm: i32, max_pwm: i32) -> Self
    {
        MPPT::<INTERVAL, PERCENT_LOSS_TOLERANCE> {
            update_delay: INTERVAL - 1,
            pwm: start_pwm,
            increment: 1,
            avg_power: 0,
            peak_power: 0,
            last_power: 0,
            min_pwm,
            max_pwm
        }
    }

    pub fn restart(&mut self, start_pwm: i32, start_increment: i32)
    {
        self.peak_power = 1;
        self.pwm = start_pwm;
        self.increment = start_increment;
    }

    pub fn update(&mut self, vout: i32, iout: i32) -> i32
    {
        let cur_power = vout * iout;

        self.avg_power = self.avg_power - (self.avg_power >> PWR_AVG_EXPONENT) + cur_power as i64;

        let avg_power_uw = (self.avg_power >> PWR_AVG_EXPONENT) as i32;

        if self.update_delay == 0 {
            self.update_delay = INTERVAL - 1;

            // decrease peak power over time
            self.peak_power = (990 * (self.peak_power as i64) / 1000) as i32;

            /*
            if avg_power_uw > self.peak_power {
                self.peak_power = avg_power_uw;
            } else if self.peak_power <= 0 {
                self.peak_power = 1;
            }

            let peak_power_loss_percent = 100 - (100 * avg_power_uw / self.peak_power);

            // invert search direction if measured power is significantly lower than previous measurement
            if peak_power_loss_percent > PERCENT_LOSS_TOLERANCE {
            */

            if avg_power_uw < self.last_power {
                //self.peak_power = cur_power; // prevent immediate turnaround
                self.increment = -self.increment;
            }

            self.last_power = avg_power_uw;

            // enforce the pwm limits
            if self.pwm == self.max_pwm {
                self.increment = -1;
            } else if self.pwm == self.min_pwm {
                self.increment = 1;
            }

            // step to the next test PWM value
            self.pwm += self.increment;
        } else {
            self.update_delay -= 1;
        }

        self.pwm
    }

    pub fn log_status(&self, logger: &mut dyn Logger)
    {
        let mut data: String<16>;

        match self.increment {
            1 => logger.log(b"\xE2\x86\x91").unwrap(), // unicode for ↑
            -1 => logger.log(b"\xE2\x86\x93").unwrap(), // unicode for ↓
            _ => {
                logger.log(b"inc: ").unwrap();

                data = String::from(self.increment);
                logger.log(data.as_bytes()).unwrap();
            }
        }

        logger.log(b" Ppk: ").unwrap();

        data = String::from(self.peak_power);
        logger.log(data.as_bytes()).unwrap();

        logger.log(b" Pavg: ").unwrap();

        data = String::from(self.avg_power >> PWR_AVG_EXPONENT);
        logger.log(data.as_bytes()).unwrap();
    }
}

struct ImpedanceControl<const T0: i32, const MOTOR_SPEC_U: i32, const MOTOR_SPEC_I: i32>
{
    pid: PID::<10000, T0>,
    iin_target: i32,
}

impl<const T0: i32, const MOTOR_SPEC_U: i32, const MOTOR_SPEC_I: i32> ImpedanceControl<T0, MOTOR_SPEC_U, MOTOR_SPEC_I>
{
    pub fn new(i_target_initial: i32, max_pwm: i32) -> Self
    {
        ImpedanceControl::<T0, MOTOR_SPEC_U, MOTOR_SPEC_I> {
            pid: PID::<10000, T0>::new(10, 5, 2, i_target_initial, 0, 0, max_pwm * 10000),
            iin_target: i_target_initial
        }
    }

    pub fn restart(&mut self, start_pwm: i32)
    {
        self.pid.restart_from_output_value(start_pwm);
    }

    pub fn update(&mut self, vin: i32, vout: i32, iout: i32) -> i32
    {
        let pout = vout * iout;
        let iin;

        if vin <= 0 {
            iin = 0;
        } else {
            iin = (pout / vin) * 85 / 100;
        }

        self.iin_target = vin * MOTOR_SPEC_I / MOTOR_SPEC_U;

        self.pid.set_target(self.iin_target);

        self.pid.update(iin)
    }

    pub fn log_status(&self, logger: &mut dyn Logger)
    {
        let mut data: String<16>;

        logger.log(b"I_target: ").unwrap();

        data = String::from(self.iin_target);
        logger.log(data.as_bytes()).unwrap();

        logger.log(b"mA - ").unwrap();

        self.pid.log_status(logger);
    }
}


pub struct SwitchControl<const T0: i32>
{
    mode: ControlMode,
    mode_switch_delay_counter: u32,
    cv_pid: PID<10000, T0>,
    mppt: MPPT<200, 0 /* percent loss of peak power */>,
    imp_ctrl: ImpedanceControl<T0, 5400, 1000>,
    pwm_max: i32,
    pwm_cur: i32,
    cv_target: i32,
    cv_max_deviation: i32
}

impl<const T0: i32> SwitchControl<T0>
{
    pub fn new(cv_target: i32, cv_max_deviation: i32, pwm_max: i32) -> Self
    {
        SwitchControl::<T0> {
            mode: ControlMode::ImpedanceControl,
            mode_switch_delay_counter: 0,
            cv_pid: PID::<10000, T0>::new(100, 50, 50, cv_target, 0, 0, pwm_max * 10000),
            mppt: MPPT::<200, 0>::new(3*pwm_max/4, 0, pwm_max),
            imp_ctrl: ImpedanceControl::<T0, 5400, 1000>::new(1000, pwm_max),
            pwm_max,
            pwm_cur: 0,
            cv_target,
            cv_max_deviation
        }
    }

    // returns the new PWM value
    pub fn update(&mut self, vin: i32, vout: i32, iout: i32) -> i32
    {
        // Mode switching (only if delay counter has expired)
        if self.mode_switch_delay_counter == 0 {
            match self.mode {
                ControlMode::MPPT =>
                    if vout > self.cv_target {
                        self.mode = ControlMode::ConstantVoltage;
                        self.cv_pid.restart_from_output_value(self.pwm_cur * 80 / 100);

                        // stay in CV mode for at least 100 update cycles
                        self.mode_switch_delay_counter = 100;
                    },

                ControlMode::ImpedanceControl =>
                    if vout > self.cv_target {
                        self.mode = ControlMode::ConstantVoltage;
                        self.cv_pid.restart_from_output_value(self.pwm_cur * 80 / 100);

                        // stay in CV mode for at least 100 update cycles
                        self.mode_switch_delay_counter = 100;
                    },

                ControlMode::ConstantVoltage =>
                    if vout < self.cv_target - self.cv_max_deviation {
                        self.mode = ControlMode::ImpedanceControl;

                        // begin MPPT tracking by searching downwards, because the CV PID controller
                        // probably has pushed the PWM to the upper limit due to too low output
                        // voltage.
                        //self.mppt.restart(self.pwm_max * 80 / 100, -1);

                        // back to impedance control
                        self.imp_ctrl.restart(self.pwm_max * 80 / 100);
                    }
            }
        } else {
            self.mode_switch_delay_counter -= 1;
        }

        // update the PWM value
        let pwm = match self.mode {
            ControlMode::ConstantVoltage => self.cv_pid.update(vout),
            ControlMode::MPPT => self.mppt.update(vout, iout),
            ControlMode::ImpedanceControl => self.imp_ctrl.update(vin, vout, iout),
        };

        // store and limit pwm value
        self.pwm_cur = pwm;
        if self.pwm_cur > self.pwm_max {
            self.pwm_cur = self.pwm_max;
        } else if self.pwm_cur < 0 {
            self.pwm_cur = 0;
        }

        self.pwm_cur
    }

    pub fn log_status(&self, logger: &mut dyn Logger)
    {
        match self.mode {
            ControlMode::MPPT => {
                logger.log(b"[MPPT] ").unwrap();
                self.mppt.log_status(logger);
            }

            ControlMode::ConstantVoltage => {
                logger.log(b"[CV] ").unwrap();
                self.cv_pid.log_status(logger);
            }

            ControlMode::ImpedanceControl => {
                logger.log(b"[Imp] ").unwrap();
                self.imp_ctrl.log_status(logger);
            }
        }

        logger.log(b"\r\n").unwrap();
    }

    pub fn get_control_mode(&self) -> ControlMode
    {
        self.mode
    }
}