tt_rsmpc_main.m 6.61 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223
function [exectime, data] = tt_switching_main(segment, data)

  exectime = 0;
  switch segment
    case 1 % Step time and read input
      data.k = data.k + 1;

      sp = ttAnalogIn(1);
      pos = ttAnalogIn(2);
      vel = ttAnalogIn(3);
      ang = ttAnalogIn(4);

      data.xk = [pos vel ang]';
      data.xref = [sp 0 0]';

      data.drop = ttAnalogIn(5);
      if data.mode == 4
        data.drop = 0;
      end
      data.Delta = zeros(1,4); % Selection vector

    case 2 % Update remote response. Mode 0 is local only. Flag_i checks feasibility. Drop=0 means network was ok.

      if data.flag_i == 1 % Feasible result
        ttAnalogOut(4, 1);
      else
        ttAnalogOut(4, 0);
      end

      if data.mode > 0 && data.flag_i == 1 && data.drop == 0
        data.Delta(1) = 1;
        data.i = data.k-1; % Set the time of start for successful HF optimization. 
        data.applyi = data.k;
        data.Ru = data.u_i;
        if data.mode == 1 || data.mode == 3 % Stop local execution
          data.local_run_at = -1;
          if data.mode == 1
            data.r = data.k + 3;
          else
            data.r = data.k + 2;
          end
        else
          data.r = data.k + mod(data.k,data.sigma)+data.sigma;
        end
      elseif data.drop == 0 && data.flag_i ~= 1
        disp(sprintf('HF infeasible, k=%d', data.k));
      end

    case 3 % Update local response and determine recovery
      if data.pend_j > -1 && data.pend_j == data.k-2
        if data.flag_j == 1  % Feasible
          ttAnalogOut(5, 1);
          data.j = data.pend_j;
          data.applyj = data.k;
          data.Lu = data.pend_Lu;
        else
          ttAnalogOut(5, 0);
        end
      end

      if data.applyj-data.applyi >= data.sigma && data.k - data.applyj < data.sigma
        data.Delta(2) = 1;
      end

      DeltaBar = 0;
      if data.k ~= 0 && data.k < data.r % What if there is no initial networked response?
        DeltaBar = 1;
      end

      data.Delta(3) = DeltaBar*(1-sum(data.Delta(1,1:2)));
      data.Delta(4) = (1-sum(data.Delta(1,1:3)));

      if sum(data.Delta) == 0 || sum(data.Delta) > 1
        ME = MException('SwitchingMPC:DeltaError', 'Invalid Delta vector %i%i%i%i', data.Delta);
        throw(ME)
      end

    case 4 % Select and apply control signal
      % HF selector function \Delta_\alpha
      deltaAlpha = zeros(1,3);
      if data.i > -1 && data.k-data.i <= length(deltaAlpha)
        deltaAlpha(data.k-data.i) = 1;
      end
      deltaB = zeros(1,2);
      if data.j > -1 && data.k-data.j > 1 && data.k-data.j-1 <= length(deltaB)
        deltaB(data.k-data.j-1) = 1;
      end
      deltaG = (1-sum(deltaAlpha(1:2)))*deltaB(1) + (1-sum(deltaAlpha(2:3)))*deltaB(2);

      deltaOne = deltaAlpha(1);

      deltaB = 0;

      if deltaOne == 1          % Remote closed loop control
        data.u = data.Ru(1);
        data.which = 2;
      elseif deltaG == 1
        data.u = data.Lu;
        data.which = 1;
      elseif deltaAlpha(2) == 1
        if data.zetaVariant == 2
          data.u = data.Ru(2);
        else
          data.u = data.Ru(1);
        end
        data.which = 1.75;
      elseif deltaAlpha(3) == 1
        if data.zetaVariant == 2
          data.u = data.Ru(3);
        else
          data.u = data.Ru(1);
        end
        data.which = 1.5;
      else
        data.u = data.mpcR.K*(data.xref-data.xk); % Calculate HF LQ for recovery mode
        data.which = 0;
      end

      zeta_which = 0;
      % The paper logic
      if data.zetaVariant == 0 % LQ control
        zeta_1 = data.mpcR.K*(data.xref-data.xk);
        zeta_which = 0;
      elseif data.zetaVariant == 1 % Hold control signal
        zeta_1 = data.Ru(1);
        zeta_which = 2;
      elseif data.zetaVariant == 2 % Open loop control
        if data.k - (data.i+1) < length(data.Ru)
          zeta_1 = data.Ru(data.k-data.i); % Note: i is not according to definition
          zeta_which = 2-0.25*(data.k-(data.i+1));
        else
          zeta_1 = 0;
          zeta_which = -1;
        end
      else  % Hold during MPC_2 execution
        if data.mode == 1 % Mode A
          % Applies one step open loop since MPC_2 is started in this time step
          zeta_1 = data.Ru(2);
          zeta_which = 1.5;
        elseif data.mode == 2 % Mode B
          if data.k == data.local_run_at
            zeta_1 = data.Ru(2);
            zeta_which = 1.5;
          else
            zeta_1 = data.Ru(1);
            zeta_which = 1.75;
          end
        else % Mode C
          % Holds control signal since MPC_2 was started in previous time step
          zeta_1 = data.Ru(1);
          zeta_which = 1.75;
        end
      end
      zeta_2 = data.mpcR.K*(data.xref-data.xk);
      data.zeta2cost = 0.5*(data.xref-data.xk)'*data.mpcR.S*(data.xref-data.xk);
      selections = [data.Ru(1) data.Lu zeta_1 zeta_2];
      newu = data.Delta*selections';

      if data.Delta(1) == 1
        data.which = 2;
      elseif data.Delta(2) == 1
        data.which = 1;
      elseif data.Delta(3) == 1
        data.which = zeta_which;
      else
        data.which = 0;
      end

      data.u = newu;

      ttAnalogOut(1, data.u);
      ttAnalogOut(2, data.which);

      if data.mode == 1 && data.local_run_at == -1 % Mode A, shut down local on remote response
        data.localExec = 0;
      end

    case 5
      % Always run the HF controller
      x = data.mpcR.A*data.xk + data.mpcR.B*data.u; % HF state prediction
      [optx, fval, exitflag, output] = runquadprog(data.mpcR, x, data.xref, data.NR, data.NR);
      if exitflag == 1
        data.u_i = optx(data.NR*3+1:data.NR*3+3); % Three steps of control signal from HF
      end
      data.flag_i = exitflag;

    case 6
      if data.mode == 1 && data.drop == 1 % Mode A, start Local MPC when drop occurs
        if data.local_run_at == -1 % New drop
          data.local_run_at = data.k;
        end
      elseif data.mode == 2 || data.mode == 3
        if data.local_run_at == -1
          data.local_run_at = data.k;
        end
      elseif data.mode == 0 && data.local_run_at == -1 % Local only, start and keep running
        data.local_run_at = data.k;
      end

    case 7
      if data.local_run_at == data.k
        x = data.mpcL.A*data.xk + data.mpcL.B*data.u;
        [ox, fv, fl, op] = runquadprog(data.mpcL, x, data.xref, data.NL, data.NL);
        data.flag_j = fl;
        data.pend_j = data.k;
        if fl == 1 
          data.pend_Lu = ox(data.NL*3+1);
        else
         data.u = 0; % Hm....
        end
        data.local_run_at = data.k+2;
        data.localExec = 1;
      end
      ttAnalogOut(3, data.localExec);
      data.localExec = -1*data.localExec;

    case 8
      exectime = 0.01;

    case 9
      exectime = -1;
  end
end