Changeset 1302 for applications/pmsm
- Timestamp:
- 03/23/11 19:39:30 (13 years ago)
- Files:
-
- 1 modified
-
applications/pmsm/pmsm_ctrl.h (modified) (24 diffs)
Legend:
- Unmodified
- Added
- Removed
-
applications/pmsm/pmsm_ctrl.h
r1298 r1302 174 174 UIREGISTER(PMSM_PICtrl); 175 175 176 /*************************************/ 177 /* LQ alpa-beta */ 178 /*************************************/ 179 176 180 class PMSM_LQCtrl: public PMSMCtrl{ 177 181 public: … … 214 218 215 219 //control maximum 216 const double MAXu; 220 double MAXu; 221 int MAXuflag; 217 222 218 223 //lqg controler class … … 225 230 r(0.005), Dt(0.000125), rec_hor(10), //for r is a default value rewrited by pcp.txt file value 226 231 A(5, 5), B(5, 2), Q(5, 5), R(2, 2), 227 uab(2), icond(6), MAXu(100.0) {232 uab(2), icond(6), MAXu(100.0), MAXuflag(0) { 228 233 //set fix matrices elements & dimensions 229 234 A.zeros(); … … 294 299 vec tmp = L*icond; 295 300 296 uab = tmp(0,1); 297 298 if(uab(0) > MAXu) uab(0) = MAXu; 299 else if(uab(0) < -MAXu) uab(0) = -MAXu; 300 if(uab(1) > MAXu) uab(1) = MAXu; 301 else if(uab(1) < -MAXu) uab(1) = -MAXu; 301 uab = tmp(0,1); 302 303 if(MAXuflag == 1){ //square cut off 304 if(uab(0) > MAXu) uab(0) = MAXu; 305 else if(uab(0) < -MAXu) uab(0) = -MAXu; 306 if(uab(1) > MAXu) uab(1) = MAXu; 307 else if(uab(1) < -MAXu) uab(1) = -MAXu; 308 } 309 else if(MAXuflag == 2){ //circular cut off 310 double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); 311 double uangl = atan2(uab(1), uab(0)); 312 if (uampl > MAXu) uampl = MAXu; 313 uab(0) = uampl*cos(uangl); 314 uab(1) = uampl*sin(uangl); 315 } 316 //else{ //(MAXuflag == 0) //no cut off } 302 317 303 318 return uab; 304 }; 319 } 320 305 321 void from_setting(const Setting &set){ 306 322 PMSMCtrl::from_setting(set); 307 323 UI::get(r,set, "r", UI::optional); 308 324 UI::get(rec_hor,set, "h", UI::optional); 325 UI::get(MAXu,set, "MAXu", UI::optional); 326 UI::get(MAXuflag,set, "MAXuflag", UI::optional); 309 327 } 310 328 … … 351 369 UIREGISTER(PMSM_LQCtrl); 352 370 371 /*************************************/ 372 /* LQ d-q 1 */ 373 /*************************************/ 374 353 375 class PMSM_LQCtrl_dq: public PMSMCtrl{ 354 376 public: … … 371 393 //input penalty 372 394 double r; 395 double rpd; //penalize differences u - u_old 373 396 374 397 //time step … … 383 406 //mat C; //2x5 384 407 mat Q; //5x5 385 mat R; //2x2 408 mat R; //2x2 409 mat Rpd; //2x4 386 410 387 411 //control matrix 388 412 mat L; 389 vec uab; 390 vec icond; 413 vec uab; 414 vec icondpd; 415 vec u_old; 391 416 392 417 //control maximum 393 const double MAXu; 418 double MAXu; 419 int MAXuflag; 394 420 395 421 //lqg controler class … … 400 426 401 427 PMSM_LQCtrl_dq():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), 402 r(0.005), Dt(0.000125), rec_hor(10), //for r is a default value rewrited by pcp.txt file value403 A(5, 5), B(5, 2), Q(5, 5), R(2, 2), 404 uab(2), icond(6), MAXu(100.0) {428 r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), //for r is a default value rewrited by pcp.txt file value 429 A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), 430 uab(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0) { 405 431 //set fix matrices elements & dimensions 406 432 A.zeros(); … … 413 439 A(3, 3) = A(4, 4) = 1.0; 414 440 B.zeros(); 415 B(0, 0) = B(1, 1) = c; 416 //C.zeros(); 417 // C(0, 0) = C(1, 1) = 1.0; 441 B(0, 0) = B(1, 1) = c; 418 442 Q.zeros(); 419 443 Q(2, 2) = 1.0; 420 444 R.zeros(); 421 445 Rpd.zeros(); 446 u_old.zeros(); 422 447 } 423 448 … … 425 450 virtual vec ctrlaction(const itpp::vec& cond) { 426 451 PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww 427 428 int i; 452 429 453 vec udq; 454 vec tmp; 455 430 456 lq.resetTime(); 431 432 //create prediction 433 /*p_isa.zeros(); 434 p_isb.zeros(); 435 p_ome.zeros(); 436 p_the.zeros(); 437 p_isa(0) = isa; 438 p_isb(0) = isb; 439 p_ome(0) = ome; 440 p_the(0) = the; 441 442 //create control matrix 443 /*for(i = rec_hor; i > 0; i--){ 444 lq.redesign(); 445 } 446 lq.redesign(); 447 */ 457 448 458 L = lq.getL(); 449 icond(0) = isa*cos(the) + isb*sin(the); 450 icond(1) = isb*cos(the) - isa*sin(the); 451 icond(2) = ome - Ww; 452 icond(3) = the; 453 icond(4) = Ww; 454 icond(5) = 1; 455 vec tmp = L*icond; 456 459 460 icondpd(0) = isa*cos(the) + isb*sin(the); 461 icondpd(1) = isb*cos(the) - isa*sin(the); 462 icondpd(2) = ome - Ww; 463 icondpd(3) = the; 464 icondpd(4) = Ww; 465 icondpd(5) = u_old(0); 466 icondpd(6) = u_old(1); 467 icondpd(7) = 1; 468 469 tmp = L*icondpd; 470 457 471 udq = tmp(0,1); 458 472 … … 461 475 uab(1) = udq(1)*cos(the) + udq(0)*sin(the); 462 476 463 if(uab(0) > MAXu) uab(0) = MAXu; 464 else if(uab(0) < -MAXu) uab(0) = -MAXu; 465 if(uab(1) > MAXu) uab(1) = MAXu; 466 else if(uab(1) < -MAXu) uab(1) = -MAXu; 477 if(MAXuflag == 1){ //square cut off 478 if(uab(0) > MAXu) uab(0) = MAXu; 479 else if(uab(0) < -MAXu) uab(0) = -MAXu; 480 if(uab(1) > MAXu) uab(1) = MAXu; 481 else if(uab(1) < -MAXu) uab(1) = -MAXu; 482 } 483 else if(MAXuflag == 2){ //circular cut off 484 double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); 485 double uangl = atan2(uab(1), uab(0)); 486 if (uampl > MAXu) uampl = MAXu; 487 uab(0) = uampl*cos(uangl); 488 uab(1) = uampl*sin(uangl); 489 } 490 //else{ //(MAXuflag == 0) //no cut off } 491 492 u_old = uab; 467 493 468 494 return uab; 469 }; 495 } 496 470 497 void from_setting(const Setting &set){ 471 498 PMSMCtrl::from_setting(set); 472 499 UI::get(r,set, "r", UI::optional); 473 500 UI::get(rec_hor,set, "h", UI::optional); 501 UI::get(MAXu,set, "MAXu", UI::optional); 502 UI::get(MAXuflag,set, "MAXuflag", UI::optional); 503 UI::get(rpd,set, "rpd", UI::optional); 474 504 } 475 505 476 506 void validate(){ 477 R(0,0)=R(1,1)=r; 478 507 R(0, 0) = R(1, 1) = r; 508 Rpd(0, 0) = Rpd(1, 1) = rpd; 509 Rpd(0, 2) = Rpd(1, 3) = -rpd; 510 479 511 p_isa.set_length(rec_hor+1); 480 512 p_isb.set_length(rec_hor+1); … … 482 514 p_the.set_length(rec_hor+1); 483 515 484 Array<quadraticfn> qloss( 2);516 Array<quadraticfn> qloss(3); 485 517 qloss(0).Q.setCh(Q); 486 qloss(0).rv = RV("x", 5, 1); 518 qloss(0).rv = RV("x", 5, 1); 487 519 qloss(1).Q.setCh(R); 488 qloss(1).rv = RV("u", 2, 0); 520 qloss(1).rv = RV("u", 2, 0); 521 qloss(2).Q.setCh(Rpd); 522 qloss(2).rv = RV("u", 2, 0); 523 qloss(2).rv.add(RV("u", 2, -1)); 489 524 lq.Losses = qloss; 490 525 491 //set lq 492 lq.rv = RV("u", 2, 0); 493 lq.set_rvc(RV("x", 5, 0)); 526 //set lq 527 lq.rv = RV("u", 2, 0); 528 RV tmp = RV("x", 5, 0); 529 tmp.add(RV("u", 2, -1)); 530 lq.set_rvc(tmp); 494 531 lq.horizon = rec_hor; 495 532 … … 507 544 lq.finalLoss.Q.setCh(Q); 508 545 lq.finalLoss.rv = RV("x", 5, 1); 509 546 510 547 lq.validate(); 511 548 512 549 uab.zeros(); 513 550 514 551 //create control matrix 515 552 for(int i = rec_hor; i > 0; i--){ … … 520 557 }; 521 558 UIREGISTER(PMSM_LQCtrl_dq); 559 560 /*************************************/ 561 /* LQ d-q 2 */ 562 /*************************************/ 522 563 523 564 class PMSM_LQCtrl_dq2: public PMSMCtrl{ … … 541 582 //input penalty 542 583 double r; 584 double rpd; //penalize differences u - u_old 543 585 544 586 //time step … … 553 595 //mat C; //2x5 554 596 mat Q; //5x5 555 mat R; //2x2 597 mat R; //2x2 598 mat Rpd; //2x4 556 599 557 600 //control matrix 558 601 mat L; 559 602 vec uab,udq; 560 vec icond; 603 vec icondpd; 604 vec u_old; 561 605 562 606 //control maximum 563 const double MAXu; 607 double MAXu; 608 int MAXuflag; 564 609 565 610 //lqg controler class … … 570 615 571 616 PMSM_LQCtrl_dq2():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), 572 r(0.005), Dt(0.000125), rec_hor(10), //for r is a default value rewrited by pcp.txt file value573 A(5, 5), B(5, 2), Q(5, 5), R(2, 2), 574 uab(2), udq(2), icond(6), MAXu(100.0) {617 r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), //for r is a default value rewrited by pcp.txt file value 618 A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), 619 uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0) { 575 620 //set fix matrices elements & dimensions 576 621 A.zeros(); … … 582 627 A(3, 3) = A(4, 4) = 1.0; 583 628 B.zeros(); 584 B(0, 0) = B(1, 1) = c; 585 //C.zeros(); 586 // C(0, 0) = C(1, 1) = 1.0; 629 B(0, 0) = B(1, 1) = c; 587 630 Q.zeros(); 588 631 Q(2, 2) = 1.0; 589 632 R.zeros(); 590 633 Rpd.zeros(); 634 u_old.zeros(); 591 635 } 592 636 … … 596 640 597 641 int i; 642 vec tmp; 598 643 599 644 lq.resetTime(); 600 //cout << "OK" << endl;645 601 646 //create prediction 602 647 p_isd.zeros(); … … 608 653 p_ome(0) = ome; 609 654 p_the(0) = the; 610 //cout << "OKkkk" << endl; 611 for(i = 0; i < rec_hor-1; i++){ 612 /*p_isa(i+1) = a*p_isa(i) + b*p_ome(i)*sin(p_the(i)) + c*0;//uab(0); //use last used control 613 p_isb(i+1) = a*p_isb(i) - b*p_ome(i)*cos(p_the(i)) + c*0;//uab(1); 614 p_ome(i+1) = d*p_ome(i) + e*(p_isb(i)*cos(p_the(i)) - p_isa(i)*sin(p_the(i))); 615 p_the(i+1) = p_the(i) + Dt*p_ome(i);*/ 616 p_isd(i+1) = a*p_isd(i) + p_isq(i)*p_ome(i)*Dt + c*udq(0); 617 //cout << "OKaa" << endl; 655 656 for(i = 0; i < rec_hor-1; i++){ 657 p_isd(i+1) = a*p_isd(i) + p_isq(i)*p_ome(i)*Dt + c*udq(0); 618 658 p_isq(i+1) = -p_isd(i)*p_ome(i)*Dt + a*p_isq(i) - b*p_ome(i) + c*udq(1); 619 659 p_ome(i+1) = d*p_ome(i) + e*p_isq(i); 620 p_the(i+1) = p_the(i) + Dt*p_ome(i); 621 //cout << "1"; 660 p_the(i+1) = p_the(i) + Dt*p_ome(i); 622 661 } 623 662 624 663 //create control matrix 625 664 for(i = rec_hor; i > 0; i--){ 626 //set variable matrices elements (A matrix only) 627 /*A(0, 2) = b*sin(p_the(i)); 628 A(0, 3) = b*(p_ome(i))*cos(p_the(i)); 629 A(0, 4) = b*sin(p_the(i)); 630 A(1, 2) = -b*cos(p_the(i)); 631 A(1, 3) = b*(p_ome(i))*sin(p_the(i)); 632 A(1, 4) = -b*cos(p_the(i)); 633 A(2, 0) = -e*sin(p_the(i)); 634 A(2, 1) = e*cos(p_the(i)); 635 A(2, 3) = -e*(p_isa(i)*cos(p_the(i)) + p_isb(i)*sin(p_the(i)));*/ 665 //set variable matrices elements (A matrix only) 636 666 A(0, 1) = Dt*p_ome(i); 637 667 A(0, 2) = Dt*p_isq(i); … … 647 677 648 678 L = lq.getL(); 649 //icond(0) = isa; 650 //icond(1) = isb; 651 icond(0) = isa*cos(the) + isb*sin(the); 652 icond(1) = isb*cos(the) - isa*sin(the); 653 icond(2) = ome - Ww; 654 icond(3) = the; 655 icond(4) = Ww; 656 icond(5) = 1; 657 vec tmp = L*icond; 658 659 //uab = tmp(0,1); 679 680 icondpd(0) = isa*cos(the) + isb*sin(the); 681 icondpd(1) = isb*cos(the) - isa*sin(the); 682 icondpd(2) = ome - Ww; 683 icondpd(3) = the; 684 icondpd(4) = Ww; 685 icondpd(5) = u_old(0); 686 icondpd(6) = u_old(1); 687 icondpd(7) = 1; 688 689 tmp = L*icondpd; 690 660 691 udq = tmp(0,1); 661 692 … … 664 695 uab(1) = udq(1)*cos(the) + udq(0)*sin(the); 665 696 666 if(uab(0) > MAXu) uab(0) = MAXu; 667 else if(uab(0) < -MAXu) uab(0) = -MAXu; 668 if(uab(1) > MAXu) uab(1) = MAXu; 669 else if(uab(1) < -MAXu) uab(1) = -MAXu; 697 if(MAXuflag == 1){ //square cut off 698 if(uab(0) > MAXu) uab(0) = MAXu; 699 else if(uab(0) < -MAXu) uab(0) = -MAXu; 700 if(uab(1) > MAXu) uab(1) = MAXu; 701 else if(uab(1) < -MAXu) uab(1) = -MAXu; 702 } 703 else if(MAXuflag == 2){ //circular cut off 704 double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); 705 double uangl = atan2(uab(1), uab(0)); 706 if (uampl > MAXu) uampl = MAXu; 707 uab(0) = uampl*cos(uangl); 708 uab(1) = uampl*sin(uangl); 709 } 710 //else{ //(MAXuflag == 0) //no cut off } 711 712 u_old = uab; 670 713 671 714 return uab; 672 }; 715 } 716 673 717 void from_setting(const Setting &set){ 674 718 PMSMCtrl::from_setting(set); 675 719 UI::get(r,set, "r", UI::optional); 676 720 UI::get(rec_hor,set, "h", UI::optional); 721 UI::get(MAXu,set, "MAXu", UI::optional); 722 UI::get(MAXuflag,set, "MAXuflag", UI::optional); 723 UI::get(rpd,set, "rpd", UI::optional); 677 724 } 678 725 679 726 void validate(){ 680 R(0,0)=R(1,1)=r; 681 727 R(0, 0) = R(1, 1) = r; 728 Rpd(0, 0) = Rpd(1, 1) = rpd; 729 Rpd(0, 2) = Rpd(1, 3) = -rpd; 730 682 731 p_isd.set_length(rec_hor+1); 683 732 p_isq.set_length(rec_hor+1); … … 685 734 p_the.set_length(rec_hor+1); 686 735 687 Array<quadraticfn> qloss( 2);736 Array<quadraticfn> qloss(3); 688 737 qloss(0).Q.setCh(Q); 689 qloss(0).rv = RV("x", 5, 1); 738 qloss(0).rv = RV("x", 5, 1); 690 739 qloss(1).Q.setCh(R); 691 qloss(1).rv = RV("u", 2, 0); 740 qloss(1).rv = RV("u", 2, 0); 741 qloss(2).Q.setCh(Rpd); 742 qloss(2).rv = RV("u", 2, 0); 743 qloss(2).rv.add(RV("u", 2, -1)); 692 744 lq.Losses = qloss; 693 745 694 //set lq 695 lq.rv = RV("u", 2, 0); 696 lq.set_rvc(RV("x", 5, 0)); 746 //set lq 747 lq.rv = RV("u", 2, 0); 748 RV tmp = RV("x", 5, 0); 749 tmp.add(RV("u", 2, -1)); 750 lq.set_rvc(tmp); 751 697 752 lq.horizon = rec_hor; 698 753 … … 710 765 lq.finalLoss.Q.setCh(Q); 711 766 lq.finalLoss.rv = RV("x", 5, 1); 712 767 713 768 lq.validate(); 714 769 715 770 uab.zeros(); 716 udq.zeros(); 717 771 udq.zeros(); 718 772 } 719 773 };
