| | 330 | class PMSM_LQCtrl_dq: public PMSMCtrl{ |
| | 331 | public: |
| | 332 | /* |
| | 333 | PMSMCtrl: |
| | 334 | double isa; |
| | 335 | double isb; |
| | 336 | double ome; |
| | 337 | double the; |
| | 338 | double Ww; |
| | 339 | */ |
| | 340 | |
| | 341 | //PMSM parameters |
| | 342 | const double a; |
| | 343 | const double b; |
| | 344 | const double c; |
| | 345 | const double d; |
| | 346 | const double e; |
| | 347 | |
| | 348 | //input penalty |
| | 349 | double r; |
| | 350 | |
| | 351 | //time step |
| | 352 | const double Dt; |
| | 353 | |
| | 354 | //receding horizon |
| | 355 | int rec_hor; |
| | 356 | |
| | 357 | //system matrices |
| | 358 | mat A; //5x5 |
| | 359 | mat B; //5x2 |
| | 360 | //mat C; //2x5 |
| | 361 | mat Q; //5x5 |
| | 362 | mat R; //2x2 |
| | 363 | |
| | 364 | //control matrix |
| | 365 | mat L; |
| | 366 | vec uab; |
| | 367 | vec icond; |
| | 368 | |
| | 369 | //control maximum |
| | 370 | const double MAXu; |
| | 371 | |
| | 372 | //lqg controler class |
| | 373 | LQG_universal lq; |
| | 374 | |
| | 375 | //prediction |
| | 376 | vec p_isa, p_isb, p_ome, p_the; |
| | 377 | |
| | 378 | PMSM_LQCtrl_dq():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), |
| | 379 | r(0.005), Dt(0.000125), rec_hor(10), //for r is a default value rewrited by pcp.txt file value |
| | 380 | A(5, 5), B(5, 2), Q(5, 5), R(2, 2), |
| | 381 | uab(2), icond(6), MAXu(100.0) { |
| | 382 | //set fix matrices elements & dimensions |
| | 383 | A.zeros(); |
| | 384 | A(0, 0) = A(1, 1) = a; |
| | 385 | A(1, 2) = A(1, 4) = -b; |
| | 386 | A(2, 1) = e; |
| | 387 | A(2, 2) = d; |
| | 388 | A(2, 4) = d - 1; |
| | 389 | A(3, 2) = A(3, 4) = Dt; |
| | 390 | A(3, 3) = A(4, 4) = 1.0; |
| | 391 | B.zeros(); |
| | 392 | B(0, 0) = B(1, 1) = c; |
| | 393 | //C.zeros(); |
| | 394 | // C(0, 0) = C(1, 1) = 1.0; |
| | 395 | Q.zeros(); |
| | 396 | Q(2, 2) = 1.0; |
| | 397 | R.zeros(); |
| | 398 | |
| | 399 | } |
| | 400 | |
| | 401 | |
| | 402 | virtual vec ctrlaction(const itpp::vec& cond) { |
| | 403 | PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww |
| | 404 | |
| | 405 | int i; |
| | 406 | vec udq; |
| | 407 | lq.resetTime(); |
| | 408 | |
| | 409 | //create prediction |
| | 410 | p_isa.zeros(); |
| | 411 | p_isb.zeros(); |
| | 412 | p_ome.zeros(); |
| | 413 | p_the.zeros(); |
| | 414 | p_isa(0) = isa; |
| | 415 | p_isb(0) = isb; |
| | 416 | p_ome(0) = ome; |
| | 417 | p_the(0) = the; |
| | 418 | |
| | 419 | //create control matrix |
| | 420 | /*for(i = rec_hor; i > 0; i--){ |
| | 421 | lq.redesign(); |
| | 422 | } |
| | 423 | lq.redesign(); |
| | 424 | */ |
| | 425 | L = lq.getL(); |
| | 426 | icond(0) = isa*cos(the) + isb*sin(the); |
| | 427 | icond(1) = isb*cos(the) - isa*sin(the); |
| | 428 | icond(2) = ome - Ww; |
| | 429 | icond(3) = the; |
| | 430 | icond(4) = Ww; |
| | 431 | icond(5) = 1; |
| | 432 | vec tmp = L*icond; |
| | 433 | |
| | 434 | udq = tmp(0,1); |
| | 435 | |
| | 436 | uab = udq; //set size |
| | 437 | uab(0) = udq(0)*cos(the) - udq(1)*sin(the); |
| | 438 | uab(1) = udq(1)*cos(the) + udq(0)*sin(the); |
| | 439 | |
| | 440 | if(uab(0) > MAXu) uab(0) = MAXu; |
| | 441 | else if(uab(0) < -MAXu) uab(0) = -MAXu; |
| | 442 | if(uab(1) > MAXu) uab(1) = MAXu; |
| | 443 | else if(uab(1) < -MAXu) uab(1) = -MAXu; |
| | 444 | |
| | 445 | return uab; |
| | 446 | }; |
| | 447 | void from_setting(const Setting &set){ |
| | 448 | PMSMCtrl::from_setting(set); |
| | 449 | UI::get(r,set, "r", UI::optional); |
| | 450 | UI::get(rec_hor,set, "h", UI::optional); |
| | 451 | } |
| | 452 | |
| | 453 | void validate(){ |
| | 454 | R(0,0)=R(1,1)=r; |
| | 455 | |
| | 456 | p_isa.set_length(rec_hor+1); |
| | 457 | p_isb.set_length(rec_hor+1); |
| | 458 | p_ome.set_length(rec_hor+1); |
| | 459 | p_the.set_length(rec_hor+1); |
| | 460 | |
| | 461 | Array<quadraticfn> qloss(2); |
| | 462 | qloss(0).Q.setCh(Q); |
| | 463 | qloss(0).rv = RV("x", 5, 1); |
| | 464 | qloss(1).Q.setCh(R); |
| | 465 | qloss(1).rv = RV("u", 2, 0); |
| | 466 | lq.Losses = qloss; |
| | 467 | |
| | 468 | //set lq |
| | 469 | lq.rv = RV("u", 2, 0); |
| | 470 | lq.set_rvc(RV("x", 5, 0)); |
| | 471 | lq.horizon = rec_hor; |
| | 472 | |
| | 473 | Array<linfnEx> model(2); |
| | 474 | model(0).A = A; |
| | 475 | model(0).B = vec("0 0 0 0 0"); |
| | 476 | model(0).rv = RV("x", 5, 0); |
| | 477 | model(0).rv_ret = RV("x", 5, 1); |
| | 478 | model(1).A = B; |
| | 479 | model(1).B = vec("0 0"); |
| | 480 | model(1).rv = RV("u", 2, 0); |
| | 481 | model(1).rv_ret = RV("x", 5, 1); |
| | 482 | lq.Models = model; |
| | 483 | |
| | 484 | lq.finalLoss.Q.setCh(Q); |
| | 485 | lq.finalLoss.rv = RV("x", 5, 1); |
| | 486 | |
| | 487 | lq.validate(); |
| | 488 | |
| | 489 | uab.zeros(); |
| | 490 | |
| | 491 | //create control matrix |
| | 492 | for(int i = rec_hor; i > 0; i--){ |
| | 493 | lq.redesign(); |
| | 494 | } |
| | 495 | lq.redesign(); |
| | 496 | } |
| | 497 | }; |
| | 498 | UIREGISTER(PMSM_LQCtrl_dq); |
| | 499 | |
