/*
 * WESPION_MC.c
 *
 *  Created on: Jul 8, 2023
 *      Author: JuHyung
 */


#include "WESPION.h"

WP_ControllerTypeDef WP_Ctrl={0,};
WP_ControllerErrorTypeDef WP_CtrlErr={0,};
WP_ErrorValueTypeDef WP_ErrRef;

WP_InverterTypeDef Inv={0,};

WP_MotorAngleTypeDef Motor1_Ang={0,}, Motor2_Ang={0,};
WP_MotorContrlTypeDef Motor1={0,}, Motor2={0,};

WP_InverterADrawdataTypeDef ADinj;
WP_InverterComAdTypeDef ComAd;

WP_InverterLpfTypeDef LpfCur, LpfVdc, LpfSpd;

const float sinTab[2048] =
{
    0.00000,0.00308,0.00613,0.00922,0.01227,0.01535,0.01840,0.02148,0.02454,0.02762,0.03067,0.03375,0.03680,0.03989,0.04294,0.04599,0.04907,0.05212,0.05521,0.05826,0.06131,0.06439,0.06744,0.07050,0.07355,0.07663,0.07968,0.08273,0.08578,0.08884,0.09192,0.09497,0.09802,0.10107,0.10413,0.10718,0.11023,0.11328,0.11630,0.11935,0.12241,0.12546,0.12848,0.13153,0.13458,0.13760,0.14066,0.14368,0.14673,0.14975,0.15280,0.15582,0.15884,0.16190,0.16492,0.16794,0.17096,0.17398,0.17700,0.18002,0.18304,0.18604,0.18906,0.19208,0.19510,0.19809,0.20111,0.20410,0.20709,0.21011,0.21310,0.21609,0.21909,0.22208,0.22507,0.22806,0.23105,0.23404,0.23703,0.23999,0.24298,0.24594,0.24893,0.25189,0.25485,0.25781,0.26077,0.26373,0.26669,0.26965,0.27261,0.27557,0.27850,0.28146,0.28439,0.28735,0.29028,0.29321,0.29614,0.29907,0.30200,0.30493,0.30783,0.31076,0.31366,0.31659,0.31949,0.32239,0.32529,0.32819,0.33109,0.33398,0.33688,0.33975,0.34265,0.34552,0.34842,0.35129,0.35416,0.35703,0.35989,0.36273,0.36560,0.36847,0.37131,0.37415,0.37698,0.37982,
    0.38266,0.38550,0.38834,0.39114,0.39398,0.39679,0.39960,0.40244,0.40524,0.40802,0.41083,0.41364,0.41641,0.41919,0.42200,0.42477,0.42755,0.43033,0.43307,0.43585,0.43860,0.44135,0.44412,0.44687,0.44958,0.45233,0.45508,0.45779,0.46051,0.46326,0.46597,0.46869,0.47137,0.47409,0.47678,0.47949,0.48218,0.48486,0.48755,0.49020,0.49289,0.49554,0.49820,0.50089,0.50351,0.50616,0.50882,0.51144,0.51410,0.51672,0.51935,0.52197,0.52457,0.52719,0.52979,0.53238,0.53497,0.53757,0.54016,0.54272,0.54532,0.54788,0.55045,0.55301,0.55554,0.55811,0.56064,0.56317,0.56570,0.56824,0.57077,0.57327,0.57581,0.57831,0.58081,0.58328,0.58578,0.58826,0.59073,0.59320,0.59567,0.59814,0.60059,0.60306,0.60550,0.60794,0.61035,0.61279,0.61520,0.61761,0.62003,0.62244,0.62485,0.62723,0.62961,0.63199,0.63437,0.63675,0.63910,0.64145,0.64380,0.64615,0.64850,0.65082,0.65317,0.65549,0.65778,0.66010,0.66238,0.66470,0.66699,0.66925,0.67154,0.67380,0.67609,0.67831,0.68057,0.68283,0.68506,0.68729,0.68951,0.69174,0.69394,0.69617,0.69836,0.70056,0.70273,0.70493,
    0.70709,0.70926,0.71140,0.71356,0.71570,0.71783,0.71997,0.72211,0.72421,0.72635,0.72845,0.73053,0.73264,0.73471,0.73679,0.73886,0.74094,0.74298,0.74503,0.74707,0.74911,0.75113,0.75317,0.75519,0.75717,0.75919,0.76117,0.76315,0.76514,0.76712,0.76907,0.77103,0.77298,0.77493,0.77686,0.77878,0.78070,0.78262,0.78455,0.78644,0.78833,0.79019,0.79208,0.79395,0.79581,0.79767,0.79950,0.80136,0.80319,0.80499,0.80682,0.80862,0.81042,0.81223,0.81403,0.81580,0.81757,0.81934,0.82108,0.82281,0.82455,0.82629,0.82803,0.82974,0.83145,0.83313,0.83484,0.83652,0.83820,0.83987,0.84152,0.84317,0.84482,0.84647,0.84808,0.84970,0.85132,0.85294,0.85452,0.85611,0.85770,0.85928,0.86084,0.86240,0.86395,0.86548,0.86703,0.86853,0.87006,0.87158,0.87308,0.87457,0.87604,0.87753,0.87900,0.88043,0.88190,0.88333,0.88477,0.88620,0.88760,0.88901,0.89041,0.89182,0.89319,0.89456,0.89594,0.89731,0.89865,0.89999,0.90131,0.90265,0.90396,0.90527,0.90656,0.90787,0.90915,0.91040,0.91168,0.91293,0.91418,0.91544,0.91666,0.91788,0.91910,0.92029,0.92148,0.92267,
    0.92386,0.92502,0.92618,0.92734,0.92847,0.92963,0.93073,0.93185,0.93295,0.93405,0.93515,0.93625,0.93732,0.93839,0.93942,0.94049,0.94153,0.94254,0.94357,0.94458,0.94559,0.94656,0.94757,0.94855,0.94949,0.95047,0.95142,0.95233,0.95328,0.95419,0.95511,0.95602,0.95691,0.95779,0.95868,0.95953,0.96039,0.96124,0.96210,0.96292,0.96375,0.96457,0.96536,0.96616,0.96695,0.96771,0.96851,0.96924,0.97000,0.97073,0.97147,0.97220,0.97290,0.97360,0.97430,0.97501,0.97568,0.97635,0.97699,0.97766,0.97830,0.97891,0.97955,0.98016,0.98074,0.98135,0.98193,0.98251,0.98306,0.98364,0.98419,0.98471,0.98526,0.98578,0.98627,0.98679,0.98727,0.98776,0.98822,0.98868,0.98914,0.98959,0.99002,0.99045,0.99088,0.99127,0.99167,0.99207,0.99246,0.99283,0.99319,0.99353,0.99387,0.99420,0.99454,0.99484,0.99515,0.99545,0.99573,0.99600,0.99628,0.99655,0.99680,0.99704,0.99725,0.99747,0.99768,0.99789,0.99808,0.99826,0.99844,0.99860,0.99878,0.99890,0.99905,0.99918,0.99930,0.99939,0.99951,0.99960,0.99966,0.99973,0.99979,0.99985,0.99991,0.99994,0.99994,0.99997,
    0.99997,0.99997,0.99994,0.99994,0.99991,0.99985,0.99979,0.99973,0.99966,0.99960,0.99951,0.99939,0.99930,0.99918,0.99905,0.99890,0.99878,0.99860,0.99844,0.99826,0.99808,0.99789,0.99768,0.99747,0.99725,0.99704,0.99680,0.99655,0.99628,0.99600,0.99573,0.99545,0.99515,0.99484,0.99454,0.99420,0.99387,0.99353,0.99319,0.99283,0.99246,0.99207,0.99167,0.99127,0.99088,0.99045,0.99002,0.98959,0.98914,0.98868,0.98822,0.98776,0.98727,0.98679,0.98627,0.98578,0.98526,0.98471,0.98419,0.98364,0.98306,0.98251,0.98193,0.98135,0.98074,0.98016,0.97955,0.97891,0.97830,0.97766,0.97699,0.97635,0.97568,0.97501,0.97430,0.97360,0.97290,0.97220,0.97147,0.97073,0.97000,0.96924,0.96851,0.96771,0.96695,0.96616,0.96536,0.96457,0.96375,0.96292,0.96210,0.96124,0.96039,0.95953,0.95868,0.95779,0.95691,0.95602,0.95511,0.95419,0.95328,0.95233,0.95142,0.95047,0.94949,0.94855,0.94757,0.94656,0.94559,0.94458,0.94357,0.94254,0.94153,0.94049,0.93942,0.93839,0.93732,0.93625,0.93515,0.93405,0.93295,0.93185,0.93073,0.92963,0.92847,0.92734,0.92618,0.92502,
    0.92386,0.92267,0.92148,0.92029,0.91910,0.91788,0.91666,0.91544,0.91418,0.91293,0.91168,0.91040,0.90915,0.90787,0.90656,0.90527,0.90396,0.90265,0.90131,0.89999,0.89865,0.89731,0.89594,0.89456,0.89319,0.89182,0.89041,0.88901,0.88760,0.88620,0.88477,0.88333,0.88190,0.88043,0.87900,0.87753,0.87604,0.87457,0.87308,0.87158,0.87006,0.86853,0.86703,0.86548,0.86395,0.86240,0.86084,0.85928,0.85770,0.85611,0.85452,0.85294,0.85132,0.84970,0.84808,0.84647,0.84482,0.84317,0.84152,0.83987,0.83820,0.83652,0.83484,0.83313,0.83145,0.82974,0.82803,0.82629,0.82455,0.82281,0.82108,0.81934,0.81757,0.81580,0.81403,0.81223,0.81042,0.80862,0.80682,0.80499,0.80319,0.80136,0.79950,0.79767,0.79581,0.79395,0.79208,0.79019,0.78833,0.78644,0.78455,0.78262,0.78070,0.77878,0.77686,0.77493,0.77298,0.77103,0.76907,0.76712,0.76514,0.76315,0.76117,0.75919,0.75717,0.75519,0.75317,0.75113,0.74911,0.74707,0.74503,0.74298,0.74094,0.73886,0.73679,0.73471,0.73264,0.73053,0.72845,0.72635,0.72421,0.72211,0.71997,0.71783,0.71570,0.71356,0.71140,0.70926,
    0.70709,0.70493,0.70273,0.70056,0.69836,0.69617,0.69394,0.69174,0.68951,0.68729,0.68506,0.68283,0.68057,0.67831,0.67609,0.67380,0.67154,0.66925,0.66699,0.66470,0.66238,0.66010,0.65778,0.65549,0.65317,0.65082,0.64850,0.64615,0.64380,0.64145,0.63910,0.63675,0.63437,0.63199,0.62961,0.62723,0.62485,0.62244,0.62003,0.61761,0.61520,0.61279,0.61035,0.60794,0.60550,0.60306,0.60059,0.59814,0.59567,0.59320,0.59073,0.58826,0.58578,0.58328,0.58081,0.57831,0.57581,0.57327,0.57077,0.56824,0.56570,0.56317,0.56064,0.55811,0.55554,0.55301,0.55045,0.54788,0.54532,0.54272,0.54016,0.53757,0.53497,0.53238,0.52979,0.52719,0.52457,0.52197,0.51935,0.51672,0.51410,0.51144,0.50882,0.50616,0.50351,0.50089,0.49820,0.49554,0.49289,0.49020,0.48755,0.48486,0.48218,0.47949,0.47678,0.47409,0.47137,0.46869,0.46597,0.46326,0.46051,0.45779,0.45508,0.45233,0.44958,0.44687,0.44412,0.44135,0.43860,0.43585,0.43307,0.43033,0.42755,0.42477,0.42200,0.41919,0.41641,0.41364,0.41083,0.40802,0.40524,0.40244,0.39960,0.39679,0.39398,0.39114,0.38834,0.38550,
    0.38266,0.37982,0.37698,0.37415,0.37131,0.36847,0.36560,0.36273,0.35989,0.35703,0.35416,0.35129,0.34842,0.34552,0.34265,0.33975,0.33688,0.33398,0.33109,0.32819,0.32529,0.32239,0.31949,0.31659,0.31366,0.31076,0.30783,0.30493,0.30200,0.29907,0.29614,0.29321,0.29028,0.28735,0.28439,0.28146,0.27850,0.27557,0.27261,0.26965,0.26669,0.26373,0.26077,0.25781,0.25485,0.25189,0.24893,0.24594,0.24298,0.23999,0.23703,0.23404,0.23105,0.22806,0.22507,0.22208,0.21909,0.21609,0.21310,0.21011,0.20709,0.20410,0.20111,0.19809,0.19510,0.19208,0.18906,0.18604,0.18304,0.18002,0.17700,0.17398,0.17096,0.16794,0.16492,0.16190,0.15884,0.15582,0.15280,0.14975,0.14673,0.14368,0.14066,0.13760,0.13458,0.13153,0.12848,0.12546,0.12241,0.11935,0.11630,0.11328,0.11023,0.10718,0.10413,0.10107,0.09802,0.09497,0.09192,0.08884,0.08578,0.08273,0.07968,0.07663,0.07355,0.07050,0.06744,0.06439,0.06131,0.05826,0.05521,0.05212,0.04907,0.04599,0.04294,0.03989,0.03680,0.03375,0.03067,0.02762,0.02454,0.02148,0.01840,0.01535,0.01227,0.00922,0.00613,0.00308,
    0.00000,-0.00308,-0.00613,-0.00922,-0.01227,-0.01535,-0.01840,-0.02148,-0.02454,-0.02762,-0.03067,-0.03375,-0.03680,-0.03989,-0.04294,-0.04599,-0.04907,-0.05212,-0.05521,-0.05826,-0.06131,-0.06439,-0.06744,-0.07050,-0.07355,-0.07663,-0.07968,-0.08273,-0.08578,-0.08884,-0.09192,-0.09497,-0.09802,-0.10107,-0.10413,-0.10718,-0.11023,-0.11328,-0.11630,-0.11935,-0.12241,-0.12546,-0.12848,-0.13153,-0.13458,-0.13760,-0.14066,-0.14368,-0.14673,-0.14975,-0.15280,-0.15582,-0.15884,-0.16190,-0.16492,-0.16794,-0.17096,-0.17398,-0.17700,-0.18002,-0.18304,-0.18604,-0.18906,-0.19208,-0.19510,-0.19809,-0.20111,-0.20410,-0.20709,-0.21011,-0.21310,-0.21609,-0.21909,-0.22208,-0.22507,-0.22806,-0.23105,-0.23404,-0.23703,-0.23999,-0.24298,-0.24594,-0.24893,-0.25189,-0.25485,-0.25781,-0.26077,-0.26373,-0.26669,-0.26965,-0.27261,-0.27557,-0.27850,-0.28146,-0.28439,-0.28735,-0.29028,-0.29321,-0.29614,-0.29907,-0.30200,-0.30493,-0.30783,-0.31076,-0.31366,-0.31659,-0.31949,-0.32239,-0.32529,-0.32819,-0.33109,-0.33398,-0.33688,-0.33975,-0.34265,-0.34552,-0.34842,-0.35129,-0.35416,-0.35703,-0.35989,-0.36273,-0.36560,-0.36847,-0.37131,-0.37415,-0.37698,-0.37982,
    -0.38266,-0.38550,-0.38834,-0.39114,-0.39398,-0.39679,-0.39960,-0.40244,-0.40524,-0.40802,-0.41083,-0.41364,-0.41641,-0.41919,-0.42200,-0.42477,-0.42755,-0.43033,-0.43307,-0.43585,-0.43860,-0.44135,-0.44412,-0.44687,-0.44958,-0.45233,-0.45508,-0.45779,-0.46051,-0.46326,-0.46597,-0.46869,-0.47137,-0.47409,-0.47678,-0.47949,-0.48218,-0.48486,-0.48755,-0.49020,-0.49289,-0.49554,-0.49820,-0.50089,-0.50351,-0.50616,-0.50882,-0.51144,-0.51410,-0.51672,-0.51935,-0.52197,-0.52457,-0.52719,-0.52979,-0.53238,-0.53497,-0.53757,-0.54016,-0.54272,-0.54532,-0.54788,-0.55045,-0.55301,-0.55554,-0.55811,-0.56064,-0.56317,-0.56570,-0.56824,-0.57077,-0.57327,-0.57581,-0.57831,-0.58081,-0.58328,-0.58578,-0.58826,-0.59073,-0.59320,-0.59567,-0.59814,-0.60059,-0.60306,-0.60550,-0.60794,-0.61035,-0.61279,-0.61520,-0.61761,-0.62003,-0.62244,-0.62485,-0.62723,-0.62961,-0.63199,-0.63437,-0.63675,-0.63910,-0.64145,-0.64380,-0.64615,-0.64850,-0.65082,-0.65317,-0.65549,-0.65778,-0.66010,-0.66238,-0.66470,-0.66699,-0.66925,-0.67154,-0.67380,-0.67609,-0.67831,-0.68057,-0.68283,-0.68506,-0.68729,-0.68951,-0.69174,-0.69394,-0.69617,-0.69836,-0.70056,-0.70273,-0.70493,
    -0.70709,-0.70926,-0.71140,-0.71356,-0.71570,-0.71783,-0.71997,-0.72211,-0.72421,-0.72635,-0.72845,-0.73053,-0.73264,-0.73471,-0.73679,-0.73886,-0.74094,-0.74298,-0.74503,-0.74707,-0.74911,-0.75113,-0.75317,-0.75519,-0.75717,-0.75919,-0.76117,-0.76315,-0.76514,-0.76712,-0.76907,-0.77103,-0.77298,-0.77493,-0.77686,-0.77878,-0.78070,-0.78262,-0.78455,-0.78644,-0.78833,-0.79019,-0.79208,-0.79395,-0.79581,-0.79767,-0.79950,-0.80136,-0.80319,-0.80499,-0.80682,-0.80862,-0.81042,-0.81223,-0.81403,-0.81580,-0.81757,-0.81934,-0.82108,-0.82281,-0.82455,-0.82629,-0.82803,-0.82974,-0.83145,-0.83313,-0.83484,-0.83652,-0.83820,-0.83987,-0.84152,-0.84317,-0.84482,-0.84647,-0.84808,-0.84970,-0.85132,-0.85294,-0.85452,-0.85611,-0.85770,-0.85928,-0.86084,-0.86240,-0.86395,-0.86548,-0.86703,-0.86853,-0.87006,-0.87158,-0.87308,-0.87457,-0.87604,-0.87753,-0.87900,-0.88043,-0.88190,-0.88333,-0.88477,-0.88620,-0.88760,-0.88901,-0.89041,-0.89182,-0.89319,-0.89456,-0.89594,-0.89731,-0.89865,-0.89999,-0.90131,-0.90265,-0.90396,-0.90527,-0.90656,-0.90787,-0.90915,-0.91040,-0.91168,-0.91293,-0.91418,-0.91544,-0.91666,-0.91788,-0.91910,-0.92029,-0.92148,-0.92267,
    -0.92386,-0.92502,-0.92618,-0.92734,-0.92847,-0.92963,-0.93073,-0.93185,-0.93295,-0.93405,-0.93515,-0.93625,-0.93732,-0.93839,-0.93942,-0.94049,-0.94153,-0.94254,-0.94357,-0.94458,-0.94559,-0.94656,-0.94757,-0.94855,-0.94949,-0.95047,-0.95142,-0.95233,-0.95328,-0.95419,-0.95511,-0.95602,-0.95691,-0.95779,-0.95868,-0.95953,-0.96039,-0.96124,-0.96210,-0.96292,-0.96375,-0.96457,-0.96536,-0.96616,-0.96695,-0.96771,-0.96851,-0.96924,-0.97000,-0.97073,-0.97147,-0.97220,-0.97290,-0.97360,-0.97430,-0.97501,-0.97568,-0.97635,-0.97699,-0.97766,-0.97830,-0.97891,-0.97955,-0.98016,-0.98074,-0.98135,-0.98193,-0.98251,-0.98306,-0.98364,-0.98419,-0.98471,-0.98526,-0.98578,-0.98627,-0.98679,-0.98727,-0.98776,-0.98822,-0.98868,-0.98914,-0.98959,-0.99002,-0.99045,-0.99088,-0.99127,-0.99167,-0.99207,-0.99246,-0.99283,-0.99319,-0.99353,-0.99387,-0.99420,-0.99454,-0.99484,-0.99515,-0.99545,-0.99573,-0.99600,-0.99628,-0.99655,-0.99680,-0.99704,-0.99725,-0.99747,-0.99768,-0.99789,-0.99808,-0.99826,-0.99844,-0.99860,-0.99878,-0.99890,-0.99905,-0.99918,-0.99930,-0.99939,-0.99951,-0.99960,-0.99966,-0.99973,-0.99979,-0.99985,-0.99991,-0.99994,-0.99994,-0.99997,
    -0.99997,-0.99997,-0.99994,-0.99994,-0.99991,-0.99985,-0.99979,-0.99973,-0.99966,-0.99960,-0.99951,-0.99939,-0.99930,-0.99918,-0.99905,-0.99890,-0.99878,-0.99860,-0.99844,-0.99826,-0.99808,-0.99789,-0.99768,-0.99747,-0.99725,-0.99704,-0.99680,-0.99655,-0.99628,-0.99600,-0.99573,-0.99545,-0.99515,-0.99484,-0.99454,-0.99420,-0.99387,-0.99353,-0.99319,-0.99283,-0.99246,-0.99207,-0.99167,-0.99127,-0.99088,-0.99045,-0.99002,-0.98959,-0.98914,-0.98868,-0.98822,-0.98776,-0.98727,-0.98679,-0.98627,-0.98578,-0.98526,-0.98471,-0.98419,-0.98364,-0.98306,-0.98251,-0.98193,-0.98135,-0.98074,-0.98016,-0.97955,-0.97891,-0.97830,-0.97766,-0.97699,-0.97635,-0.97568,-0.97501,-0.97430,-0.97360,-0.97290,-0.97220,-0.97147,-0.97073,-0.97000,-0.96924,-0.96851,-0.96771,-0.96695,-0.96616,-0.96536,-0.96457,-0.96375,-0.96292,-0.96210,-0.96124,-0.96039,-0.95953,-0.95868,-0.95779,-0.95691,-0.95602,-0.95511,-0.95419,-0.95328,-0.95233,-0.95142,-0.95047,-0.94949,-0.94855,-0.94757,-0.94656,-0.94559,-0.94458,-0.94357,-0.94254,-0.94153,-0.94049,-0.93942,-0.93839,-0.93732,-0.93625,-0.93515,-0.93405,-0.93295,-0.93185,-0.93073,-0.92963,-0.92847,-0.92734,-0.92618,-0.92502,
    -0.92386,-0.92267,-0.92148,-0.92029,-0.91910,-0.91788,-0.91666,-0.91544,-0.91418,-0.91293,-0.91168,-0.91040,-0.90915,-0.90787,-0.90656,-0.90527,-0.90396,-0.90265,-0.90131,-0.89999,-0.89865,-0.89731,-0.89594,-0.89456,-0.89319,-0.89182,-0.89041,-0.88901,-0.88760,-0.88620,-0.88477,-0.88333,-0.88190,-0.88043,-0.87900,-0.87753,-0.87604,-0.87457,-0.87308,-0.87158,-0.87006,-0.86853,-0.86703,-0.86548,-0.86395,-0.86240,-0.86084,-0.85928,-0.85770,-0.85611,-0.85452,-0.85294,-0.85132,-0.84970,-0.84808,-0.84647,-0.84482,-0.84317,-0.84152,-0.83987,-0.83820,-0.83652,-0.83484,-0.83313,-0.83145,-0.82974,-0.82803,-0.82629,-0.82455,-0.82281,-0.82108,-0.81934,-0.81757,-0.81580,-0.81403,-0.81223,-0.81042,-0.80862,-0.80682,-0.80499,-0.80319,-0.80136,-0.79950,-0.79767,-0.79581,-0.79395,-0.79208,-0.79019,-0.78833,-0.78644,-0.78455,-0.78262,-0.78070,-0.77878,-0.77686,-0.77493,-0.77298,-0.77103,-0.76907,-0.76712,-0.76514,-0.76315,-0.76117,-0.75919,-0.75717,-0.75519,-0.75317,-0.75113,-0.74911,-0.74707,-0.74503,-0.74298,-0.74094,-0.73886,-0.73679,-0.73471,-0.73264,-0.73053,-0.72845,-0.72635,-0.72421,-0.72211,-0.71997,-0.71783,-0.71570,-0.71356,-0.71140,-0.70926,
    -0.70709,-0.70493,-0.70273,-0.70056,-0.69836,-0.69617,-0.69394,-0.69174,-0.68951,-0.68729,-0.68506,-0.68283,-0.68057,-0.67831,-0.67609,-0.67380,-0.67154,-0.66925,-0.66699,-0.66470,-0.66238,-0.66010,-0.65778,-0.65549,-0.65317,-0.65082,-0.64850,-0.64615,-0.64380,-0.64145,-0.63910,-0.63675,-0.63437,-0.63199,-0.62961,-0.62723,-0.62485,-0.62244,-0.62003,-0.61761,-0.61520,-0.61279,-0.61035,-0.60794,-0.60550,-0.60306,-0.60059,-0.59814,-0.59567,-0.59320,-0.59073,-0.58826,-0.58578,-0.58328,-0.58081,-0.57831,-0.57581,-0.57327,-0.57077,-0.56824,-0.56570,-0.56317,-0.56064,-0.55811,-0.55554,-0.55301,-0.55045,-0.54788,-0.54532,-0.54272,-0.54016,-0.53757,-0.53497,-0.53238,-0.52979,-0.52719,-0.52457,-0.52197,-0.51935,-0.51672,-0.51410,-0.51144,-0.50882,-0.50616,-0.50351,-0.50089,-0.49820,-0.49554,-0.49289,-0.49020,-0.48755,-0.48486,-0.48218,-0.47949,-0.47678,-0.47409,-0.47137,-0.46869,-0.46597,-0.46326,-0.46051,-0.45779,-0.45508,-0.45233,-0.44958,-0.44687,-0.44412,-0.44135,-0.43860,-0.43585,-0.43307,-0.43033,-0.42755,-0.42477,-0.42200,-0.41919,-0.41641,-0.41364,-0.41083,-0.40802,-0.40524,-0.40244,-0.39960,-0.39679,-0.39398,-0.39114,-0.38834,-0.38550,
    -0.38266,-0.37982,-0.37698,-0.37415,-0.37131,-0.36847,-0.36560,-0.36273,-0.35989,-0.35703,-0.35416,-0.35129,-0.34842,-0.34552,-0.34265,-0.33975,-0.33688,-0.33398,-0.33109,-0.32819,-0.32529,-0.32239,-0.31949,-0.31659,-0.31366,-0.31076,-0.30783,-0.30493,-0.30200,-0.29907,-0.29614,-0.29321,-0.29028,-0.28735,-0.28439,-0.28146,-0.27850,-0.27557,-0.27261,-0.26965,-0.26669,-0.26373,-0.26077,-0.25781,-0.25485,-0.25189,-0.24893,-0.24594,-0.24298,-0.23999,-0.23703,-0.23404,-0.23105,-0.22806,-0.22507,-0.22208,-0.21909,-0.21609,-0.21310,-0.21011,-0.20709,-0.20410,-0.20111,-0.19809,-0.19510,-0.19208,-0.18906,-0.18604,-0.18304,-0.18002,-0.17700,-0.17398,-0.17096,-0.16794,-0.16492,-0.16190,-0.15884,-0.15582,-0.15280,-0.14975,-0.14673,-0.14368,-0.14066,-0.13760,-0.13458,-0.13153,-0.12848,-0.12546,-0.12241,-0.11935,-0.11630,-0.11328,-0.11023,-0.10718,-0.10413,-0.10107,-0.09802,-0.09497,-0.09192,-0.08884,-0.08578,-0.08273,-0.07968,-0.07663,-0.07355,-0.07050,-0.06744,-0.06439,-0.06131,-0.05826,-0.05521,-0.05212,-0.04907,-0.04599,-0.04294,-0.03989,-0.03680,-0.03375,-0.03067,-0.02762,-0.02454,-0.02148,-0.01840,-0.01535,-0.01227,-0.00922,-0.00613,-0.00308

};

/* ************************************************************************************
 *                             WESPION Motor Control Function                         *
 * ************************************************************************************/
int Debug_MotorErrChk = 0;
int Cnt_MtrCtrl_Alternating = 0;
int Cnt_Motor1_CurrCtrl, Cnt_Motor2_CurrCtrl;
int Check_MtrErrChk = 0;
void WESPION_MotorControl(void) // It's in ADC_IRQHandler Function
{   // No control 14.2us, 1 motor control only 10us


//	digitalWrite(57, 1);
//	digitalWrite(87, 1);
	InvAdcGet();
//	digitalWrite(57, 0);
//	digitalWrite(87, 1);
	MotorCtrl_FeedbackCal();
//	digitalWrite(57, 0);
//	digitalWrite(87, 0);
//	digitalWrite(87, 1);
	if (Debug_MotorErrChk ==1) {
		if(WP_Ctrl.AdOffsetFlg==1)
		{
			Check_MtrErrChk++;
			MotorControl_ErrChk(&WP_CtrlErr.Motor1, &WP_ErrRef, &Motor1.Fb, &Motor1_Ang, &Inv.Ctrl.AD1);
			MotorControl_ErrChk(&WP_CtrlErr.Motor2, &WP_ErrRef, &Motor2.Fb, &Motor2_Ang, &Inv.Ctrl.AD2);
			Controller_ErrChk(&WP_CtrlErr, &WP_ErrRef, &Inv.Ctrl);
		}
	}
//    digitalWrite(87, 0);
//	digitalWrite(88, 1);
	MotorCtrl_StateMachine();       //PI
//	Debug_MtrCtrl_Alternating++;
//	digitalWrite(88, 0);

}

inline void WESPION_MotorAddon(void) // It's in main while loop
{
  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q);
  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q);
}

/*==========================================================================================================*/
inline void MotorCtrl_StateMachine(void)
{
  switch(WP_Ctrl.Mode)
  {
    case Inv_Off:
      if(WP_Ctrl.AdOffsetFlg==1)
      {
        WP_Ctrl.Mode = Inv_Init;
      }
      else{}
//      Motor1.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
//      Motor1.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
//      Motor1.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;
//      Motor2.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
//      Motor2.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
//      Motor2.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;
//      WP_CtrlErr.Motor1.PWM = WP_PWMout(MOTOR1, Motor1.Fb.PWMcnt);
//      WP_CtrlErr.Motor2.PWM = WP_PWMout(MOTOR2, Motor2.Fb.PWMcnt);
//      Motor1En(WP_Gate_EN);
//      Motor2En(WP_Gate_EN);
//      Motor1Ctrl_PwmStart();
//      Motor2Ctrl_PwmStart();
      Motor1En(WP_Gate_DIS);
      Motor2En(WP_Gate_DIS);
//      Motor1Ctrl_PwmStop();
//      Motor2Ctrl_PwmStop();
      WP_Ctrl.ActiveMotor = Drv_NoMotor;

      break;
    case Inv_Init:      // 1번 모드 - 초기화
      Motor1En(WP_Gate_DIS);
      Motor2En(WP_Gate_DIS);
//      Motor1Ctrl_PwmStop();
//      Motor2Ctrl_PwmStop();
      WP_Ctrl.ActiveMotor = Drv_NoMotor;
      Motor1Ctrl_VariSetZero();
      Motor2Ctrl_VariSetZero();
      WP_Ctrl.Mode = Inv_Ready;
    break;
    case Inv_Ready:     // 2번 모드 - 구동 대기
      Motor1Ctrl_VariSetZero();
      Motor2Ctrl_VariSetZero();
      WP_Ctrl.ActiveMotor = Drv_NoMotor;
      break;
    case Inv_VbyFCtrl:    // 3번 모드 - V by F

    case Inv_IbyFCtrl:    // 4번 모드 - I by F

    case Inv_TorqueCtrl:  // 5번 모드 - FOC !!

    case Inv_TorqueWeakCtrl:  // 6번 모드 - FOC with field weak control !!
      MotorCurControl();
      break;
    case Inv_SpeedCtrl:   // 7번 모드 - 속도 제어
      MotorSpdControl();
      break;
    case Inv_Fault:     // 8번 모드 - 에러 발생 시 진입
      WP_Ctrl.ActiveMotor = Drv_NoMotor;
//      Motor1Ctrl_PwmStop();
//      Motor2Ctrl_PwmStop();
      Motor1En(WP_Gate_DIS);
      Motor2En(WP_Gate_DIS);
      Motor1Ctrl_VariSetZero();
      Motor2Ctrl_VariSetZero();
      break;
    default:
      Motor1En(WP_Gate_DIS);
      Motor2En(WP_Gate_DIS);
//      Motor1Ctrl_PwmStop();
//      Motor2Ctrl_PwmStop();
      WP_Ctrl.Mode = Inv_Off;
      break;
  }
}

inline void MotorControl_ErrChk(WP_MotorControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_MotorAxisTypeDef *_fb, WP_MotorAngleTypeDef *_ang, WP_InverterAdOffsetTypeDef *_off)
{
  uint32_t error_flag = 0x00;

  if(_off->iRaw.U > _errRef->IsensorOpen)     //3908.0f;  // 50A에 해당하는 값
  {
    _err->CurrentSensorOpenU = WP_ERR;
    Bit_Set(error_flag, ERROR_U_I_SENSOR_OPEN);
  }
  if(_off->iRaw.V > _errRef->IsensorOpen)
  {
    _err->CurrentSensorOpenV = WP_ERR;
    Bit_Set(error_flag, ERROR_V_I_SENSOR_OPEN);
  }
  if(_off->iRaw.W > _errRef->IsensorOpen)
  {
    _err->CurrentSensorOpenW = WP_ERR;
    Bit_Set(error_flag, ERROR_W_I_SENSOR_OPEN);
  }
  if(_off->iRaw.U < _errRef->IsensorShort)     // 186.0f;  // -50A에 해당하는 값
  {
    _err->CurrentSensorShortU = WP_ERR;
    Bit_Set(error_flag, ERROR_U_I_SENSOR_SHORT);
  }
  if(_off->iRaw.V < _errRef->IsensorShort)
  {
    _err->CurrentSensorShortV = WP_ERR;
    Bit_Set(error_flag, ERROR_V_I_SENSOR_SHORT);
  }
  if(_off->iRaw.W < _errRef->IsensorShort)
  {
    _err->CurrentSensorShortW = WP_ERR;
    Bit_Set(error_flag, ERROR_W_I_SENSOR_SHORT);
  }

  if(_off->iOffset.U < _errRef->IsensorOffsetMin)
  {
    _err->CurrentSensorOffsetU = WP_ERR;
    Bit_Set(error_flag, ERROR_U_I_SENSOR_OFFSET_MIN);
  }
  else if(_off->iOffset.U > _errRef->IsensorOffsetMax)
  {
    _err->CurrentSensorOffsetU = WP_ERR;
    Bit_Set(error_flag, ERROR_U_I_SENSOR_OFFSET_MAX);
  }
  if(_off->iOffset.V < _errRef->IsensorOffsetMin)
  {
    _err->CurrentSensorOffsetV = WP_ERR;
    Bit_Set(error_flag, ERROR_V_I_SENSOR_OFFSET_MIN);
  }
  else if(_off->iOffset.V > _errRef->IsensorOffsetMax)
  {
    _err->CurrentSensorOffsetV = WP_ERR;
    Bit_Set(error_flag, ERROR_V_I_SENSOR_OFFSET_MAX);
  }
  if(_off->iOffset.W < _errRef->IsensorOffsetMin)
  {
    _err->CurrentSensorOffsetW = WP_ERR;
    Bit_Set(error_flag, ERROR_W_I_SENSOR_OFFSET_MIN);
  }
  else if(_off->iOffset.W > _errRef->IsensorOffsetMax)
  {
    _err->CurrentSensorOffsetW = WP_ERR;
    Bit_Set(error_flag, ERROR_W_I_SENSOR_OFFSET_MAX);
  }

  if(Abs(_fb->I3.U) > _errRef->OverCurrent)
  {
    _err->OverCurrentU = WP_ERR;
    Bit_Set(error_flag, ERROR_U_I_SENSOR_OVER);
  }
  if(Abs(_fb->I3.V) > _errRef->OverCurrent)
  {
    _err->OverCurrentV = WP_ERR;
    Bit_Set(error_flag, ERROR_V_I_SENSOR_OVER);
  }
  if(Abs(_fb->I3.W) > _errRef->OverCurrent)
  {
    _err->OverCurrentW = WP_ERR;
    Bit_Set(error_flag, ERROR_W_I_SENSOR_OVER);
  }
  if(Abs(_fb->Ie.d) > _errRef->OverCurrent)
  {
    _err->OverCurrentD = WP_ERR;
    Bit_Set(error_flag, ERROR_D_I_OVER);
  }
  if(Abs(_fb->Ie.q) > _errRef->OverCurrent)
  {
    _err->OverCurrentQ = WP_ERR;
    Bit_Set(error_flag, ERROR_D_I_OVER);
  }

  if(Abs(_ang->RpmFil) > _errRef->OverSpeed)
  {
    _err->OverSpd = WP_ERR;
    Bit_Set(error_flag, ERROR_SPEED_OVER);
  }

  error_flag |= (Error_Flag & 0xFF000000);

  if(Error_Flag != error_flag) {
    Error_Flag = error_flag;

    if(Error_Flag_Enable)     Return_4byte(FAIL_SAFE_ERROR, Error_Flag);
  }
}

inline void Controller_ErrChk(WP_ControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_InverterControlTypeDef *_inv)
{
  uint32_t error_flag = 0x00;

  if(_inv->VdcFil > _errRef->OverVoltage)
  {
    _err->OverV = WP_ERR;
    Bit_Set(error_flag, ERROR_VOLT_OVER);
  }
  if(_inv->VdcFil < _errRef->UnderVoltage)
  {
    _err->UnderV = WP_ERR;
    Bit_Set(error_flag, ERROR_VOLT_UNDER);
  }

  error_flag |= (Error_Flag & 0x00FFFFFF);

  if(Error_Flag != error_flag) {
    Error_Flag = error_flag;

    if(Error_Flag_Enable)     Return_4byte(FAIL_SAFE_ERROR, Error_Flag);
  }
}

inline void MotorCurControl(void)
{

  switch(WP_Ctrl.ActiveMotor&0x1)   // Motor1 Test
  {
    case WP_RUN:
//      Motor1Ctrl_PwmStart();    // in main.c
      if (MotorCurrCtrl_Alternating) {
        if ((Cnt_MtrCtrl_Alternating+1) % 2) {
//          DAC1_SetValue(3000);
          Motor1CurCtrl();
//          DAC1_SetValue(0);
          Cnt_Motor1_CurrCtrl++;
        }
      }else{
        Motor1CurCtrl();
        Cnt_Motor1_CurrCtrl++;
      }
      Motor1En(WP_Gate_EN);
      break;
    case WP_STOP:
      Motor1Ctrl_VariSetZero();
      Motor1En(WP_Gate_DIS);
      //      Motor1Ctrl_PwmStop();
      break;
    default:
      Motor1Ctrl_VariSetZero();
      Motor1En(WP_Gate_DIS);
      //      Motor1Ctrl_PwmStop();
      break;
  }
  switch((WP_Ctrl.ActiveMotor>>1)&0x1)  // Motor2 Test
  {
    case WP_RUN:
//      Motor2Ctrl_PwmStart();    // in main.c
      if(MotorCurrCtrl_Alternating){
        if (Cnt_MtrCtrl_Alternating % 2) {
//          DAC2_SetValue(3000);
          Motor2CurCtrl();
//          DAC2_SetValue(0);
          Cnt_Motor2_CurrCtrl++;
        }
      } else {
        Motor2CurCtrl();
        Cnt_Motor2_CurrCtrl++;
      }

      Motor2En(WP_Gate_EN);
      break;
    case WP_STOP:
      Motor2Ctrl_VariSetZero();
      Motor2En(WP_Gate_DIS);
      //      Motor2Ctrl_PwmStop();
      break;
    default:
      Motor2Ctrl_VariSetZero();
      Motor2En(WP_Gate_DIS);
      //      Motor2Ctrl_PwmStop();
      break;
  }

  Cnt_MtrCtrl_Alternating++;

}


inline void Motor1CurCtrl(void)
{

  WP_RampUpDn(&Motor1.Cmd.Icmd.q, &Motor1.Cmd.Iref.q, 1.0f, 1.0f);
  if(WP_Ctrl.Mode==Inv_TorqueWeakCtrl)
  {
#if FIELDWEAK_MODE == FIELDWEAK_MOT
    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor1.Fb.VphaseFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);
#endif
#if FIELDWEAK_MODE == FIELDWEAK_GEN
    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);
#endif
  }
  else
  {
    WP_RampUpDn(&Motor1.Cmd.Icmd.d, &Motor1.Cmd.Iref.d, 1.0f, 1.0f);
  }
  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_Weight.FfScale.Temp[L];
//  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_Weight.FfScale.Temp[MOTOR_L];// + Motor1.Para.Ld * Motor1.Fb.Ie.d * Motor1_Ang.WeFil;
//  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_WeightFeel.FfScale_L;
//  Motor1.PiCurD.Ff = - Motor1.Para.Lq * Motor1.Fb.Ie.q * Motor1_Ang.WeFil;

//  Motor1.Fb.VeRef.q = WP_PIcontrol_Spdlim(Motor1.Cmd.Iref.q, Motor1.Fb.Ie.q, &Motor1.PiCurQ, &Motor1.GainCurQ, Inv.Ctrl.VphaseLim, Motor1_Ang.RpmFil);///somebp 2024.02.02
  Motor1.Fb.VeRef.q = WP_PIcontrol(Motor1.Cmd.Iref.q, Motor1.Fb.Ie.q, &Motor1.PiCurQ, &Motor1.GainCurQ, Inv.Ctrl.VphaseLim);                          ///origin
  Motor1.Fb.VeRef.d = WP_PIcontrol(Motor1.Cmd.Iref.d, Motor1.Fb.Ie.d, &Motor1.PiCurD, &Motor1.GainCurD, Inv.Ctrl.VphaseLim);
  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q); // 700ns
  LPF(Motor1.Fb.VphaseFil, Motor1.Fb.VphaseRef, LpfVdc.fct);
  Motor1.Fb.VphaseSet = 1.0f / Motor1.Fb.VphaseFil;   // 역수 취한 값
  switch(WP_Ctrl.Mode)
  {
    case Inv_VbyFCtrl:
      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Cmd.Vfcmd, &Motor1_Ang);
      break;
    default:
      if(Motor1.Fb.VphaseFil > Inv.Ctrl.VphaseLim)
      {
        Motor1.Fb.VeSet.d = Motor1.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;
        Motor1.Fb.VeSet.q = Motor1.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;
      }
      else
      {
        Motor1.Fb.VeSet.d = Motor1.Fb.VeRef.d;
        Motor1.Fb.VeSet.q = Motor1.Fb.VeRef.q;
      }
      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Fb.VeSet, &Motor1_Ang);
      break;
  }
  /* Limit Function Add */
  WP_InvMinMax(&Motor1.Fb.V3Set, &Motor1.Fb.V3, Inv.Ctrl.VphLim);
  WP_ClarkeParkTrans(&Motor1.Fb.V3, &Motor1.Fb.Vs, &Motor1.Fb.Ve, &Motor1_Ang);
  Motor1.PiCurQ.AW = Motor1.Fb.VeRef.q - Motor1.Fb.Ve.q;
  Motor1.PiCurD.AW = Motor1.Fb.VeRef.d - Motor1.Fb.Ve.d;
  WP_PwmScale(Motor1.Fb.PWMcnt, &Motor1.Fb.V3, Motor1.Fb.Dutyratio, &Inv);
  WP_CtrlErr.Motor1.PWM = WP_PWMout(MOTOR1, Motor1.Fb.PWMcnt);

}

inline void Motor2CurCtrl(void)
{

  WP_RampUpDn(&Motor2.Cmd.Icmd.q, &Motor2.Cmd.Iref.q, 1.0f, 1.0f);
  if(WP_Ctrl.Mode==Inv_TorqueWeakCtrl)
  {
#if FIELDWEAK_MODE == FIELDWEAK_MOT
	  Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor2.Fb.VphaseFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);
#endif
#if FIELDWEAK_MODE == FIELDWEAK_GEN
	  Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);
#endif
  }
  else
  {
    WP_RampUpDn(&Motor2.Cmd.Icmd.d, &Motor2.Cmd.Iref.d, 1.0f, 1.0f);
  }
  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_Weight.FfScale.Temp[R];
//  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_Weight.FfScale.Temp[MOTOR_R];// + Motor2.Para.Ld * Motor2.Fb.Ie.d * Motor2_Ang.WeFil;
//  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_WeightFeel.FfScale_R;
//  Motor2.PiCurD.Ff = - Motor2.Para.Lq * Motor2.Fb.Ie.q * Motor2_Ang.WeFil;

//  Motor2.Fb.VeRef.q = WP_PIcontrol_Spdlim(Motor2.Cmd.Iref.q, Motor2.Fb.Ie.q, &Motor2.PiCurQ, &Motor2.GainCurQ, Inv.Ctrl.VphaseLim, Motor2_Ang.RpmFil);///somebp 2024.02.02
  Motor2.Fb.VeRef.q = WP_PIcontrol(Motor2.Cmd.Iref.q, Motor2.Fb.Ie.q, &Motor2.PiCurQ, &Motor2.GainCurQ, Inv.Ctrl.VphaseLim);                          ///origin
  Motor2.Fb.VeRef.d = WP_PIcontrol(Motor2.Cmd.Iref.d, Motor2.Fb.Ie.d, &Motor2.PiCurD, &Motor2.GainCurD, Inv.Ctrl.VphaseLim);
  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q); // 700ns
  LPF(Motor2.Fb.VphaseFil, Motor2.Fb.VphaseRef, LpfVdc.fct);
  Motor2.Fb.VphaseSet = 1.0f / Motor2.Fb.VphaseFil;
  switch(WP_Ctrl.Mode)
  {
    case Inv_VbyFCtrl:
      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Cmd.Vfcmd, &Motor2_Ang);
      break;
    default:
      if(Motor2.Fb.VphaseFil > Inv.Ctrl.VphaseLim)
      {
    	  Motor2.Fb.VeSet.d = Motor2.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;
          Motor2.Fb.VeSet.q = Motor2.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;
      }
      else
      {
        Motor2.Fb.VeSet.d = Motor2.Fb.VeRef.d;
        Motor2.Fb.VeSet.q = Motor2.Fb.VeRef.q;
      }
      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Fb.VeSet, &Motor2_Ang);
      break;
  }
  /* Limit Function Add */
  WP_InvMinMax(&Motor2.Fb.V3Set, &Motor2.Fb.V3, Inv.Ctrl.VphLim);
  WP_ClarkeParkTrans(&Motor2.Fb.V3, &Motor2.Fb.Vs, &Motor2.Fb.Ve, &Motor2_Ang);
  Motor2.PiCurQ.AW = Motor2.Fb.VeRef.q - Motor2.Fb.Ve.q;
  Motor2.PiCurD.AW = Motor2.Fb.VeRef.d - Motor2.Fb.Ve.d;
  WP_PwmScale(Motor2.Fb.PWMcnt, &Motor2.Fb.V3, Motor2.Fb.Dutyratio, &Inv);
  WP_CtrlErr.Motor2.PWM = WP_PWMout(MOTOR2, Motor2.Fb.PWMcnt);

}

inline void MotorSpdControl(void)
{
  switch(WP_Ctrl.ActiveMotor&0x1)   // Motor1 Test
  {
    case WP_RUN:
//      Motor1Ctrl_PwmStart();
      if(Motor1_Ang._1msFlg == 1)
      {
        Motor1SpdCtrl();
        Motor1_Ang._1msFlg = 0;
      }
      Motor1CurCtrl();
      Motor1En(WP_Gate_EN);
      break;
    case WP_STOP:
      Motor1Ctrl_VariSetZero();
      Motor1En(WP_Gate_DIS);
//      Motor1Ctrl_PwmStop();
      break;
    default:
      Motor1Ctrl_VariSetZero();
      Motor1En(WP_Gate_DIS);
//      Motor1Ctrl_PwmStop();
      break;
  }
  switch((WP_Ctrl.ActiveMotor>>1)&0x1)  // Motor2 Test
  {
    case WP_RUN:
//      Motor2Ctrl_PwmStart();
      if(Motor2_Ang._1msFlg == 1)
      {
        Motor2SpdCtrl();
        Motor2_Ang._1msFlg = 0;
      }
      Motor2CurCtrl();
      Motor2En(WP_Gate_EN);
      break;
    case WP_STOP:
      Motor2Ctrl_VariSetZero();
      Motor2En(WP_Gate_DIS);
//      Motor2Ctrl_PwmStop();
      break;
    default:
      Motor2Ctrl_VariSetZero();
      Motor2En(WP_Gate_DIS);
//      Motor2Ctrl_PwmStop();
      break;
  }
}

inline void Motor1SpdCtrl(void)
{
  WP_RampUpDn(&Motor1.Cmd.RpmCmd, &Motor1.Cmd.RpmRef, 10.0f, 10.0f);
  Motor1.Cmd.Icmd.q = WP_PIcontrol(Motor1.Cmd.RpmRef, Motor1_Ang.RpmFil, &Motor1.PiSpd, &Motor1.GainSpd, Motor1_Ang.SpdCtrlOutLim);
}

inline void Motor2SpdCtrl(void)
{
  WP_RampUpDn(&Motor2.Cmd.RpmCmd, &Motor2.Cmd.RpmRef, 10.0f, 10.0f);
  Motor2.Cmd.Icmd.q = WP_PIcontrol(Motor2.Cmd.RpmRef, Motor2_Ang.RpmFil, &Motor2.PiSpd, &Motor2.GainSpd, Motor2_Ang.SpdCtrlOutLim);
}

/*==========================================================================================================*/
inline void InvAdcGet(void)
{
  ADinj.Raw[0][0] = ADC_Inject[0][0]; // AD_IU0;
  ADinj.Raw[0][1] = ADC_Inject[0][1]; // AD_IU1;
  ADinj.Raw[0][2] = ADC_Inject[0][2]; // AD_SIN0;
  ADinj.Raw[0][3] = ADC_Inject[0][3]; // AD_SIN1;
  ADinj.Raw[1][0] = ADC_Inject[1][0]; // AD_IV0;
  ADinj.Raw[1][1] = ADC_Inject[1][1]; // AD_IV1;
  ADinj.Raw[1][2] = ADC_Inject[1][2]; // AD_COS0;
  ADinj.Raw[1][3] = ADC_Inject[1][3]; // AD_COS1;
  ADinj.Raw[2][0] = ADC_Inject[2][0]; // AD_IW0;
  ADinj.Raw[2][1] = ADC_Inject[2][1]; // AD_IW1;
  ADinj.Raw[2][2] = ADC_Inject[2][2]; // AD_VDC;
  ADinj.Raw[2][3] = ADC_Inject[2][3]; // AD_EXT0;
}

inline void MotorCtrl_FeedbackCal(void)
{

//  digitalWrite(57, 1);
  Inv.Ctrl.Vdc = ADinj.Raw[2][2] * Inv.Para.VdcScale;

  Inv.Ctrl.AD1.iRaw.U = ADinj.Raw[0][0];
  Inv.Ctrl.AD2.iRaw.U = ADinj.Raw[0][1];
  Inv.Ctrl.AD1.iRaw.V = ADinj.Raw[1][0];
  Inv.Ctrl.AD2.iRaw.V = ADinj.Raw[1][1];
  Inv.Ctrl.AD1.iRaw.W = ADinj.Raw[2][0];
  Inv.Ctrl.AD2.iRaw.W = ADinj.Raw[2][1];

  switch(WP_Ctrl.AdOffsetFlg)
  {
    case WP_STOP:
      WP_Ctrl.AdOffsetCnt++;
      if(WP_Ctrl.AdOffsetCnt > 5000)
      {
        WP_Ctrl.AdOffsetFlg = WP_RUN;
        WP_Ctrl.AdOffsetCnt=5001;
      }
      else
      {
        WP_Ctrl.Mode=0;
        WP_Ctrl.ActiveMotor=0;
        WP_Ctrl.AdOffsetFlg=0;
        LPF(Inv.Ctrl.AD1.iOffset.U, Inv.Ctrl.AD1.iRaw.U, LpfCur.fct);
        LPF(Inv.Ctrl.AD1.iOffset.V, Inv.Ctrl.AD1.iRaw.V, LpfCur.fct);
        LPF(Inv.Ctrl.AD1.iOffset.W, Inv.Ctrl.AD1.iRaw.W, LpfCur.fct);
        LPF(Inv.Ctrl.AD2.iOffset.U, Inv.Ctrl.AD2.iRaw.U, LpfCur.fct);
        LPF(Inv.Ctrl.AD2.iOffset.V, Inv.Ctrl.AD2.iRaw.V, LpfCur.fct);
        LPF(Inv.Ctrl.AD2.iOffset.W, Inv.Ctrl.AD2.iRaw.W, LpfCur.fct);
      }
      break;
    case WP_RUN:

      break;
    default:
      WP_CtrlErr.AdOffset = WP_ERR;
  }

  Motor1.Fb.I3.U = (Inv.Ctrl.AD1.iRaw.U - Inv.Ctrl.AD1.iOffset.U) * Inv.Para.iScale;
  Motor1.Fb.I3.V = (Inv.Ctrl.AD1.iRaw.V - Inv.Ctrl.AD1.iOffset.V) * Inv.Para.iScale;
  Motor1.Fb.I3.W = (Inv.Ctrl.AD1.iRaw.W - Inv.Ctrl.AD1.iOffset.W) * Inv.Para.iScale;
//  Motor1.Fb.I3.W = - Motor1.Fb.I3.U - Motor1.Fb.I3.V;
  Motor2.Fb.I3.U = (Inv.Ctrl.AD2.iRaw.U - Inv.Ctrl.AD2.iOffset.U) * Inv.Para.iScale;
  Motor2.Fb.I3.V = (Inv.Ctrl.AD2.iRaw.V - Inv.Ctrl.AD2.iOffset.V) * Inv.Para.iScale;
  Motor2.Fb.I3.W = (Inv.Ctrl.AD2.iRaw.W - Inv.Ctrl.AD2.iOffset.W) * Inv.Para.iScale;
//  Motor2.Fb.I3.W = - Motor2.Fb.I3.U - Motor2.Fb.I3.V;

  Motor1_Ang.MR.Raw.Sin = ADinj.Raw[0][2]; // AD_SIN0;
  Motor2_Ang.MR.Raw.Sin = ADinj.Raw[0][3]; // AD_SIN1;
  Motor1_Ang.MR.Raw.Cos = ADinj.Raw[1][2]; // AD_COS0;
  Motor2_Ang.MR.Raw.Cos = ADinj.Raw[1][3]; // AD_COS1;

//  digitalWrite(57, 0);
//  digitalWrite(87, 1);
#if ANGLE_MODE == ANGLE_MR    // 누적앵글 구하는 코드 없는 상태
  WP_MrPos(&Motor1_Ang);
  WP_MrPos(&Motor2_Ang);
#endif
#if ANGLE_MODE == ANGLE_ENC
  WP_CtrlErr.Motor1.Speed = WP_EncSpd(MOTOR1, &Motor1_Ang);
  WP_CtrlErr.Motor2.Speed = WP_EncSpd(MOTOR2, &Motor2_Ang);
#endif

//  digitalWrite(87, 0);
//  digitalWrite(88, 1);
  LPF(Inv.Ctrl.VdcFil, Inv.Ctrl.Vdc, LpfVdc.fct);

  Inv.Ctrl.VphaseLim = Inv.Ctrl.VdcFil * MT_SQ3_OVR_2 * Inv.Para.VdcMargin;
  Inv.Ctrl.VlimFweak = Inv.Ctrl.VphaseLim * 0.8f;
  Inv.Ctrl.Vdc_2 = Inv.Ctrl.VdcFil * MT_1_OVR_2;
  Inv.Ctrl.VphLim = Inv.Ctrl.Vdc_2 * Inv.Para.VdcMargin;
  Inv.Ctrl._Vdc_2 = 1.0f / Inv.Ctrl.Vdc_2;
  Inv.Ctrl.PWM_Scale = Inv.Para.PWM_Mid * Inv.Ctrl._Vdc_2;

  MotorCtrl_TransformAngle(&Motor1_Ang, Motor1.Cmd.XbyF_Freq, Inv.Para.Tsamp);
  WP_ClarkeParkTrans(&Motor1.Fb.I3, &Motor1.Fb.Is, &Motor1.Fb.Ie, &Motor1_Ang);
  MotorCtrl_TransformAngle(&Motor2_Ang, Motor2.Cmd.XbyF_Freq, Inv.Para.Tsamp);
  WP_ClarkeParkTrans(&Motor2.Fb.I3, &Motor2.Fb.Is, &Motor2.Fb.Ie, &Motor2_Ang);

  Inverter_Comm1ms();
//  digitalWrite(88, 0);
}

inline void MotorCtrl_TransformAngle(WP_MotorAngleTypeDef *angle, float cmd, float tsamp)
{
  uint32_t array_theta=0;

  switch(WP_Ctrl.Mode)
  {
    case Inv_IbyFCtrl:    // 3번 모드 - I by F
    case Inv_VbyFCtrl:    // 4번 모드 - V by F
      MotorCtrl_XbyF_Ctrl(cmd, &angle->XbyFAngle, tsamp);
      array_theta = (uint32_t)(2048*(angle->XbyFAngle)*MT_1_OVR_360);
      break;
    default:
      array_theta = (uint32_t)(2048*(angle->AngleElec)*MT_1_OVR_360);
      break;
  }

  angle->Tr_Sin = sinTab[((array_theta    )&0x7FF)];
  angle->Tr_Cos = sinTab[((array_theta+511)&0x7FF)];
}

inline void MotorCtrl_XbyF_Ctrl(float fc, float *angle, float SampT)
{
  *angle += fc * SampT * 360.0f;//fcmd*0.00001*2*3.141592;

  if(*angle >= 360.0f)   *angle -= 360.0f;
  else if(*angle < 0)    *angle += 360.0f;
}

/*==========================================================================================================*/
void Inverter_Comm(void)
{
  if(ComAd.Flg==WP_RUN)
  {
    InvComAdcGet();
    Inverter_CommDataCal();
    ComAd.Flg=WP_STOP;
    ADC_SWStart();                      //Start Trigger
  }
}

inline void Inverter_Comm1ms(void)
{
  static uint16_t _1msCnt;
#if   defined(TIM_8KHZ)
  uint8_t CntRef=8;
#elif defined(TIM_10KHZ)
  uint8_t CntRef=10;
#elif defined(TIM_16KHZ)
  uint8_t CntRef=16;
#elif defined(TIM_20KHZ)
  uint8_t CntRef=20;
#else
  uint8_t CntRef=10;
#endif

  _1msCnt++;
  if(_1msCnt > CntRef)
  {
    Inverter_CommEnable();
    _1msCnt=0;
  }
  else{}
}

inline void Inverter_CommEnable(void)
{ // This Function Run in "HAL_ADC_ConvCpltCallback" in adc.c
  ComAd.Flg=WP_RUN;
}

inline void InvComAdcGet(void)
{
  ComAd.Raw.VU0 = ADC_Buffer[0];
  ComAd.Raw.VU1 = ADC_Buffer[1];
  ComAd.Raw.NTC0 = ADC_Buffer[2];
  ComAd.Raw.NTC1 = ADC_Buffer[3];
  ComAd.Raw.EXT1 = ADC_Buffer[4];
  ComAd.Raw.EXT2 = ADC_Buffer[5];
  ComAd.Raw.EXT3 = ADC_Buffer[6];
  ComAd.Raw.EXT4 = ADC_Buffer[7];
}

inline void Inverter_CommDataCal(void)
{
  ComAd.Data.VU0 = ComAd.Raw.VU0 * Inv.Para.HwAdcScale;
  ComAd.Data.VU1 = ComAd.Raw.VU1 * Inv.Para.HwAdcScale;
  ComAd.Data.NTC0 = ComAd.Raw.NTC0 * Inv.Para.HwAdcScale;
  ComAd.Data.NTC1 = ComAd.Raw.NTC1 * Inv.Para.HwAdcScale;
  ComAd.Data.EXT1 = ComAd.Raw.EXT1 * Inv.Para.HwAdcScale;
  ComAd.Data.EXT2 = ComAd.Raw.EXT2 * Inv.Para.HwAdcScale;
  ComAd.Data.EXT3 = ComAd.Raw.EXT3 * Inv.Para.HwAdcScale;
  ComAd.Data.EXT4 = ComAd.Raw.EXT4 * Inv.Para.HwAdcScale;
}

/*==========================================================================================================*/
void MotorCtrl_Initialize(void)
{
  // Motor Parameters
  // Gate IC DISABLE Pin Settings
  pinMode(1, OUTPUT);  // PWM0_EN
  pinMode(2, OUTPUT);  // PWM1_EN
  Motor1En(WP_Gate_DIS);
  Motor2En(WP_Gate_DIS);

//  kpQ=3000.*motorLq;
//  kiQ=3000.*motorR*tsamp;

  // Motor1 Parameter Settings
  //Motor1.Para.Pole = 10;     // SAT : 4, JKONG : 8, BS-105-10 : 30
  //Motor1.Para.Pole = 8;
  Motor1.Para.Pole = RAM_Data.Motor_Para_Pole;        ///MOTOR_PARA_POLE

  Motor1.Para.PolePair = Motor1.Para.Pole>>1;
  //Motor1.Para.Ld = 0.0f;
  //Motor1.Para.Lq = 0.0f;
  //Motor1.Para.Rs = 0.0f;
  //Motor1.Para.PhaiF = 72.2f * 0.001f * MT_1_OVR_SQ3;  // V/krpm
  //Motor1.Para.PhaiF = 25.3f * 0.001f * MT_1_OVR_SQ3;
  Motor1.Para.Ld = RAM_Data.Motor_Para_Ld;            ///MOTOR_PARA_Ld
  Motor1.Para.Lq = RAM_Data.Motor_Para_Lq;            ///MOTOR_PARA_Lq
  Motor1.Para.Rs = RAM_Data.Motor_Para_Rs;            ///MOTOR_PARA_Rs
  Motor1.Para.PhaiF = RAM_Data.Motor_Para_Phaif;      ///MOTOR_PARA_PhaiF

  //Motor1.GainCurD.Kp = 1.0f;
  //Motor1.GainCurD.Ki = 0.011f;
  //Motor1.GainCurD.Kaw = 1.0f / Motor1.GainCurD.Kp;
  //Motor1.GainCurQ.Kp = 1.0f;
  //Motor1.GainCurQ.Ki = 0.011f;
  //Motor1.GainCurQ.Kaw = 1.0f / Motor1.GainCurQ.Kp;
  Motor1.GainCurD.Kp = RAM_Data.Motor_GainCurD_Kp;    ///MOTOR_GAINCURD_Kp
  Motor1.GainCurD.Ki = RAM_Data.Motor_GainCurD_Ki;    ///MOTOR_GAINCURD_Ki
  Motor1.GainCurD.Kaw = RAM_Data.Motor_GainCurD_Kaw;  ///MOTOR_GAINCURD_Kaw
  Motor1.GainCurQ.Kp = RAM_Data.Motor_GainCurQ_Kp;    ///MOTOR_GAINCURQ_Kp
  Motor1.GainCurQ.Ki = RAM_Data.Motor_GainCurQ_Ki;    ///MOTOR_GAINCURQ_Ki
  Motor1.GainCurQ.Kaw = RAM_Data.Motor_GainCurQ_Kaw;  ///MOTOR_GAINCURQ_Kaw

  Motor1.GainCurQ.Kd = 0.008f;                            //somebp 2024.02.02
//  Motor1.GainCurQ.Kd = 0.008f;                // HSW 24.02.21
  //Motor1.GainSpd.Kp = 0.0022f;
  //Motor1.GainSpd.Ki = 0.00001f;
  //Motor1.GainSpd.Kaw = 1.0f / Motor1.GainSpd.Kp;
  Motor1.GainSpd.Kp = RAM_Data.Motor_GainSpd_Kp;      ///MOTOR_GAINSPD_Kp
  Motor1.GainSpd.Ki = RAM_Data.Motor_GainSpd_Ki;      ///MOTOR_GAINSPD_Ki
  Motor1.GainSpd.Kaw = RAM_Data.Motor_Gainspd_Kaw;    ///MOTOR_GAINSPD_Kaw

  //Motor1.GainFweak.Kp = 2.0f;
  //Motor1.GainFweak.Ki = 0.002f;
  Motor1.GainFweak.Kp = RAM_Data.Motor_GainFweak_Kp;  ///MOTOR_GAINFWEAK_Kp
  Motor1.GainFweak.Ki = RAM_Data.Motor_GainFweak_Ki;  ///MOTOR_GAINFWEAK_Ki


  Motor1_Ang.PolePair = Motor1.Para.PolePair;
  //Motor1_Ang.EncPulse = 4096*4;     //  TLE5012B : 4096, JKONG : 1000
  //Motor1_Ang.EncPulse = 1000*4;
  //Motor1_Ang.AngleScale = 360.0f / (float)Motor1_Ang.EncPulse;
  //Motor1_Ang.SpeedScale = 1000.0f * 60.0f / (float)Motor1_Ang.EncPulse;
  //Motor1_Ang.AngleElecOffset = 10.0f;       // SAT : 12, JKONG : #1 257 / #2 197 -> inaccurate, check every time!
  //Motor1_Ang.AngleElecOffset = 190.03f;
  //Motor1_Ang.AngleElecOffset = 194.0f;       // 24.01.05 -> Can be changed by sample modification, check every time!
  Motor1_Ang.EncPulse = RAM_Data.Motor_Ang_EncPulse;    ///MOTOR_ANG_ENCPULSE
  Motor1_Ang.AngleScale = RAM_Data.Motor_Ang_AngleScale;///MOTOR_ANG_ANGLESCALE
  Motor1_Ang.SpeedScale = RAM_Data.Motor_Ang_SpeedScale;///MOTOR_ANG_SPEEDSCALE
  Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset; ///MOTOR1_ANG_ANGLEELCOFFSET


//  Motor1_Ang.MR.Max.Sin = 0;
//  Motor1_Ang.MR.Max.Cos = 0;
//  Motor1_Ang.MR.Min.Sin = 4095;
//  Motor1_Ang.MR.Min.Cos = 4095;
//  Motor1_Ang.MR.Mid.Sin = 0;
//  Motor1_Ang.MR.Mid.Cos = 0;
//  Motor1_Ang.MR.Gain.Sin = 1.0f;
//  Motor1_Ang.MR.Gain.Cos = 1.0f;
  Motor1_Ang.MR.Max.Sin = RAM_Data.Motor_Ang_MR_Max_Sin;  ///MOTOR_ANG_MR_MAX_SIN
  Motor1_Ang.MR.Max.Cos = RAM_Data.Motor_Ang_MR_Max_Cos;  ///MOTOR_ANG_MR_MAX_COS
  Motor1_Ang.MR.Min.Sin = RAM_Data.Motor_Ang_MR_Min_Sin;  ///MOTOR_ANG_MR_MIN_SIN
  Motor1_Ang.MR.Min.Cos = RAM_Data.Motor_Ang_MR_Min_Cos;  ///MOTOR_ANG_MR_MIN_COS
  Motor1_Ang.MR.Mid.Sin = RAM_Data.Motor_Ang_MR_Mid_Sin;  ///MOTOR_ANG_MR_MID_SIN
  Motor1_Ang.MR.Mid.Cos = RAM_Data.Motor_Ang_MR_Mid_Cos;  ///MOTOR_ANG_MR_MID_COS
  Motor1_Ang.MR.Gain.Sin = RAM_Data.Motor_Ang_MR_Gain_Sin;///MOTOR_ANG_MR_GAIN_SIN
  Motor1_Ang.MR.Gain.Cos = RAM_Data.Motor_Ang_MR_Gain_Cos;///MOTOR_ANG_MR_GAIN_COS


  // Motor2 Parameter Settings
  Motor2.Para.Pole = Motor1.Para.Pole;          // SAT : 4, JKONG : 8, BS-105-10 : 30
  Motor2.Para.PolePair = Motor2.Para.Pole>>1;
  Motor2.Para.Ld = Motor1.Para.Ld;
  Motor2.Para.Lq = Motor1.Para.Lq;
  Motor2.Para.Rs = Motor1.Para.Rs;
  Motor2.Para.PhaiF = Motor1.Para.PhaiF;  // V/krpm

  Motor2.GainCurD.Kp = Motor1.GainCurD.Kp;
  Motor2.GainCurD.Ki = Motor1.GainCurD.Ki;
  Motor2.GainCurD.Kaw = 1.0f / Motor2.GainCurD.Kp;
  Motor2.GainCurQ.Kp = Motor1.GainCurQ.Kp;
  Motor2.GainCurQ.Ki = Motor1.GainCurQ.Ki;
  Motor2.GainCurQ.Kaw = 1.0f / Motor2.GainCurQ.Kp;

//  Motor2.GainCurQ.Kd = Motor1.GainCurQ.Kd;

  Motor2.GainSpd.Kp = Motor1.GainSpd.Kp;
  Motor2.GainSpd.Ki = Motor1.GainSpd.Ki;
  Motor2.GainSpd.Kaw = 1.0f / Motor2.GainSpd.Kp;

  Motor2.GainFweak.Kp = Motor1.GainFweak.Kp;
  Motor2.GainFweak.Ki = Motor1.GainFweak.Ki;

  Motor2_Ang.PolePair = Motor2.Para.PolePair;
  Motor2_Ang.EncPulse = Motor1_Ang.EncPulse;     //  TLE5012B : 4096, JKONG : 1000
  Motor2_Ang.AngleScale = Motor1_Ang.AngleScale;
  Motor2_Ang.SpeedScale = Motor1_Ang.SpeedScale;
  //Motor2_Ang.AngleElecOffset = 147.0f;       // SAT : 12, JKONG : #1 77 / #2 198.759857f -> inaccurate, check every time!
  Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset; ///MOTOR2_ANG_ANGLEELCOFFSET// SAT : 12, JKONG : #1 77 / #2 198.759857f -> inaccurate, check every time!

  Motor2_Ang.MR.Max.Sin = Motor1_Ang.MR.Max.Sin;
  Motor2_Ang.MR.Max.Cos = Motor1_Ang.MR.Max.Cos;
  Motor2_Ang.MR.Min.Sin = Motor1_Ang.MR.Min.Sin;
  Motor2_Ang.MR.Min.Cos = Motor1_Ang.MR.Min.Cos;
  Motor2_Ang.MR.Mid.Sin = Motor1_Ang.MR.Mid.Sin;
  Motor2_Ang.MR.Mid.Cos = Motor1_Ang.MR.Mid.Cos;
  Motor2_Ang.MR.Gain.Sin = Motor1_Ang.MR.Gain.Sin;
  Motor2_Ang.MR.Gain.Cos = Motor1_Ang.MR.Gain.Cos;


  // Inverter Parameters
//  Inv.Para.HwAdcScale = 3.3f / 4095.0f;
//  Inv.Para.HwVdcScale = 1.0f / (30.0f + 1.0f);
//  Inv.Para.HwiScale = 0.002f * 15.0f / 1.0f;
  Inv.Para.HwAdcScale = RAM_Data.Inv_Para_HwAdcScale; ///INV_PARA_HWADCSCALE
  Inv.Para.HwVdcScale = RAM_Data.Inv_Para_HwVdcScale; ///INV_PARA_HWVDCSCALE
  Inv.Para.HwiScale = RAM_Data.Inv_Para_HwiScale;     ///INV_PARA_HWISCALE

  Inv.Para.VdcScale = Inv.Para.HwAdcScale / Inv.Para.HwVdcScale;
  Inv.Para.iScale = - Inv.Para.HwAdcScale / Inv.Para.HwiScale;

//#if   defined(TIM_8KHZ)
//  Inv.Para.Tsamp = 1.0f/8000.0f;
//#elif defined(TIM_10KHZ)
//  Inv.Para.Tsamp = 1.0f/10000.0f;
//#elif defined(TIM_16KHZ)
//  Inv.Para.Tsamp = 1.0f/16000.0f;
//#elif defined(TIM_20KHZ)
//  Inv.Para.Tsamp = 1.0f/20000.0f;
//#else
//  Inv.Para.Tsamp = 1.0f/10000.0f;
//#endif
  Inv.Para.Tsamp = RAM_Data.Inv_Para_Tsamp;         ///INV_PARA_TSAMP

//  Inv.Para.PWM_Max = TIM_PERIOD;
//  Inv.Para.PWM_Mid = ((TIM_PERIOD+1)>>1);
//
//  Inv.Para.VdcMargin = 0.90f;
  Inv.Para.PWM_Max = RAM_Data.Inv_Para_PWM_Max;     ///INV_PARA_PWM_MAX
  Inv.Para.PWM_Mid = RAM_Data.Inv_Para_PWM_Mid;     ///INV_PARA_PWM_MID

  Inv.Para.VdcMargin = RAM_Data.Inv_Para_VdcMargin; ///INV_PARA_VDCMARGIN


//  LpfVdc.fc = 1000.0f;
//  LpfVdc.fs = 1.0f / Inv.Para.Tsamp;
  LpfVdc.fc = RAM_Data.LPFVdc_fc;                   ///LPFVDC_FC
  LpfVdc.fs = RAM_Data.LPFVdc_fs;                   ///LPFVDC_FS
  LpfFACTOR(LpfVdc.fc, LpfVdc.fs, LpfVdc.fct);

//  LpfCur.fc = 10.0f;
//  LpfCur.fs = 1.0f / Inv.Para.Tsamp;
  LpfCur.fc = RAM_Data.LPFCur_fc;                   ///LPFCUR_FC
  LpfCur.fs = RAM_Data.LPFCur_fs;                   ///LPFCUR_FS
  LpfFACTOR(LpfCur.fc, LpfCur.fs, LpfCur.fct);

//  LpfSpd.fc = 100.0f;
//  LpfSpd.fs = 1000.0f;
  LpfSpd.fc = RAM_Data.LPFSpd_fc;                   ///LPFSPD_FC
  LpfSpd.fs = RAM_Data.LPFSpd_fs;                   ///LPFSPD_FS
  LpfFACTOR(LpfSpd.fc, LpfSpd.fs, LpfSpd.fct);

//  WP_ErrRef.IsensorOpen = 3908.0f;  // 50A에 해당하는 값
//  WP_ErrRef.IsensorShort = 186.0f;  // -50A에 해당하는 값
//  WP_ErrRef.OverCurrent = 30.0f;
//  WP_ErrRef.OverSpeed = 4000.0;
//  WP_ErrRef.OverVoltage = 70.0;
//  WP_ErrRef.UnderVoltage = 30.0;
//  WP_ErrRef.IsensorOffsetMax = 2048.0f + 50.0f;
//  WP_ErrRef.IsensorOffsetMin = 2048.0f - 50.0f;
  WP_ErrRef.IsensorOpen = RAM_Data.ErrRef_IsensorOpen;          ///ERRREF_ISENSOROPEN
  WP_ErrRef.IsensorShort = RAM_Data.ErrRef_IsensorShort;        ///ERRREF_ISENSORSHORT
  WP_ErrRef.OverCurrent = RAM_Data.ErrRef_OverCurrent;          ///ERRREF_OVERCURRENT
  WP_ErrRef.OverSpeed = RAM_Data.ErrRef_OverSpeed;              ///ERRREF_OVERSPEED
  WP_ErrRef.OverVoltage = RAM_Data.ErrRef_OverVoltage;          ///ERRREF_OVERVOLTAGE
  WP_ErrRef.UnderVoltage = RAM_Data.ErrRef_UnderVoltage;        ///ERRREF_UNDERVOLTAGE
  WP_ErrRef.IsensorOffsetMax = RAM_Data.ErrRef_IsensorOffsetMax;///ERRREF_ISENSOROFFSETMAX
  WP_ErrRef.IsensorOffsetMin = RAM_Data.ErrRef_IsensorOffsetMin;///ERRREF_ISENSOROFFSETMIN


//  Inv.Ctrl.VdcLimFweak = 49.0f;                               // Generation Field weak Pi Controller Output Limit
//  Inv.Ctrl.IlimFweak = WP_ErrRef.OverCurrent * 0.8f;          // Motoring Field weak Pi controller Output Limit
  Inv.Ctrl.VdcLimFweak = RAM_Data.Inv_Ctrl_VdcLimFweak; ///INV_CTRL_VDCLIMFWEAK // Generation Field weak Pi Controller Output Limit
  Inv.Ctrl.IlimFweak = RAM_Data.Inv_Ctrl_IlimFweak;     ///INV_CTRL_ILIMFWEAK   // Motoring Field weak Pi controller Output Limit

//  Motor1_Ang.RpmLim = WP_ErrRef.OverSpeed * 0.8f;
//  Motor1_Ang.SpdCtrlOutLim = WP_ErrRef.OverCurrent * 0.8f;    // Speed Control Pi Controller Output Limit
//  Motor2_Ang.RpmLim = WP_ErrRef.OverSpeed * 0.8f;
//  Motor2_Ang.SpdCtrlOutLim = WP_ErrRef.OverCurrent * 0.8f;    // Speed Control Pi Controller Output Limit
  Motor1_Ang.RpmLim = RAM_Data.Motor_Ang_RpmLim;              ///MOTOR_ANG_RPMLIM
  Motor1_Ang.SpdCtrlOutLim = RAM_Data.Motor_Ang_SpdCtrlOutLim;///MOTOR_ANG_SPDCTRLOUTLIM  // Speed Control Pi Controller Output Limit
  Motor2_Ang.RpmLim = RAM_Data.Motor_Ang_RpmLim;              ///MOTOR_ANG_RPMLIM
  Motor2_Ang.SpdCtrlOutLim = RAM_Data.Motor_Ang_SpdCtrlOutLim;///MOTOR_ANG_SPDCTRLOUTLIM  // Speed Control Pi Controller Output Limit
}

inline void Motor1Ctrl_VariSetZero(void)
{
  Motor1.Cmd.Icmd.d = 0.0f;
  Motor1.Cmd.Icmd.q = 0.0f;
  Motor1.Cmd.Iref.d = 0.0f;
  Motor1.Cmd.Iref.q = 0.0f;
  Motor1.Cmd.IsBeta = 0.0f;
  Motor1.Cmd.IsCmd = 0.0f;
  Motor1.Cmd.XbyF_Freq = 0.0f;
  Motor1.Cmd.Vfcmd.d = 0.0f;
  Motor1.Cmd.Vfcmd.q = 0.0f;

  Motor1.PiCurD.Err = 0.0f;
  Motor1.PiCurD.Pterm = 0.0f;
  Motor1.PiCurD.Iterm = 0.0f;
  Motor1.PiCurD.PIterm = 0.0f;
  Motor1.PiCurD.Ff = 0.0f;
  Motor1.PiCurD.AW = 0.0f;
  Motor1.PiCurD.Out = 0.0f;

  Motor1.PiCurQ.Err = 0.0f;
  Motor1.PiCurQ.Pterm = 0.0f;
  Motor1.PiCurQ.Iterm = 0.0f;
  Motor1.PiCurQ.PIterm = 0.0f;

//  Motor1.PiCurQ.Dterm = 0.0f;             ///somebp 2024.02.02


  Motor1.PiCurQ.Ff = 0.0f;
  Motor1.PiCurQ.AW = 0.0f;
  Motor1.PiCurQ.Out = 0.0f;

  Motor1.Fb.VeRef.d = 0.0f;
  Motor1.Fb.VeRef.q = 0.0f;
  Motor1.Fb.VeSet.d = 0.0f;
  Motor1.Fb.VeSet.q = 0.0f;
  Motor1.Fb.V3Set.U = 0.0f;
  Motor1.Fb.V3Set.V = 0.0f;
  Motor1.Fb.V3Set.W = 0.0f;
  Motor1.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
  Motor1.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
  Motor1.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;

  Motor1.PiSpd.Err = 0.0f;
  Motor1.PiSpd.Pterm = 0.0f;
  Motor1.PiSpd.Iterm = 0.0f;
  Motor1.PiSpd.PIterm = 0.0f;
  Motor1.PiSpd.Ff = 0.0f;
  Motor1.PiSpd.AW = 0.0f;
  Motor1.PiSpd.Out = 0.0f;
}

inline void Motor2Ctrl_VariSetZero(void)
{
  Motor2.Cmd.Icmd.d = 0.0f;
  Motor2.Cmd.Icmd.q = 0.0f;
  Motor2.Cmd.Iref.d = 0.0f;
  Motor2.Cmd.Iref.q = 0.0f;
  Motor2.Cmd.IsBeta = 0.0f;
  Motor2.Cmd.IsCmd = 0.0f;
  Motor2.Cmd.XbyF_Freq = 0.0f;
  Motor2.Cmd.Vfcmd.d = 0.0f;
  Motor2.Cmd.Vfcmd.q = 0.0f;

  Motor2.PiCurD.Err = 0.0f;
  Motor2.PiCurD.Pterm = 0.0f;
  Motor2.PiCurD.Iterm = 0.0f;
  Motor2.PiCurD.PIterm = 0.0f;
  Motor2.PiCurD.Ff = 0.0f;
  Motor2.PiCurD.AW = 0.0f;
  Motor2.PiCurD.Out = 0.0f;

  Motor2.PiCurQ.Err = 0.0f;
  Motor2.PiCurQ.Pterm = 0.0f;
  Motor2.PiCurQ.Iterm = 0.0f;
  Motor2.PiCurQ.PIterm = 0.0f;
  Motor2.PiCurQ.Ff = 0.0f;
  Motor2.PiCurQ.AW = 0.0f;
  Motor2.PiCurQ.Out = 0.0f;

  Motor2.Fb.VeRef.d = 0.0f;
  Motor2.Fb.VeRef.q = 0.0f;
  Motor2.Fb.VeSet.d = 0.0f;
  Motor2.Fb.VeSet.q = 0.0f;
  Motor2.Fb.V3Set.U = 0.0f;
  Motor2.Fb.V3Set.V = 0.0f;
  Motor2.Fb.V3Set.W = 0.0f;
  Motor2.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
  Motor2.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
  Motor2.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;

  Motor2.PiSpd.Err = 0.0f;
  Motor2.PiSpd.Pterm = 0.0f;
  Motor2.PiSpd.Iterm = 0.0f;
  Motor2.PiSpd.PIterm = 0.0f;
  Motor2.PiSpd.Ff = 0.0f;
  Motor2.PiSpd.AW = 0.0f;
  Motor2.PiSpd.Out = 0.0f;
}

inline void Motor1Ctrl_PwmStart(void)
{
  switch(WP_Ctrl.Motor1Pwm&0x01)
  {
    case 0:
      HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_1);
      HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_2);
      HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_3);
      HAL_TIMEx_PWMN_Start_IT(&htim1, TIM_CHANNEL_1);
      HAL_TIMEx_PWMN_Start_IT(&htim1, TIM_CHANNEL_2);
      HAL_TIMEx_PWMN_Start_IT(&htim1, TIM_CHANNEL_3);
      WP_Ctrl.Motor1Pwm |= 0x01;
      break;
    default:
      WP_Ctrl.Motor1Pwm = 0x01;
      break;
  }
}

inline void Motor2Ctrl_PwmStart(void)
{
  switch(WP_Ctrl.Motor2Pwm&0x01)
  {
    case 0:
      HAL_TIM_PWM_Start_IT(&htim8, TIM_CHANNEL_1);
      HAL_TIM_PWM_Start_IT(&htim8, TIM_CHANNEL_2);
      HAL_TIM_PWM_Start_IT(&htim8, TIM_CHANNEL_3);
      HAL_TIMEx_PWMN_Start_IT(&htim8, TIM_CHANNEL_1);
      HAL_TIMEx_PWMN_Start_IT(&htim8, TIM_CHANNEL_2);
      HAL_TIMEx_PWMN_Start_IT(&htim8, TIM_CHANNEL_3);
      WP_Ctrl.Motor2Pwm |= 0x01;
      break;
    default:
      WP_Ctrl.Motor2Pwm = 0x01;
      break;
  }
}

inline void Motor1Ctrl_PwmStop(void)
{
  switch((WP_Ctrl.Motor1Pwm>>1)&0x01)
  {
    case 0:
    HAL_TIM_PWM_Stop_IT(&htim1,TIM_CHANNEL_1);
    HAL_TIM_PWM_Stop_IT(&htim1,TIM_CHANNEL_2);
    HAL_TIM_PWM_Stop_IT(&htim1,TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Stop_IT(&htim1,TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Stop_IT(&htim1,TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Stop_IT(&htim1,TIM_CHANNEL_3);
    WP_Ctrl.Motor1Pwm |= 0x02;
    break;
  default:
    WP_Ctrl.Motor1Pwm = 0x02;
    break;
  }
}

inline void Motor2Ctrl_PwmStop(void)
{
  switch((WP_Ctrl.Motor2Pwm>>1)&0x01)
  {
    case 0:
      HAL_TIM_PWM_Stop_IT(&htim8,TIM_CHANNEL_1);
      HAL_TIM_PWM_Stop_IT(&htim8,TIM_CHANNEL_2);
      HAL_TIM_PWM_Stop_IT(&htim8,TIM_CHANNEL_3);
      HAL_TIMEx_PWMN_Stop_IT(&htim8,TIM_CHANNEL_1);
      HAL_TIMEx_PWMN_Stop_IT(&htim8,TIM_CHANNEL_2);
      HAL_TIMEx_PWMN_Stop_IT(&htim8,TIM_CHANNEL_3);
      WP_Ctrl.Motor2Pwm |= 0x02;
      break;
    default:
      WP_Ctrl.Motor2Pwm = 0x02;
      break;
  }
}


/* ************************************************************************************
 *                             Motor Control Basic API                                *
 * ************************************************************************************/
inline void WP_ClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)
{
  // use to match q axis to a phase,  abc -> dq
  s->d = _3->U;
  s->q = MT_1_OVR_SQ3*(_3->V - _3->W);

  // abc -> d-q axis : PARK transform
  e->d = s->q * Angle->Tr_Sin + s->d * Angle->Tr_Cos;
  e->q = s->q * Angle->Tr_Cos - s->d * Angle->Tr_Sin;
//  ids = ias;
//  iqs = ONE_SQRT3*(ibs - ics);
//  ide = iqs*Sin + ids*Cos;
//  iqe = iqs*Cos - ids*Sin;
}

inline void WP_InvClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)
{
  // dq axis ->UVW axis : IPARK transform
  s->d = -e->q * Angle->Tr_Sin + e->d * Angle->Tr_Cos;
  s->q =  e->q * Angle->Tr_Cos + e->d * Angle->Tr_Sin;

  // SPACE Vector PWM
  _3->U =  s->d;
  _3->V = -MT_1_OVR_2 * s->d + MT_SQ3_OVR_2 * s->q;
  _3->W = -MT_1_OVR_2 * s->d - MT_SQ3_OVR_2 * s->q;

//  vdsRef = -vqeOut*Sin+ vdeOut*Cos;
//  vqsRef =  vqeOut*Cos+ vdeOut*Sin;
//  vasRef =  vdsRef;
//  vbsRef = -HALF*vdsRef + SQRT3_TWO*vqsRef;
//  vcsRef = -HALF*vdsRef - SQRT3_TWO*vqsRef;
}

inline void WP_InvMinMax(WP_Motor3PhaseTypeDef *_3in, WP_Motor3PhaseTypeDef *_3out, float Vlim)
{
  float a, b;

  a = _3in->W - _3in->U;
  b = ( (_3in->U - _3in->V) * a > 0 ) ? ( _3in->U ): ( (_3in->V - _3in->W) * a >=0  ? _3in->W : _3in->V );
  b = b * 0.5f;

  _3out->U = _3in->U + b;
  _3out->V = _3in->V + b;
  _3out->W = _3in->W + b;

  _3out->U = SAT(_3out->U, Vlim);
  _3out->V = SAT(_3out->V, Vlim);
  _3out->W = SAT(_3out->W, Vlim);
}

inline void WP_InvDeadTimeComp(WP_Motor3PhaseTypeDef *_3I, WP_Motor3PhaseTypeDef *_3V, WP_Motor3PhaseTypeDef *_3dead, float VT_DEAD, float Vlim)
{
  if(_3I->U > 5.0f)        _3dead->U = VT_DEAD;
  else if(_3I->U < -5.0f)  _3dead->U = -VT_DEAD;
  else                      _3dead->U=(0.0f);

  if(_3I->V > 5.0f)        _3dead->V = VT_DEAD;
  else if(_3I->V < -5.0f)  _3dead->V = -VT_DEAD;
  else                      _3dead->V=(0.0f);

  if(_3I->W > 5.0f)        _3dead->W = VT_DEAD;
  else if(_3I->W < -5.0f)  _3dead->W = -VT_DEAD;
  else                      _3dead->W = (0.0f);

  _3V->U += _3dead->U;
  _3V->V += _3dead->V;
  _3V->W += _3dead->W;

  _3V->U = SAT(_3V->U, Vlim);
  _3V->V = SAT(_3V->V, Vlim);
  _3V->W = SAT(_3V->W, Vlim);
}


inline float WP_PIcontrol(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)
{
  PiCur->Err = Cmd - Fedb;
  PiCur->Pterm = Gain->Kp * PiCur->Err;// Kp*e(k)
  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err - PiCur->AW * Gain->Kaw) + PiCur->Iterm + PiCur->Ff;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)
  PiCur->Out = SAT(PiCur->PIterm, VRefLim);
  PiCur->Iterm += Gain->Ki * (PiCur->Err - PiCur->AW * Gain->Kaw);  //x(k)=x(k-1)+Ki*e(k)
  PiCur->Iterm = SAT(PiCur->Iterm, VRefLim);

  return PiCur->Out;
}
/////somebp 2024.02.02
////add Dterm, operate  when rpm is -lim
//#define   LIMIT_RPM           90
//#define   DTERM_SCALE         2.0f            //1(Low) ~ 100(High)
//inline float WP_PIcontrol_Spdlim(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim, float rpm)
//{
//  if(rpm > LIMIT_RPM) {                 //  add Dterm, operate  when rpm is -lim
//    PiCur->Err = Cmd - Fedb;
//    PiCur->Dterm = (Gain->Kd* PiCur->Err)/0.0000625;  // Ang->RpmFil enable
//    if(PiCur->Dterm < 0) {  //if only Negative
//      float dterm  = 1-PiCur->Dterm/(1000.0f/DTERM_SCALE);              //>=1.0
//      Cmd *= dterm;
//
//      if(0)     //Add "//" to remark for Displaying
//      { //for Debugging
//        static uint32_t time_old=0;
//        uint32_t time_now=Get_Time();
//        if(time_now - time_old >= 100) {
//          time_old = time_now;
//          UART3_printf("[rpm]%s\t",Float2String(rpm, 2));
//          UART3_printf("[Dterm]%s\t",Float2String(PiCur->Dterm, 2));    //Negative
//          UART3_printf("[Scale]%s\t",Float2String(dterm, 2));
//          UART3_printf("[Cmd]%s\r\n",Float2String(Cmd, 2));
//        }
//      }
//    }
//  }
//
////WP_PIcontrol(Cmd, Fedb, PiCur, Gain, VRefLim);
//
//  PiCur->Err = Cmd - Fedb;
//  PiCur->Pterm = Gain->Kp * PiCur->Err;// Kp*e(k) //PiCur->Aw*Hain->kaw :error가 너무 쌓였을 때 안티와인드업 / 현재의 적분텀과 과거의 적분텀을 더함
//  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err - PiCur->AW * Gain->Kaw) + PiCur->Iterm + PiCur->Ff;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)
//  PiCur->Out = SAT(PiCur->PIterm, VRefLim);       //SAT: Cut out of range
//  PiCur->Iterm += Gain->Ki * (PiCur->Err - PiCur->AW * Gain->Kaw);  //x(k)=x(k-1)+Ki*e(k)
//  PiCur->Iterm = SAT(PiCur->Iterm, VRefLim);      // VRefLim: currently x
//
//  return PiCur->Out;
//}
/////somebp 2024.02.02

inline float WP_PIcontrolForFieldWeak(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)
{
  PiCur->Err = Cmd - Fedb;
  PiCur->Pterm = Gain->Kp * PiCur->Err;// Kp*e(k)
  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err) + PiCur->Iterm;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)
  PiCur->Out = SAT_Fw(PiCur->PIterm, VRefLim);
  PiCur->Iterm += Gain->Ki * (PiCur->Err);  //x(k)=x(k-1)+Ki*e(k)
  PiCur->Iterm = SAT_Fw(PiCur->Iterm, VRefLim);

  return PiCur->Out;
}

inline void WP_PwmScale(uint32_t* Duty, WP_Motor3PhaseTypeDef *_3V, float *Rt, WP_InverterTypeDef *inv)
{
  float Du[3];

  *(Rt)   = _3V->U * inv->Ctrl._Vdc_2;
  *(Rt+1) = _3V->V * inv->Ctrl._Vdc_2;
  *(Rt+2) = _3V->W * inv->Ctrl._Vdc_2;
  Du[0] = SAT(_3V->U * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);
  Du[1] = SAT(_3V->V * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);
  Du[2] = SAT(_3V->W * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);
  *(Duty)   = (uint32_t) SAT_U((Du[0] + inv->Para.PWM_Mid), inv->Para.PWM_Max);
  *(Duty+1) = (uint32_t) SAT_U((Du[1] + inv->Para.PWM_Mid), inv->Para.PWM_Max);
  *(Duty+2) = (uint32_t) SAT_U((Du[2] + inv->Para.PWM_Mid), inv->Para.PWM_Max);
}

inline WP_ErrorStatus WP_PWMout(uint8_t Mot, uint32_t* Duty)
{
  switch(Mot)
  {
    case MOTOR1:
      PWM0_U = *(Duty);
      PWM0_V = *(Duty+1);
      PWM0_W = *(Duty+2);
      return WP_OK;
      break;
    case MOTOR2:
      PWM1_U = *(Duty);
      PWM1_V = *(Duty+1);
      PWM1_W = *(Duty+2);
      return WP_OK;
      break;
    default:
      return WP_ERR;
      break;
  }
}
//*cmd : Motor1.Cmd.Icmd.q/d, Ref : Motor1.Cmd.Iref.q/d
inline void WP_RampUpDn(float *Cmd, float *Ref, float RampU, float RampD) // 단위 : 1A / 스위칭
{
  if(*Ref < *Cmd){
    *Ref += RampU;
    *Ref = (*Ref > *Cmd) ? *Cmd : *Ref;
  }
  else if(*Ref > *Cmd){
    *Ref -= RampD;
    *Ref = (*Ref < *Cmd) ? *Cmd : *Ref;
  }
}


inline void WP_MrPos(WP_MotorAngleTypeDef *Ang)
{
  if(WP_Ctrl.MrOffsetFlg==1)
  {
    if(Ang->MR.Raw.Sin >= Ang->MR.Max.Sin) Ang->MR.Max.Sin = Ang->MR.Raw.Sin;
    if(Ang->MR.Raw.Sin <= Ang->MR.Min.Sin) Ang->MR.Min.Sin = Ang->MR.Raw.Sin;
    if(Ang->MR.Raw.Cos >= Ang->MR.Max.Cos) Ang->MR.Max.Cos = Ang->MR.Raw.Cos;
    if(Ang->MR.Raw.Cos <= Ang->MR.Min.Cos) Ang->MR.Min.Cos = Ang->MR.Raw.Cos;

    Ang->MR.Mid.Sin = (Ang->MR.Max.Sin + Ang->MR.Min.Sin)>>1;
    Ang->MR.Mid.Cos = (Ang->MR.Max.Cos + Ang->MR.Min.Cos)>>1;

    Ang->MR.Gain.Sin = 1.0f;
    Ang->MR.Gain.Cos = (((float)Ang->MR.Max.Sin - Ang->MR.Mid.Sin))/(((float)Ang->MR.Max.Cos - Ang->MR.Mid.Cos));
  }
  else{}

  Ang->MR.Scaled.Sin = ((float)Ang->MR.Raw.Sin - (float)Ang->MR.Mid.Sin) * Ang->MR.Gain.Sin;
  Ang->MR.Scaled.Cos = ((float)Ang->MR.Raw.Cos - (float)Ang->MR.Mid.Cos) * Ang->MR.Gain.Cos;

  Ang->AngleMech = TR_RAD2ANG * (atan2f(Ang->MR.Scaled.Sin, Ang->MR.Scaled.Cos)+PI);

  if(Ang->AngleMech >= 360.0f)   Ang->AngleMech -= 360.0f;
  else if(Ang->AngleMech < 0)    Ang->AngleMech += 360.0f;

  Ang->AngleElec = Ang->AngleMech * (float)Ang->PolePair;
  Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;

  if(Ang->AngleElec >= 360.0f)   Ang->AngleElec -= 360.0f;
  else if(Ang->AngleElec < 0)    Ang->AngleElec += 360.0f;
}


inline WP_ErrorStatus WP_EncPos(uint8_t Mot, WP_MotorAngleTypeDef *Ang)  // this function in 'stm32f4xx_it.c'
{
  if(Mot==MOTOR1)
  {
//    Ang->Pulse = __HAL_TIM_GET_COUNTER(&htim2);
    Ang->Pulse = -1 *__HAL_TIM_GET_COUNTER(&htim2);
    //    cnt1 = tick*4;
//    Ang->diffL += ((Ang->Pulse - Ang->PulseOld)<<16)>>16;
    Ang->diffL += ((Ang->PulseOld - Ang->Pulse)<<16)>>16;
//    Ang->diffF += (float)(((Ang->Pulse - Ang->PulseOld)<<16)>>16);
    Ang->diffF += (float)(((Ang->PulseOld - Ang->Pulse)<<16)>>16);
    if(Ang -> diffL > Ang->EncPulse){
      Ang->diffL = Ang->diffL - Ang->EncPulse;
    }
    else if(Ang->diffL < 0){
      Ang->diffL = Ang->diffL + Ang->EncPulse;
    }
    Ang->AngleMech = (float)Ang->diffL * Ang->AngleScale;
    Ang->AngleMechAll = Ang->diffF * Ang->AngleScale;
    Ang->PulseOld = Ang->Pulse;

    Ang->AngleElec = Ang->AngleMech * (float)Ang->PolePair;
    Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;

    if(Ang->AngleElec >= 360.0f)   Ang->AngleElec -= 360.0f;
    else if(Ang->AngleElec < 0)    Ang->AngleElec += 360.0f;

    return WP_OK;
  }
  else if(Mot==MOTOR2)
  {
    Ang->Pulse = __HAL_TIM_GET_COUNTER(&htim4);

//    Ang->diffL += ((Ang->Pulse - Ang->PulseOld)<<16)>>16;
    Ang->diffL += ((Ang->PulseOld - Ang->Pulse)<<16)>>16;
//    Ang->diffF += (float)(((Ang->Pulse - Ang->PulseOld)<<16)>>16);
    Ang->diffF += (float)(((Ang->PulseOld - Ang->Pulse)<<16)>>16);
    if(Ang -> diffL > Ang->EncPulse){
      Ang->diffL = Ang->diffL - Ang->EncPulse;
    }
    else if(Ang->diffL < 0){
      Ang->diffL = Ang->diffL + Ang->EncPulse;
    }
    Ang->AngleMech = (float)Ang->diffL * Ang->AngleScale;
    Ang->AngleMechAll = Ang->diffF * Ang->AngleScale;
    Ang->PulseOld = Ang->Pulse;

    Ang->AngleElec = Ang->AngleMech * (float)Ang->PolePair;
    Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;

    if(Ang->AngleElec >= 360.0f)   Ang->AngleElec -= 360.0f;
    else if(Ang->AngleElec < 0)    Ang->AngleElec += 360.0f;

    return WP_OK;
  }
  else{
    return WP_ERR;
  }
}

inline WP_ErrorStatus WP_EncSpd(uint8_t Mot, WP_MotorAngleTypeDef *Ang)
{
#if   defined(TIM_8KHZ)
  uint8_t CntRef=8;
#elif defined(TIM_10KHZ)
  uint8_t CntRef=10;
#elif defined(TIM_16KHZ)
  uint8_t CntRef=16;
#elif defined(TIM_20KHZ)
  uint8_t CntRef=20;
#else
  uint8_t CntRef=10;
#endif

  if(Mot==MOTOR1)
  {
    Ang->_1msCnt++;
    if(Ang->_1msCnt > CntRef)
    {
//      Ang->mnew = __HAL_TIM_GET_COUNTER(&htim2);
      Ang->mnew = -1 * __HAL_TIM_GET_COUNTER(&htim2);

//      Ang->diffM = ((Ang->mnew - Ang->mold)<<16)>>16;
      Ang->diffM = ((Ang->mold - Ang->mnew)<<16)>>16;

      Ang->Rpm = (float)Ang->diffM * Ang->SpeedScale * 0.3333; // 기어비 반영
      Ang->mold = Ang->mnew;

      LPF(Ang->RpmFil, Ang->Rpm, LpfSpd.fct);
      Ang->WeFil = Ang->RpmFil * TR_RPM2RAD;
      Ang->_1msCnt = 0;
      Ang->_1msFlg = 1;
    }
    else{}

    return WP_OK;
  }
  else if(Mot==MOTOR2)
  {
    Ang->_1msCnt++;
    if(Ang->_1msCnt > CntRef)
    {
      Ang->mnew = __HAL_TIM_GET_COUNTER(&htim4);

//      Ang->diffM = ((Ang->mnew - Ang->mold)<<16)>>16;
      Ang->diffM = ((Ang->mold - Ang->mnew)<<16)>>16;

      Ang->Rpm = (float)Ang->diffM * Ang->SpeedScale * 0.3333; // 기어비 반영
      Ang->mold = Ang->mnew;

      LPF(Ang->RpmFil, Ang->Rpm, LpfSpd.fct);
      Ang->WeFil = Ang->RpmFil * TR_RPM2RAD;
      Ang->_1msCnt = 0;
      Ang->_1msFlg = 1;
    }
    else{}
    return WP_OK;
  }
  else{
    return WP_ERR;
  }
}


//################################################################################
//
//    you must change the "HAL_GPIO_EXTI_Callback" function (see below)
//    the "HAL_GPIO_EXTI_Callback" function is in the "gpio.c" file
//    If it is not existed, add code like below
//
//################################################################################
/*
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if(GPIO_Pin==GPIO_PIN_15){
    Motor2_Ang.diffL = 0;
  }
  else if(GPIO_Pin==GPIO_PIN_0){
    Motor1_Ang.diffL = 0;
  }
}
*/








///*
// * WESPION_MC.c
// *
// *  Created on: Jul 8, 2023
// *      Author: JuHyung
// */
//
//
//#include "WESPION.h"
//
//WP_ControllerTypeDef WP_Ctrl={0,};
//WP_ControllerErrorTypeDef WP_CtrlErr={0,};
//WP_ErrorValueTypeDef WP_ErrRef;
//
//WP_InverterTypeDef Inv={0,};
//
//WP_MotorAngleTypeDef Motor1_Ang={0,}, Motor2_Ang={0,};
//WP_MotorContrlTypeDef Motor1={0,}, Motor2={0,};
//
//WP_InverterADrawdataTypeDef ADinj;
//WP_InverterComAdTypeDef ComAd;
//
//WP_InverterLpfTypeDef LpfCur, LpfVdc, LpfSpd;
//
//const float sinTab[2048] =
//{
//    0.00000,0.00308,0.00613,0.00922,0.01227,0.01535,0.01840,0.02148,0.02454,0.02762,0.03067,0.03375,0.03680,0.03989,0.04294,0.04599,0.04907,0.05212,0.05521,0.05826,0.06131,0.06439,0.06744,0.07050,0.07355,0.07663,0.07968,0.08273,0.08578,0.08884,0.09192,0.09497,0.09802,0.10107,0.10413,0.10718,0.11023,0.11328,0.11630,0.11935,0.12241,0.12546,0.12848,0.13153,0.13458,0.13760,0.14066,0.14368,0.14673,0.14975,0.15280,0.15582,0.15884,0.16190,0.16492,0.16794,0.17096,0.17398,0.17700,0.18002,0.18304,0.18604,0.18906,0.19208,0.19510,0.19809,0.20111,0.20410,0.20709,0.21011,0.21310,0.21609,0.21909,0.22208,0.22507,0.22806,0.23105,0.23404,0.23703,0.23999,0.24298,0.24594,0.24893,0.25189,0.25485,0.25781,0.26077,0.26373,0.26669,0.26965,0.27261,0.27557,0.27850,0.28146,0.28439,0.28735,0.29028,0.29321,0.29614,0.29907,0.30200,0.30493,0.30783,0.31076,0.31366,0.31659,0.31949,0.32239,0.32529,0.32819,0.33109,0.33398,0.33688,0.33975,0.34265,0.34552,0.34842,0.35129,0.35416,0.35703,0.35989,0.36273,0.36560,0.36847,0.37131,0.37415,0.37698,0.37982,
//    0.38266,0.38550,0.38834,0.39114,0.39398,0.39679,0.39960,0.40244,0.40524,0.40802,0.41083,0.41364,0.41641,0.41919,0.42200,0.42477,0.42755,0.43033,0.43307,0.43585,0.43860,0.44135,0.44412,0.44687,0.44958,0.45233,0.45508,0.45779,0.46051,0.46326,0.46597,0.46869,0.47137,0.47409,0.47678,0.47949,0.48218,0.48486,0.48755,0.49020,0.49289,0.49554,0.49820,0.50089,0.50351,0.50616,0.50882,0.51144,0.51410,0.51672,0.51935,0.52197,0.52457,0.52719,0.52979,0.53238,0.53497,0.53757,0.54016,0.54272,0.54532,0.54788,0.55045,0.55301,0.55554,0.55811,0.56064,0.56317,0.56570,0.56824,0.57077,0.57327,0.57581,0.57831,0.58081,0.58328,0.58578,0.58826,0.59073,0.59320,0.59567,0.59814,0.60059,0.60306,0.60550,0.60794,0.61035,0.61279,0.61520,0.61761,0.62003,0.62244,0.62485,0.62723,0.62961,0.63199,0.63437,0.63675,0.63910,0.64145,0.64380,0.64615,0.64850,0.65082,0.65317,0.65549,0.65778,0.66010,0.66238,0.66470,0.66699,0.66925,0.67154,0.67380,0.67609,0.67831,0.68057,0.68283,0.68506,0.68729,0.68951,0.69174,0.69394,0.69617,0.69836,0.70056,0.70273,0.70493,
//    0.70709,0.70926,0.71140,0.71356,0.71570,0.71783,0.71997,0.72211,0.72421,0.72635,0.72845,0.73053,0.73264,0.73471,0.73679,0.73886,0.74094,0.74298,0.74503,0.74707,0.74911,0.75113,0.75317,0.75519,0.75717,0.75919,0.76117,0.76315,0.76514,0.76712,0.76907,0.77103,0.77298,0.77493,0.77686,0.77878,0.78070,0.78262,0.78455,0.78644,0.78833,0.79019,0.79208,0.79395,0.79581,0.79767,0.79950,0.80136,0.80319,0.80499,0.80682,0.80862,0.81042,0.81223,0.81403,0.81580,0.81757,0.81934,0.82108,0.82281,0.82455,0.82629,0.82803,0.82974,0.83145,0.83313,0.83484,0.83652,0.83820,0.83987,0.84152,0.84317,0.84482,0.84647,0.84808,0.84970,0.85132,0.85294,0.85452,0.85611,0.85770,0.85928,0.86084,0.86240,0.86395,0.86548,0.86703,0.86853,0.87006,0.87158,0.87308,0.87457,0.87604,0.87753,0.87900,0.88043,0.88190,0.88333,0.88477,0.88620,0.88760,0.88901,0.89041,0.89182,0.89319,0.89456,0.89594,0.89731,0.89865,0.89999,0.90131,0.90265,0.90396,0.90527,0.90656,0.90787,0.90915,0.91040,0.91168,0.91293,0.91418,0.91544,0.91666,0.91788,0.91910,0.92029,0.92148,0.92267,
//    0.92386,0.92502,0.92618,0.92734,0.92847,0.92963,0.93073,0.93185,0.93295,0.93405,0.93515,0.93625,0.93732,0.93839,0.93942,0.94049,0.94153,0.94254,0.94357,0.94458,0.94559,0.94656,0.94757,0.94855,0.94949,0.95047,0.95142,0.95233,0.95328,0.95419,0.95511,0.95602,0.95691,0.95779,0.95868,0.95953,0.96039,0.96124,0.96210,0.96292,0.96375,0.96457,0.96536,0.96616,0.96695,0.96771,0.96851,0.96924,0.97000,0.97073,0.97147,0.97220,0.97290,0.97360,0.97430,0.97501,0.97568,0.97635,0.97699,0.97766,0.97830,0.97891,0.97955,0.98016,0.98074,0.98135,0.98193,0.98251,0.98306,0.98364,0.98419,0.98471,0.98526,0.98578,0.98627,0.98679,0.98727,0.98776,0.98822,0.98868,0.98914,0.98959,0.99002,0.99045,0.99088,0.99127,0.99167,0.99207,0.99246,0.99283,0.99319,0.99353,0.99387,0.99420,0.99454,0.99484,0.99515,0.99545,0.99573,0.99600,0.99628,0.99655,0.99680,0.99704,0.99725,0.99747,0.99768,0.99789,0.99808,0.99826,0.99844,0.99860,0.99878,0.99890,0.99905,0.99918,0.99930,0.99939,0.99951,0.99960,0.99966,0.99973,0.99979,0.99985,0.99991,0.99994,0.99994,0.99997,
//    0.99997,0.99997,0.99994,0.99994,0.99991,0.99985,0.99979,0.99973,0.99966,0.99960,0.99951,0.99939,0.99930,0.99918,0.99905,0.99890,0.99878,0.99860,0.99844,0.99826,0.99808,0.99789,0.99768,0.99747,0.99725,0.99704,0.99680,0.99655,0.99628,0.99600,0.99573,0.99545,0.99515,0.99484,0.99454,0.99420,0.99387,0.99353,0.99319,0.99283,0.99246,0.99207,0.99167,0.99127,0.99088,0.99045,0.99002,0.98959,0.98914,0.98868,0.98822,0.98776,0.98727,0.98679,0.98627,0.98578,0.98526,0.98471,0.98419,0.98364,0.98306,0.98251,0.98193,0.98135,0.98074,0.98016,0.97955,0.97891,0.97830,0.97766,0.97699,0.97635,0.97568,0.97501,0.97430,0.97360,0.97290,0.97220,0.97147,0.97073,0.97000,0.96924,0.96851,0.96771,0.96695,0.96616,0.96536,0.96457,0.96375,0.96292,0.96210,0.96124,0.96039,0.95953,0.95868,0.95779,0.95691,0.95602,0.95511,0.95419,0.95328,0.95233,0.95142,0.95047,0.94949,0.94855,0.94757,0.94656,0.94559,0.94458,0.94357,0.94254,0.94153,0.94049,0.93942,0.93839,0.93732,0.93625,0.93515,0.93405,0.93295,0.93185,0.93073,0.92963,0.92847,0.92734,0.92618,0.92502,
//    0.92386,0.92267,0.92148,0.92029,0.91910,0.91788,0.91666,0.91544,0.91418,0.91293,0.91168,0.91040,0.90915,0.90787,0.90656,0.90527,0.90396,0.90265,0.90131,0.89999,0.89865,0.89731,0.89594,0.89456,0.89319,0.89182,0.89041,0.88901,0.88760,0.88620,0.88477,0.88333,0.88190,0.88043,0.87900,0.87753,0.87604,0.87457,0.87308,0.87158,0.87006,0.86853,0.86703,0.86548,0.86395,0.86240,0.86084,0.85928,0.85770,0.85611,0.85452,0.85294,0.85132,0.84970,0.84808,0.84647,0.84482,0.84317,0.84152,0.83987,0.83820,0.83652,0.83484,0.83313,0.83145,0.82974,0.82803,0.82629,0.82455,0.82281,0.82108,0.81934,0.81757,0.81580,0.81403,0.81223,0.81042,0.80862,0.80682,0.80499,0.80319,0.80136,0.79950,0.79767,0.79581,0.79395,0.79208,0.79019,0.78833,0.78644,0.78455,0.78262,0.78070,0.77878,0.77686,0.77493,0.77298,0.77103,0.76907,0.76712,0.76514,0.76315,0.76117,0.75919,0.75717,0.75519,0.75317,0.75113,0.74911,0.74707,0.74503,0.74298,0.74094,0.73886,0.73679,0.73471,0.73264,0.73053,0.72845,0.72635,0.72421,0.72211,0.71997,0.71783,0.71570,0.71356,0.71140,0.70926,
//    0.70709,0.70493,0.70273,0.70056,0.69836,0.69617,0.69394,0.69174,0.68951,0.68729,0.68506,0.68283,0.68057,0.67831,0.67609,0.67380,0.67154,0.66925,0.66699,0.66470,0.66238,0.66010,0.65778,0.65549,0.65317,0.65082,0.64850,0.64615,0.64380,0.64145,0.63910,0.63675,0.63437,0.63199,0.62961,0.62723,0.62485,0.62244,0.62003,0.61761,0.61520,0.61279,0.61035,0.60794,0.60550,0.60306,0.60059,0.59814,0.59567,0.59320,0.59073,0.58826,0.58578,0.58328,0.58081,0.57831,0.57581,0.57327,0.57077,0.56824,0.56570,0.56317,0.56064,0.55811,0.55554,0.55301,0.55045,0.54788,0.54532,0.54272,0.54016,0.53757,0.53497,0.53238,0.52979,0.52719,0.52457,0.52197,0.51935,0.51672,0.51410,0.51144,0.50882,0.50616,0.50351,0.50089,0.49820,0.49554,0.49289,0.49020,0.48755,0.48486,0.48218,0.47949,0.47678,0.47409,0.47137,0.46869,0.46597,0.46326,0.46051,0.45779,0.45508,0.45233,0.44958,0.44687,0.44412,0.44135,0.43860,0.43585,0.43307,0.43033,0.42755,0.42477,0.42200,0.41919,0.41641,0.41364,0.41083,0.40802,0.40524,0.40244,0.39960,0.39679,0.39398,0.39114,0.38834,0.38550,
//    0.38266,0.37982,0.37698,0.37415,0.37131,0.36847,0.36560,0.36273,0.35989,0.35703,0.35416,0.35129,0.34842,0.34552,0.34265,0.33975,0.33688,0.33398,0.33109,0.32819,0.32529,0.32239,0.31949,0.31659,0.31366,0.31076,0.30783,0.30493,0.30200,0.29907,0.29614,0.29321,0.29028,0.28735,0.28439,0.28146,0.27850,0.27557,0.27261,0.26965,0.26669,0.26373,0.26077,0.25781,0.25485,0.25189,0.24893,0.24594,0.24298,0.23999,0.23703,0.23404,0.23105,0.22806,0.22507,0.22208,0.21909,0.21609,0.21310,0.21011,0.20709,0.20410,0.20111,0.19809,0.19510,0.19208,0.18906,0.18604,0.18304,0.18002,0.17700,0.17398,0.17096,0.16794,0.16492,0.16190,0.15884,0.15582,0.15280,0.14975,0.14673,0.14368,0.14066,0.13760,0.13458,0.13153,0.12848,0.12546,0.12241,0.11935,0.11630,0.11328,0.11023,0.10718,0.10413,0.10107,0.09802,0.09497,0.09192,0.08884,0.08578,0.08273,0.07968,0.07663,0.07355,0.07050,0.06744,0.06439,0.06131,0.05826,0.05521,0.05212,0.04907,0.04599,0.04294,0.03989,0.03680,0.03375,0.03067,0.02762,0.02454,0.02148,0.01840,0.01535,0.01227,0.00922,0.00613,0.00308,
//    0.00000,-0.00308,-0.00613,-0.00922,-0.01227,-0.01535,-0.01840,-0.02148,-0.02454,-0.02762,-0.03067,-0.03375,-0.03680,-0.03989,-0.04294,-0.04599,-0.04907,-0.05212,-0.05521,-0.05826,-0.06131,-0.06439,-0.06744,-0.07050,-0.07355,-0.07663,-0.07968,-0.08273,-0.08578,-0.08884,-0.09192,-0.09497,-0.09802,-0.10107,-0.10413,-0.10718,-0.11023,-0.11328,-0.11630,-0.11935,-0.12241,-0.12546,-0.12848,-0.13153,-0.13458,-0.13760,-0.14066,-0.14368,-0.14673,-0.14975,-0.15280,-0.15582,-0.15884,-0.16190,-0.16492,-0.16794,-0.17096,-0.17398,-0.17700,-0.18002,-0.18304,-0.18604,-0.18906,-0.19208,-0.19510,-0.19809,-0.20111,-0.20410,-0.20709,-0.21011,-0.21310,-0.21609,-0.21909,-0.22208,-0.22507,-0.22806,-0.23105,-0.23404,-0.23703,-0.23999,-0.24298,-0.24594,-0.24893,-0.25189,-0.25485,-0.25781,-0.26077,-0.26373,-0.26669,-0.26965,-0.27261,-0.27557,-0.27850,-0.28146,-0.28439,-0.28735,-0.29028,-0.29321,-0.29614,-0.29907,-0.30200,-0.30493,-0.30783,-0.31076,-0.31366,-0.31659,-0.31949,-0.32239,-0.32529,-0.32819,-0.33109,-0.33398,-0.33688,-0.33975,-0.34265,-0.34552,-0.34842,-0.35129,-0.35416,-0.35703,-0.35989,-0.36273,-0.36560,-0.36847,-0.37131,-0.37415,-0.37698,-0.37982,
//    -0.38266,-0.38550,-0.38834,-0.39114,-0.39398,-0.39679,-0.39960,-0.40244,-0.40524,-0.40802,-0.41083,-0.41364,-0.41641,-0.41919,-0.42200,-0.42477,-0.42755,-0.43033,-0.43307,-0.43585,-0.43860,-0.44135,-0.44412,-0.44687,-0.44958,-0.45233,-0.45508,-0.45779,-0.46051,-0.46326,-0.46597,-0.46869,-0.47137,-0.47409,-0.47678,-0.47949,-0.48218,-0.48486,-0.48755,-0.49020,-0.49289,-0.49554,-0.49820,-0.50089,-0.50351,-0.50616,-0.50882,-0.51144,-0.51410,-0.51672,-0.51935,-0.52197,-0.52457,-0.52719,-0.52979,-0.53238,-0.53497,-0.53757,-0.54016,-0.54272,-0.54532,-0.54788,-0.55045,-0.55301,-0.55554,-0.55811,-0.56064,-0.56317,-0.56570,-0.56824,-0.57077,-0.57327,-0.57581,-0.57831,-0.58081,-0.58328,-0.58578,-0.58826,-0.59073,-0.59320,-0.59567,-0.59814,-0.60059,-0.60306,-0.60550,-0.60794,-0.61035,-0.61279,-0.61520,-0.61761,-0.62003,-0.62244,-0.62485,-0.62723,-0.62961,-0.63199,-0.63437,-0.63675,-0.63910,-0.64145,-0.64380,-0.64615,-0.64850,-0.65082,-0.65317,-0.65549,-0.65778,-0.66010,-0.66238,-0.66470,-0.66699,-0.66925,-0.67154,-0.67380,-0.67609,-0.67831,-0.68057,-0.68283,-0.68506,-0.68729,-0.68951,-0.69174,-0.69394,-0.69617,-0.69836,-0.70056,-0.70273,-0.70493,
//    -0.70709,-0.70926,-0.71140,-0.71356,-0.71570,-0.71783,-0.71997,-0.72211,-0.72421,-0.72635,-0.72845,-0.73053,-0.73264,-0.73471,-0.73679,-0.73886,-0.74094,-0.74298,-0.74503,-0.74707,-0.74911,-0.75113,-0.75317,-0.75519,-0.75717,-0.75919,-0.76117,-0.76315,-0.76514,-0.76712,-0.76907,-0.77103,-0.77298,-0.77493,-0.77686,-0.77878,-0.78070,-0.78262,-0.78455,-0.78644,-0.78833,-0.79019,-0.79208,-0.79395,-0.79581,-0.79767,-0.79950,-0.80136,-0.80319,-0.80499,-0.80682,-0.80862,-0.81042,-0.81223,-0.81403,-0.81580,-0.81757,-0.81934,-0.82108,-0.82281,-0.82455,-0.82629,-0.82803,-0.82974,-0.83145,-0.83313,-0.83484,-0.83652,-0.83820,-0.83987,-0.84152,-0.84317,-0.84482,-0.84647,-0.84808,-0.84970,-0.85132,-0.85294,-0.85452,-0.85611,-0.85770,-0.85928,-0.86084,-0.86240,-0.86395,-0.86548,-0.86703,-0.86853,-0.87006,-0.87158,-0.87308,-0.87457,-0.87604,-0.87753,-0.87900,-0.88043,-0.88190,-0.88333,-0.88477,-0.88620,-0.88760,-0.88901,-0.89041,-0.89182,-0.89319,-0.89456,-0.89594,-0.89731,-0.89865,-0.89999,-0.90131,-0.90265,-0.90396,-0.90527,-0.90656,-0.90787,-0.90915,-0.91040,-0.91168,-0.91293,-0.91418,-0.91544,-0.91666,-0.91788,-0.91910,-0.92029,-0.92148,-0.92267,
//    -0.92386,-0.92502,-0.92618,-0.92734,-0.92847,-0.92963,-0.93073,-0.93185,-0.93295,-0.93405,-0.93515,-0.93625,-0.93732,-0.93839,-0.93942,-0.94049,-0.94153,-0.94254,-0.94357,-0.94458,-0.94559,-0.94656,-0.94757,-0.94855,-0.94949,-0.95047,-0.95142,-0.95233,-0.95328,-0.95419,-0.95511,-0.95602,-0.95691,-0.95779,-0.95868,-0.95953,-0.96039,-0.96124,-0.96210,-0.96292,-0.96375,-0.96457,-0.96536,-0.96616,-0.96695,-0.96771,-0.96851,-0.96924,-0.97000,-0.97073,-0.97147,-0.97220,-0.97290,-0.97360,-0.97430,-0.97501,-0.97568,-0.97635,-0.97699,-0.97766,-0.97830,-0.97891,-0.97955,-0.98016,-0.98074,-0.98135,-0.98193,-0.98251,-0.98306,-0.98364,-0.98419,-0.98471,-0.98526,-0.98578,-0.98627,-0.98679,-0.98727,-0.98776,-0.98822,-0.98868,-0.98914,-0.98959,-0.99002,-0.99045,-0.99088,-0.99127,-0.99167,-0.99207,-0.99246,-0.99283,-0.99319,-0.99353,-0.99387,-0.99420,-0.99454,-0.99484,-0.99515,-0.99545,-0.99573,-0.99600,-0.99628,-0.99655,-0.99680,-0.99704,-0.99725,-0.99747,-0.99768,-0.99789,-0.99808,-0.99826,-0.99844,-0.99860,-0.99878,-0.99890,-0.99905,-0.99918,-0.99930,-0.99939,-0.99951,-0.99960,-0.99966,-0.99973,-0.99979,-0.99985,-0.99991,-0.99994,-0.99994,-0.99997,
//    -0.99997,-0.99997,-0.99994,-0.99994,-0.99991,-0.99985,-0.99979,-0.99973,-0.99966,-0.99960,-0.99951,-0.99939,-0.99930,-0.99918,-0.99905,-0.99890,-0.99878,-0.99860,-0.99844,-0.99826,-0.99808,-0.99789,-0.99768,-0.99747,-0.99725,-0.99704,-0.99680,-0.99655,-0.99628,-0.99600,-0.99573,-0.99545,-0.99515,-0.99484,-0.99454,-0.99420,-0.99387,-0.99353,-0.99319,-0.99283,-0.99246,-0.99207,-0.99167,-0.99127,-0.99088,-0.99045,-0.99002,-0.98959,-0.98914,-0.98868,-0.98822,-0.98776,-0.98727,-0.98679,-0.98627,-0.98578,-0.98526,-0.98471,-0.98419,-0.98364,-0.98306,-0.98251,-0.98193,-0.98135,-0.98074,-0.98016,-0.97955,-0.97891,-0.97830,-0.97766,-0.97699,-0.97635,-0.97568,-0.97501,-0.97430,-0.97360,-0.97290,-0.97220,-0.97147,-0.97073,-0.97000,-0.96924,-0.96851,-0.96771,-0.96695,-0.96616,-0.96536,-0.96457,-0.96375,-0.96292,-0.96210,-0.96124,-0.96039,-0.95953,-0.95868,-0.95779,-0.95691,-0.95602,-0.95511,-0.95419,-0.95328,-0.95233,-0.95142,-0.95047,-0.94949,-0.94855,-0.94757,-0.94656,-0.94559,-0.94458,-0.94357,-0.94254,-0.94153,-0.94049,-0.93942,-0.93839,-0.93732,-0.93625,-0.93515,-0.93405,-0.93295,-0.93185,-0.93073,-0.92963,-0.92847,-0.92734,-0.92618,-0.92502,
//    -0.92386,-0.92267,-0.92148,-0.92029,-0.91910,-0.91788,-0.91666,-0.91544,-0.91418,-0.91293,-0.91168,-0.91040,-0.90915,-0.90787,-0.90656,-0.90527,-0.90396,-0.90265,-0.90131,-0.89999,-0.89865,-0.89731,-0.89594,-0.89456,-0.89319,-0.89182,-0.89041,-0.88901,-0.88760,-0.88620,-0.88477,-0.88333,-0.88190,-0.88043,-0.87900,-0.87753,-0.87604,-0.87457,-0.87308,-0.87158,-0.87006,-0.86853,-0.86703,-0.86548,-0.86395,-0.86240,-0.86084,-0.85928,-0.85770,-0.85611,-0.85452,-0.85294,-0.85132,-0.84970,-0.84808,-0.84647,-0.84482,-0.84317,-0.84152,-0.83987,-0.83820,-0.83652,-0.83484,-0.83313,-0.83145,-0.82974,-0.82803,-0.82629,-0.82455,-0.82281,-0.82108,-0.81934,-0.81757,-0.81580,-0.81403,-0.81223,-0.81042,-0.80862,-0.80682,-0.80499,-0.80319,-0.80136,-0.79950,-0.79767,-0.79581,-0.79395,-0.79208,-0.79019,-0.78833,-0.78644,-0.78455,-0.78262,-0.78070,-0.77878,-0.77686,-0.77493,-0.77298,-0.77103,-0.76907,-0.76712,-0.76514,-0.76315,-0.76117,-0.75919,-0.75717,-0.75519,-0.75317,-0.75113,-0.74911,-0.74707,-0.74503,-0.74298,-0.74094,-0.73886,-0.73679,-0.73471,-0.73264,-0.73053,-0.72845,-0.72635,-0.72421,-0.72211,-0.71997,-0.71783,-0.71570,-0.71356,-0.71140,-0.70926,
//    -0.70709,-0.70493,-0.70273,-0.70056,-0.69836,-0.69617,-0.69394,-0.69174,-0.68951,-0.68729,-0.68506,-0.68283,-0.68057,-0.67831,-0.67609,-0.67380,-0.67154,-0.66925,-0.66699,-0.66470,-0.66238,-0.66010,-0.65778,-0.65549,-0.65317,-0.65082,-0.64850,-0.64615,-0.64380,-0.64145,-0.63910,-0.63675,-0.63437,-0.63199,-0.62961,-0.62723,-0.62485,-0.62244,-0.62003,-0.61761,-0.61520,-0.61279,-0.61035,-0.60794,-0.60550,-0.60306,-0.60059,-0.59814,-0.59567,-0.59320,-0.59073,-0.58826,-0.58578,-0.58328,-0.58081,-0.57831,-0.57581,-0.57327,-0.57077,-0.56824,-0.56570,-0.56317,-0.56064,-0.55811,-0.55554,-0.55301,-0.55045,-0.54788,-0.54532,-0.54272,-0.54016,-0.53757,-0.53497,-0.53238,-0.52979,-0.52719,-0.52457,-0.52197,-0.51935,-0.51672,-0.51410,-0.51144,-0.50882,-0.50616,-0.50351,-0.50089,-0.49820,-0.49554,-0.49289,-0.49020,-0.48755,-0.48486,-0.48218,-0.47949,-0.47678,-0.47409,-0.47137,-0.46869,-0.46597,-0.46326,-0.46051,-0.45779,-0.45508,-0.45233,-0.44958,-0.44687,-0.44412,-0.44135,-0.43860,-0.43585,-0.43307,-0.43033,-0.42755,-0.42477,-0.42200,-0.41919,-0.41641,-0.41364,-0.41083,-0.40802,-0.40524,-0.40244,-0.39960,-0.39679,-0.39398,-0.39114,-0.38834,-0.38550,
//    -0.38266,-0.37982,-0.37698,-0.37415,-0.37131,-0.36847,-0.36560,-0.36273,-0.35989,-0.35703,-0.35416,-0.35129,-0.34842,-0.34552,-0.34265,-0.33975,-0.33688,-0.33398,-0.33109,-0.32819,-0.32529,-0.32239,-0.31949,-0.31659,-0.31366,-0.31076,-0.30783,-0.30493,-0.30200,-0.29907,-0.29614,-0.29321,-0.29028,-0.28735,-0.28439,-0.28146,-0.27850,-0.27557,-0.27261,-0.26965,-0.26669,-0.26373,-0.26077,-0.25781,-0.25485,-0.25189,-0.24893,-0.24594,-0.24298,-0.23999,-0.23703,-0.23404,-0.23105,-0.22806,-0.22507,-0.22208,-0.21909,-0.21609,-0.21310,-0.21011,-0.20709,-0.20410,-0.20111,-0.19809,-0.19510,-0.19208,-0.18906,-0.18604,-0.18304,-0.18002,-0.17700,-0.17398,-0.17096,-0.16794,-0.16492,-0.16190,-0.15884,-0.15582,-0.15280,-0.14975,-0.14673,-0.14368,-0.14066,-0.13760,-0.13458,-0.13153,-0.12848,-0.12546,-0.12241,-0.11935,-0.11630,-0.11328,-0.11023,-0.10718,-0.10413,-0.10107,-0.09802,-0.09497,-0.09192,-0.08884,-0.08578,-0.08273,-0.07968,-0.07663,-0.07355,-0.07050,-0.06744,-0.06439,-0.06131,-0.05826,-0.05521,-0.05212,-0.04907,-0.04599,-0.04294,-0.03989,-0.03680,-0.03375,-0.03067,-0.02762,-0.02454,-0.02148,-0.01840,-0.01535,-0.01227,-0.00922,-0.00613,-0.00308
//
//};
//
///* ************************************************************************************
// *                             WESPION Motor Control Function                         *
// * ************************************************************************************/
//int Debug_MotorErrChk = 0;
//int Debug_MtrCtrl_Alternating = 0;
//int Check_MtrErrChk = 0;
//void WESPION_MotorControl(void) // It's in ADC_IRQHandler Function
//{   // No control 14.2us, 1 motor control only 10us
////  digitalWrite(57, 1);
////  digitalWrite(87, 1);
//  InvAdcGet();
////  digitalWrite(57, 0);
////  digitalWrite(87, 1);
//  MotorCtrl_FeedbackCal();
////  digitalWrite(57, 0);
////  digitalWrite(87, 0);
////  digitalWrite(87, 1);
//  if (Debug_MotorErrChk ==1) {
//    if(WP_Ctrl.AdOffsetFlg==1)
//    {
//      Check_MtrErrChk++;
//      MotorControl_ErrChk(&WP_CtrlErr.Motor1, &WP_ErrRef, &Motor1.Fb, &Motor1_Ang, &Inv.Ctrl.AD1);
//      MotorControl_ErrChk(&WP_CtrlErr.Motor2, &WP_ErrRef, &Motor2.Fb, &Motor2_Ang, &Inv.Ctrl.AD2);
//      Controller_ErrChk(&WP_CtrlErr, &WP_ErrRef, &Inv.Ctrl);
//    }
//  }
////    digitalWrite(87, 0);
////  digitalWrite(88, 1);
//  MotorCtrl_StateMachine();
////  Debug_MtrCtrl_Alternating++;
////  digitalWrite(88, 0);
//}
//
//inline void WESPION_MotorAddon(void) // It's in main while loop
//{
//  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q);
//  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q);
//}
//
///*==========================================================================================================*/
//inline void MotorCtrl_StateMachine(void)
//{
//  switch(WP_Ctrl.Mode)
//  {
//    case Inv_Off:
//      if(WP_Ctrl.AdOffsetFlg==1)
//      {
//        WP_Ctrl.Mode = Inv_Init;
//      }
//      else{}
////      Motor1.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
////      Motor1.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
////      Motor1.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;
////      Motor2.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
////      Motor2.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
////      Motor2.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;
////      WP_CtrlErr.Motor1.PWM = WP_PWMout(MOTOR1, Motor1.Fb.PWMcnt);
////      WP_CtrlErr.Motor2.PWM = WP_PWMout(MOTOR2, Motor2.Fb.PWMcnt);
////      Motor1En(WP_Gate_EN);
////      Motor2En(WP_Gate_EN);
////      Motor1Ctrl_PwmStart();
////      Motor2Ctrl_PwmStart();
//      Motor1En(WP_Gate_DIS);
//      Motor2En(WP_Gate_DIS);
////      Motor1Ctrl_PwmStop();
////      Motor2Ctrl_PwmStop();
//      WP_Ctrl.ActiveMotor = Drv_NoMotor;
//
//      break;
//    case Inv_Init:      // 1번 모드 - 초기화
//      Motor1En(WP_Gate_DIS);
//      Motor2En(WP_Gate_DIS);
////      Motor1Ctrl_PwmStop();
////      Motor2Ctrl_PwmStop();
//      WP_Ctrl.ActiveMotor = Drv_NoMotor;
//      Motor1Ctrl_VariSetZero();
//      Motor2Ctrl_VariSetZero();
//      WP_Ctrl.Mode = Inv_Ready;
//    break;
//    case Inv_Ready:     // 2번 모드 - 구동 대기
//      Motor1Ctrl_VariSetZero();
//      Motor2Ctrl_VariSetZero();
//      WP_Ctrl.ActiveMotor = Drv_NoMotor;
//      break;
//    case Inv_VbyFCtrl:    // 3번 모드 - V by F
//
//    case Inv_IbyFCtrl:    // 4번 모드 - I by F
//
//    case Inv_TorqueCtrl:  // 5번 모드 - FOC !!
//
//    case Inv_TorqueWeakCtrl:  // 6번 모드 - FOC with field weak control !!
//      MotorCurControl();
//      break;
//    case Inv_SpeedCtrl:   // 7번 모드 - 속도 제어
//      MotorSpdControl();
//      break;
//    case Inv_Fault:     // 8번 모드 - 에러 발생 시 진입
//      WP_Ctrl.ActiveMotor = Drv_NoMotor;
////      Motor1Ctrl_PwmStop();
////      Motor2Ctrl_PwmStop();
//      Motor1En(WP_Gate_DIS);
//      Motor2En(WP_Gate_DIS);
//      Motor1Ctrl_VariSetZero();
//      Motor2Ctrl_VariSetZero();
//      break;
//    default:
//      Motor1En(WP_Gate_DIS);
//      Motor2En(WP_Gate_DIS);
////      Motor1Ctrl_PwmStop();
////      Motor2Ctrl_PwmStop();
//      WP_Ctrl.Mode = Inv_Off;
//      break;
//  }
//}
//
//inline void MotorControl_ErrChk(WP_MotorControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_MotorAxisTypeDef *_fb, WP_MotorAngleTypeDef *_ang, WP_InverterAdOffsetTypeDef *_off)
//{
//  if(_off->iRaw.U > _errRef->IsensorOpen)
//  {
//    _err->CurrentSensorOpenU = WP_ERR;
//  }
//  if(_off->iRaw.V > _errRef->IsensorOpen)
//  {
//    _err->CurrentSensorOpenV = WP_ERR;
//  }
//  if(_off->iRaw.W > _errRef->IsensorOpen)
//  {
//    _err->CurrentSensorOpenW = WP_ERR;
//  }
//  if(_off->iRaw.U < _errRef->IsensorShort)
//  {
//    _err->CurrentSensorShortU = WP_ERR;
//  }
//  if(_off->iRaw.V < _errRef->IsensorShort)
//  {
//    _err->CurrentSensorShortV = WP_ERR;
//  }
//  if(_off->iRaw.W < _errRef->IsensorShort)
//  {
//    _err->CurrentSensorShortW = WP_ERR;
//  }
//
//  if(_off->iOffset.U < _errRef->IsensorOffsetMin)
//  {
//    _err->CurrentSensorOffsetU = WP_ERR;
//  }
//  else if(_off->iOffset.U > _errRef->IsensorOffsetMax)
//  {
//    _err->CurrentSensorOffsetU = WP_ERR;
//  }
//  if(_off->iOffset.V < _errRef->IsensorOffsetMin)
//  {
//    _err->CurrentSensorOffsetV = WP_ERR;
//  }
//  else if(_off->iOffset.V > _errRef->IsensorOffsetMax)
//  {
//    _err->CurrentSensorOffsetV = WP_ERR;
//  }
//  if(_off->iOffset.W < _errRef->IsensorOffsetMin)
//  {
//    _err->CurrentSensorOffsetW = WP_ERR;
//  }
//  else if(_off->iOffset.W > _errRef->IsensorOffsetMax)
//  {
//    _err->CurrentSensorOffsetW = WP_ERR;
//  }
//
//  if(Abs(_fb->I3.U) > _errRef->OverCurrent)
//  {
//    _err->OverCurrentU = WP_ERR;
//  }
//  if(Abs(_fb->I3.U) > _errRef->OverCurrent)
//  {
//    _err->OverCurrentV = WP_ERR;
//  }
//  if(Abs(_fb->I3.U) > _errRef->OverCurrent)
//  {
//    _err->OverCurrentW = WP_ERR;
//  }
//  if(Abs(_fb->Ie.d) > _errRef->OverCurrent)
//  {
//    _err->OverCurrentD = WP_ERR;
//  }
//  if(Abs(_fb->Ie.q) > _errRef->OverCurrent)
//  {
//    _err->OverCurrentQ = WP_ERR;
//  }
//
//  if(Abs(_ang->RpmFil) > _errRef->OverSpeed)
//  {
//    _err->OverSpd = WP_ERR;
//  }
//}
//
//inline void Controller_ErrChk(WP_ControllerErrorTypeDef *_err, WP_ErrorValueTypeDef *_errRef, WP_InverterControlTypeDef *_inv)
//{
//  if(_inv->VdcFil > _errRef->OverVoltage)
//  {
//    _err->OverV = WP_ERR;
//  }
//  if(_inv->VdcFil < _errRef->UnderVoltage)
//  {
//    _err->UnderV = WP_ERR;
//  }
//}
//int Check_Motor1_CurrCtrl, Check_Motor2_CurrCtrl;
//inline void MotorCurControl(void)
//{
//  switch(WP_Ctrl.ActiveMotor&0x1)   // Motor1 Test
//  {
//    case WP_RUN:
////      Motor1Ctrl_PwmStart();
//      if ((Debug_MtrCtrl_Alternating+1) % 2) {
//        Motor1CurCtrl();
//        Check_Motor1_CurrCtrl++;
//      }
////      Motor1CurCtrl();
//
//      Motor1En(WP_Gate_EN);
//      break;
//    case WP_STOP:
//      Motor1Ctrl_VariSetZero();
//      Motor1En(WP_Gate_DIS);
////      Motor1Ctrl_PwmStop();
//      break;
//    default:
//      Motor1Ctrl_VariSetZero();
//      Motor1En(WP_Gate_DIS);
////      Motor1Ctrl_PwmStop();
//      break;
//  }
//  switch((WP_Ctrl.ActiveMotor>>1)&0x1)  // Motor2 Test
//  {
//    case WP_RUN:
////      Motor2Ctrl_PwmStart();
//      if (Debug_MtrCtrl_Alternating % 2) {
//        Motor2CurCtrl();
//        Check_Motor2_CurrCtrl++;
//      }
////      Motor2CurCtrl();
//
//      Motor2En(WP_Gate_EN);
//      break;
//    case WP_STOP:
//      Motor2Ctrl_VariSetZero();
//      Motor2En(WP_Gate_DIS);
////      Motor2Ctrl_PwmStop();
//      break;
//    default:
//      Motor2Ctrl_VariSetZero();
//      Motor2En(WP_Gate_DIS);
////      Motor2Ctrl_PwmStop();
//      break;
//  }
//
//  Debug_MtrCtrl_Alternating++;
//
//}
//
//inline void Motor1CurCtrl(void)
//{
//  WP_RampUpDn(&Motor1.Cmd.Icmd.q, &Motor1.Cmd.Iref.q, 1.0f, 1.0f);
//  if(WP_Ctrl.Mode==Inv_TorqueWeakCtrl)
//  {
//#if FIELDWEAK_MODE == FIELDWEAK_MOT
//    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor1.Fb.VphaseFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);
//#endif
//#if FIELDWEAK_MODE == FIELDWEAK_GEN
//    Motor1.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor1.PiFweak, &Motor1.GainFweak, Inv.Ctrl.IlimFweak);
//#endif
//  }
//  else
//  {
//    WP_RampUpDn(&Motor1.Cmd.Icmd.d, &Motor1.Cmd.Iref.d, 1.0f, 1.0f);
//  }
//  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_Weight.FfScale.Temp[MOTOR_L];// + Motor1.Para.Ld * Motor1.Fb.Ie.d * Motor1_Ang.WeFil;
////  Motor1.PiCurQ.Ff = Motor1.Para.PhaiF * Motor1_Ang.RpmFil * WP_WeightFeel.FfScale_L;
////  Motor1.PiCurD.Ff = - Motor1.Para.Lq * Motor1.Fb.Ie.q * Motor1_Ang.WeFil;
//  Motor1.Fb.VeRef.q = WP_PIcontrol(Motor1.Cmd.Iref.q, Motor1.Fb.Ie.q, &Motor1.PiCurQ, &Motor1.GainCurQ, Inv.Ctrl.VphaseLim);
//  Motor1.Fb.VeRef.d = WP_PIcontrol(Motor1.Cmd.Iref.d, Motor1.Fb.Ie.d, &Motor1.PiCurD, &Motor1.GainCurD, Inv.Ctrl.VphaseLim);
//  Motor1.Fb.VphaseRef = sqrtf(Motor1.Fb.VeRef.d * Motor1.Fb.VeRef.d + Motor1.Fb.VeRef.q * Motor1.Fb.VeRef.q); // 700ns
//  LPF(Motor1.Fb.VphaseFil, Motor1.Fb.VphaseRef, LpfVdc.fct);
//  Motor1.Fb.VphaseSet = 1.0f / Motor1.Fb.VphaseFil;   // 역수 취한 값
//  switch(WP_Ctrl.Mode)
//  {
//    case Inv_VbyFCtrl:
//      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Cmd.Vfcmd, &Motor1_Ang);
//      break;
//    default:
//      if(Motor1.Fb.VphaseFil > Inv.Ctrl.VphaseLim)
//      {
//        Motor1.Fb.VeSet.d = Motor1.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;
//        Motor1.Fb.VeSet.q = Motor1.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor1.Fb.VphaseSet;
//      }
//      else
//      {
//        Motor1.Fb.VeSet.d = Motor1.Fb.VeRef.d;
//        Motor1.Fb.VeSet.q = Motor1.Fb.VeRef.q;
//      }
//      WP_InvClarkeParkTrans(&Motor1.Fb.V3Set, &Motor1.Fb.VsSet, &Motor1.Fb.VeSet, &Motor1_Ang);
//      break;
//  }
//  /* Limit Function Add */
//  WP_InvMinMax(&Motor1.Fb.V3Set, &Motor1.Fb.V3, Inv.Ctrl.VphLim);
//  WP_ClarkeParkTrans(&Motor1.Fb.V3, &Motor1.Fb.Vs, &Motor1.Fb.Ve, &Motor1_Ang);
//  Motor1.PiCurQ.AW = Motor1.Fb.VeRef.q - Motor1.Fb.Ve.q;
//  Motor1.PiCurD.AW = Motor1.Fb.VeRef.d - Motor1.Fb.Ve.d;
//  WP_PwmScale(Motor1.Fb.PWMcnt, &Motor1.Fb.V3, Motor1.Fb.Dutyratio, &Inv);
//  WP_CtrlErr.Motor1.PWM = WP_PWMout(MOTOR1, Motor1.Fb.PWMcnt);
//}
//
//inline void Motor2CurCtrl(void)
//{
//  WP_RampUpDn(&Motor2.Cmd.Icmd.q, &Motor2.Cmd.Iref.q, 1.0f, 1.0f);
//  if(WP_Ctrl.Mode==Inv_TorqueWeakCtrl)
//  {
//#if FIELDWEAK_MODE == FIELDWEAK_MOT
//    Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VlimFweak, Motor2.Fb.VphaseFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);
//#endif
//#if FIELDWEAK_MODE == FIELDWEAK_GEN
//    Motor2.Cmd.Iref.d = WP_PIcontrolForFieldWeak(Inv.Ctrl.VdcLimFweak, Inv.Ctrl.VdcFil, &Motor2.PiFweak, &Motor2.GainFweak, Inv.Ctrl.IlimFweak);
//#endif
//  }
//  else
//  {
//    WP_RampUpDn(&Motor2.Cmd.Icmd.d, &Motor2.Cmd.Iref.d, 1.0f, 1.0f);
//  }
//  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_Weight.FfScale.Temp[MOTOR_R];// + Motor2.Para.Ld * Motor2.Fb.Ie.d * Motor2_Ang.WeFil;
////  Motor2.PiCurQ.Ff = Motor2.Para.PhaiF * Motor2_Ang.RpmFil * WP_WeightFeel.FfScale_R;
////  Motor2.PiCurD.Ff = - Motor2.Para.Lq * Motor2.Fb.Ie.q * Motor2_Ang.WeFil;
//  Motor2.Fb.VeRef.q = WP_PIcontrol(Motor2.Cmd.Iref.q, Motor2.Fb.Ie.q, &Motor2.PiCurQ, &Motor2.GainCurQ, Inv.Ctrl.VphaseLim);
//  Motor2.Fb.VeRef.d = WP_PIcontrol(Motor2.Cmd.Iref.d, Motor2.Fb.Ie.d, &Motor2.PiCurD, &Motor2.GainCurD, Inv.Ctrl.VphaseLim);
//  Motor2.Fb.VphaseRef = sqrtf(Motor2.Fb.VeRef.d * Motor2.Fb.VeRef.d + Motor2.Fb.VeRef.q * Motor2.Fb.VeRef.q); // 700ns
//  LPF(Motor2.Fb.VphaseFil, Motor2.Fb.VphaseRef, LpfVdc.fct);
//  Motor2.Fb.VphaseSet = 1.0f / Motor2.Fb.VphaseFil;
//  switch(WP_Ctrl.Mode)
//  {
//    case Inv_VbyFCtrl:
//      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Cmd.Vfcmd, &Motor2_Ang);
//      break;
//    default:
//      if(Motor2.Fb.VphaseFil > Inv.Ctrl.VphaseLim)
//      {
//        Motor2.Fb.VeSet.d = Motor2.Fb.VeRef.d * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;
//          Motor2.Fb.VeSet.q = Motor2.Fb.VeRef.q * Inv.Ctrl.VphaseLim * Motor2.Fb.VphaseSet;
//      }
//      else
//      {
//        Motor2.Fb.VeSet.d = Motor2.Fb.VeRef.d;
//        Motor2.Fb.VeSet.q = Motor2.Fb.VeRef.q;
//      }
//      WP_InvClarkeParkTrans(&Motor2.Fb.V3Set, &Motor2.Fb.VsSet, &Motor2.Fb.VeSet, &Motor2_Ang);
//      break;
//  }
//  /* Limit Function Add */
//  WP_InvMinMax(&Motor2.Fb.V3Set, &Motor2.Fb.V3, Inv.Ctrl.VphLim);
//  WP_ClarkeParkTrans(&Motor2.Fb.V3, &Motor2.Fb.Vs, &Motor2.Fb.Ve, &Motor2_Ang);
//  Motor2.PiCurQ.AW = Motor2.Fb.VeRef.q - Motor2.Fb.Ve.q;
//  Motor2.PiCurD.AW = Motor2.Fb.VeRef.d - Motor2.Fb.Ve.d;
//  WP_PwmScale(Motor2.Fb.PWMcnt, &Motor2.Fb.V3, Motor2.Fb.Dutyratio, &Inv);
//  WP_CtrlErr.Motor2.PWM = WP_PWMout(MOTOR2, Motor2.Fb.PWMcnt);
//}
//
//inline void MotorSpdControl(void)
//{
//  switch(WP_Ctrl.ActiveMotor&0x1)   // Motor1 Test
//  {
//    case WP_RUN:
////      Motor1Ctrl_PwmStart();
//      if(Motor1_Ang._1msFlg == 1)
//      {
//        Motor1SpdCtrl();
//        Motor1_Ang._1msFlg = 0;
//      }
//      Motor1CurCtrl();
//      Motor1En(WP_Gate_EN);
//      break;
//    case WP_STOP:
//      Motor1Ctrl_VariSetZero();
//      Motor1En(WP_Gate_DIS);
////      Motor1Ctrl_PwmStop();
//      break;
//    default:
//      Motor1Ctrl_VariSetZero();
//      Motor1En(WP_Gate_DIS);
////      Motor1Ctrl_PwmStop();
//      break;
//  }
//  switch((WP_Ctrl.ActiveMotor>>1)&0x1)  // Motor2 Test
//  {
//    case WP_RUN:
////      Motor2Ctrl_PwmStart();
//      if(Motor2_Ang._1msFlg == 1)
//      {
//        Motor2SpdCtrl();
//        Motor2_Ang._1msFlg = 0;
//      }
//      Motor2CurCtrl();
//      Motor2En(WP_Gate_EN);
//      break;
//    case WP_STOP:
//      Motor2Ctrl_VariSetZero();
//      Motor2En(WP_Gate_DIS);
////      Motor2Ctrl_PwmStop();
//      break;
//    default:
//      Motor2Ctrl_VariSetZero();
//      Motor2En(WP_Gate_DIS);
////      Motor2Ctrl_PwmStop();
//      break;
//  }
//}
//
//inline void Motor1SpdCtrl(void)
//{
//  WP_RampUpDn(&Motor1.Cmd.RpmCmd, &Motor1.Cmd.RpmRef, 10.0f, 10.0f);
//  Motor1.Cmd.Icmd.q = WP_PIcontrol(Motor1.Cmd.RpmRef, Motor1_Ang.RpmFil, &Motor1.PiSpd, &Motor1.GainSpd, Motor1_Ang.SpdCtrlOutLim);
//}
//
//inline void Motor2SpdCtrl(void)
//{
//  WP_RampUpDn(&Motor2.Cmd.RpmCmd, &Motor2.Cmd.RpmRef, 10.0f, 10.0f);
//  Motor2.Cmd.Icmd.q = WP_PIcontrol(Motor2.Cmd.RpmRef, Motor2_Ang.RpmFil, &Motor2.PiSpd, &Motor2.GainSpd, Motor2_Ang.SpdCtrlOutLim);
//}
//
///*==========================================================================================================*/
//inline void InvAdcGet(void)
//{
//  ADinj.Raw[0][0] = ADC_Inject[0][0]; // AD_IU0;
//  ADinj.Raw[0][1] = ADC_Inject[0][1]; // AD_IU1;
//  ADinj.Raw[0][2] = ADC_Inject[0][2]; // AD_SIN0;
//  ADinj.Raw[0][3] = ADC_Inject[0][3]; // AD_SIN1;
//  ADinj.Raw[1][0] = ADC_Inject[1][0]; // AD_IV0;
//  ADinj.Raw[1][1] = ADC_Inject[1][1]; // AD_IV1;
//  ADinj.Raw[1][2] = ADC_Inject[1][2]; // AD_COS0;
//  ADinj.Raw[1][3] = ADC_Inject[1][3]; // AD_COS1;
//  ADinj.Raw[2][0] = ADC_Inject[2][0]; // AD_IW0;
//  ADinj.Raw[2][1] = ADC_Inject[2][1]; // AD_IW1;
//  ADinj.Raw[2][2] = ADC_Inject[2][2]; // AD_VDC;
//  ADinj.Raw[2][3] = ADC_Inject[2][3]; // AD_EXT0;
//}
//
//inline void MotorCtrl_FeedbackCal(void)
//{
//
////  digitalWrite(57, 1);
//  Inv.Ctrl.Vdc = ADinj.Raw[2][2] * Inv.Para.VdcScale;
//
//  Inv.Ctrl.AD1.iRaw.U = ADinj.Raw[0][0];
//  Inv.Ctrl.AD2.iRaw.U = ADinj.Raw[0][1];
//  Inv.Ctrl.AD1.iRaw.V = ADinj.Raw[1][0];
//  Inv.Ctrl.AD2.iRaw.V = ADinj.Raw[1][1];
//  Inv.Ctrl.AD1.iRaw.W = ADinj.Raw[2][0];
//  Inv.Ctrl.AD2.iRaw.W = ADinj.Raw[2][1];
//
//  switch(WP_Ctrl.AdOffsetFlg)
//  {
//    case WP_STOP:
//      WP_Ctrl.AdOffsetCnt++;
//      if(WP_Ctrl.AdOffsetCnt > 5000)
//      {
//        WP_Ctrl.AdOffsetFlg = WP_RUN;
//        WP_Ctrl.AdOffsetCnt=5001;
//      }
//      else
//      {
//        WP_Ctrl.Mode=0;
//        WP_Ctrl.ActiveMotor=0;
//        WP_Ctrl.AdOffsetFlg=0;
//        LPF(Inv.Ctrl.AD1.iOffset.U, Inv.Ctrl.AD1.iRaw.U, LpfCur.fct);
//        LPF(Inv.Ctrl.AD1.iOffset.V, Inv.Ctrl.AD1.iRaw.V, LpfCur.fct);
//        LPF(Inv.Ctrl.AD1.iOffset.W, Inv.Ctrl.AD1.iRaw.W, LpfCur.fct);
//        LPF(Inv.Ctrl.AD2.iOffset.U, Inv.Ctrl.AD2.iRaw.U, LpfCur.fct);
//        LPF(Inv.Ctrl.AD2.iOffset.V, Inv.Ctrl.AD2.iRaw.V, LpfCur.fct);
//        LPF(Inv.Ctrl.AD2.iOffset.W, Inv.Ctrl.AD2.iRaw.W, LpfCur.fct);
//      }
//      break;
//    case WP_RUN:
//
//      break;
//    default:
//      WP_CtrlErr.AdOffset = WP_ERR;
//  }
//
//  Motor1.Fb.I3.U = (Inv.Ctrl.AD1.iRaw.U - Inv.Ctrl.AD1.iOffset.U) * Inv.Para.iScale;
//  Motor1.Fb.I3.V = (Inv.Ctrl.AD1.iRaw.V - Inv.Ctrl.AD1.iOffset.V) * Inv.Para.iScale;
//  Motor1.Fb.I3.W = (Inv.Ctrl.AD1.iRaw.W - Inv.Ctrl.AD1.iOffset.W) * Inv.Para.iScale;
////  Motor1.Fb.I3.W = - Motor1.Fb.I3.U - Motor1.Fb.I3.V;
//  Motor2.Fb.I3.U = (Inv.Ctrl.AD2.iRaw.U - Inv.Ctrl.AD2.iOffset.U) * Inv.Para.iScale;
//  Motor2.Fb.I3.V = (Inv.Ctrl.AD2.iRaw.V - Inv.Ctrl.AD2.iOffset.V) * Inv.Para.iScale;
//  Motor2.Fb.I3.W = (Inv.Ctrl.AD2.iRaw.W - Inv.Ctrl.AD2.iOffset.W) * Inv.Para.iScale;
////  Motor2.Fb.I3.W = - Motor2.Fb.I3.U - Motor2.Fb.I3.V;
//
//  Motor1_Ang.MR.Raw.Sin = ADinj.Raw[0][2]; // AD_SIN0;
//  Motor2_Ang.MR.Raw.Sin = ADinj.Raw[0][3]; // AD_SIN1;
//  Motor1_Ang.MR.Raw.Cos = ADinj.Raw[1][2]; // AD_COS0;
//  Motor2_Ang.MR.Raw.Cos = ADinj.Raw[1][3]; // AD_COS1;
//
////  digitalWrite(57, 0);
////  digitalWrite(87, 1);
//#if ANGLE_MODE == ANGLE_MR    // 누적앵글 구하는 코드 없는 상태
//  WP_MrPos(&Motor1_Ang);
//  WP_MrPos(&Motor2_Ang);
//#endif
//#if ANGLE_MODE == ANGLE_ENC
//  WP_CtrlErr.Motor1.Speed = WP_EncSpd(MOTOR1, &Motor1_Ang);
//  WP_CtrlErr.Motor2.Speed = WP_EncSpd(MOTOR2, &Motor2_Ang);
//#endif
//
////  digitalWrite(87, 0);
////  digitalWrite(88, 1);
//  LPF(Inv.Ctrl.VdcFil, Inv.Ctrl.Vdc, LpfVdc.fct);
//
//  Inv.Ctrl.VphaseLim = Inv.Ctrl.VdcFil * MT_SQ3_OVR_2 * Inv.Para.VdcMargin;
//  Inv.Ctrl.VlimFweak = Inv.Ctrl.VphaseLim * 0.8f;
//  Inv.Ctrl.Vdc_2 = Inv.Ctrl.VdcFil * MT_1_OVR_2;
//  Inv.Ctrl.VphLim = Inv.Ctrl.Vdc_2 * Inv.Para.VdcMargin;
//  Inv.Ctrl._Vdc_2 = 1.0f / Inv.Ctrl.Vdc_2;
//  Inv.Ctrl.PWM_Scale = Inv.Para.PWM_Mid * Inv.Ctrl._Vdc_2;
//
//  MotorCtrl_TransformAngle(&Motor1_Ang, Motor1.Cmd.XbyF_Freq, Inv.Para.Tsamp);
//  WP_ClarkeParkTrans(&Motor1.Fb.I3, &Motor1.Fb.Is, &Motor1.Fb.Ie, &Motor1_Ang);
//  MotorCtrl_TransformAngle(&Motor2_Ang, Motor2.Cmd.XbyF_Freq, Inv.Para.Tsamp);
//  WP_ClarkeParkTrans(&Motor2.Fb.I3, &Motor2.Fb.Is, &Motor2.Fb.Ie, &Motor2_Ang);
//
//  Inverter_Comm1ms();
////  digitalWrite(88, 0);
//}
//
//inline void MotorCtrl_TransformAngle(WP_MotorAngleTypeDef *angle, float cmd, float tsamp)
//{
//  uint32_t array_theta=0;
//
//  switch(WP_Ctrl.Mode)
//  {
//    case Inv_IbyFCtrl:    // 3번 모드 - I by F
//    case Inv_VbyFCtrl:    // 4번 모드 - V by F
//      MotorCtrl_XbyF_Ctrl(cmd, &angle->XbyFAngle, tsamp);
//      array_theta = (uint32_t)(2048*(angle->XbyFAngle)*MT_1_OVR_360);
//      break;
//    default:
//      array_theta = (uint32_t)(2048*(angle->AngleElec)*MT_1_OVR_360);
//      break;
//  }
//
//  angle->Tr_Sin = sinTab[((array_theta    )&0x7FF)];
//  angle->Tr_Cos = sinTab[((array_theta+511)&0x7FF)];
//}
//
//inline void MotorCtrl_XbyF_Ctrl(float fc, float *angle, float SampT)
//{
//  *angle += fc * SampT * 360.0f;//fcmd*0.00001*2*3.141592;
//
//  if(*angle >= 360.0f)   *angle -= 360.0f;
//  else if(*angle < 0)    *angle += 360.0f;
//}
//
///*==========================================================================================================*/
//void Inverter_Comm(void)
//{
//  if(ComAd.Flg==WP_RUN)
//  {
//    InvComAdcGet();
//    Inverter_CommDataCal();
//    ComAd.Flg=WP_STOP;
//    ADC_SWStart();                      //Start Trigger
//  }
//}
//
//inline void Inverter_Comm1ms(void)
//{
//  static uint16_t _1msCnt;
//#if   defined(TIM_8KHZ)
//  uint8_t CntRef=8;
//#elif defined(TIM_10KHZ)
//  uint8_t CntRef=10;
//#elif defined(TIM_16KHZ)
//  uint8_t CntRef=16;
//#elif defined(TIM_20KHZ)
//  uint8_t CntRef=20;
//#else
//  uint8_t CntRef=10;
//#endif
//
//  _1msCnt++;
//  if(_1msCnt > CntRef)
//  {
//    Inverter_CommEnable();
//    _1msCnt=0;
//  }
//  else{}
//}
//
//inline void Inverter_CommEnable(void)
//{ // This Function Run in "HAL_ADC_ConvCpltCallback" in adc.c
//  ComAd.Flg=WP_RUN;
//}
//
//inline void InvComAdcGet(void)
//{
//  ComAd.Raw.VU0 = ADC_Buffer[0];
//  ComAd.Raw.VU1 = ADC_Buffer[1];
//  ComAd.Raw.NTC0 = ADC_Buffer[2];
//  ComAd.Raw.NTC1 = ADC_Buffer[3];
//  ComAd.Raw.EXT1 = ADC_Buffer[4];
//  ComAd.Raw.EXT2 = ADC_Buffer[5];
//  ComAd.Raw.EXT3 = ADC_Buffer[6];
//  ComAd.Raw.EXT4 = ADC_Buffer[7];
//}
//
//inline void Inverter_CommDataCal(void)
//{
//  ComAd.Data.VU0 = ComAd.Raw.VU0 * Inv.Para.HwAdcScale;
//  ComAd.Data.VU1 = ComAd.Raw.VU1 * Inv.Para.HwAdcScale;
//  ComAd.Data.NTC0 = ComAd.Raw.NTC0 * Inv.Para.HwAdcScale;
//  ComAd.Data.NTC1 = ComAd.Raw.NTC1 * Inv.Para.HwAdcScale;
//  ComAd.Data.EXT1 = ComAd.Raw.EXT1 * Inv.Para.HwAdcScale;
//  ComAd.Data.EXT2 = ComAd.Raw.EXT2 * Inv.Para.HwAdcScale;
//  ComAd.Data.EXT3 = ComAd.Raw.EXT3 * Inv.Para.HwAdcScale;
//  ComAd.Data.EXT4 = ComAd.Raw.EXT4 * Inv.Para.HwAdcScale;
//}
//
///*==========================================================================================================*/
//void MotorCtrl_Initialize(void)
//{
//  // Motor Parameters
//  // Gate IC DISABLE Pin Settings
//  pinMode(1, OUTPUT);  // PWM0_EN
//  pinMode(2, OUTPUT);  // PWM1_EN
//  Motor1En(WP_Gate_DIS);
//  Motor2En(WP_Gate_DIS);
//
////  kpQ=3000.*motorLq;
////  kiQ=3000.*motorR*tsamp;
//
//  // Motor1 Parameter Settings
//  Motor1.Para.Pole = 10;     // SAT : 4, JKONG : 8, BS-105-10 : 30
//  Motor1.Para.PolePair = Motor1.Para.Pole>>1;
//  Motor1.Para.Ld = 0.0f;
//  Motor1.Para.Lq = 0.0f;
//  Motor1.Para.Rs = 0.0f;
//  Motor1.Para.PhaiF = 72.2f * 0.001f * MT_1_OVR_SQ3;  // V/krpm
//
//  Motor1.GainCurD.Kp = 1.0f;
//  Motor1.GainCurD.Ki = 0.011f;
//  Motor1.GainCurD.Kaw = 1.0f / Motor1.GainCurD.Kp;
//  Motor1.GainCurQ.Kp = 1.0f;
//  Motor1.GainCurQ.Ki = 0.011f;
//  Motor1.GainCurQ.Kaw = 1.0f / Motor1.GainCurQ.Kp;
//
//  Motor1.GainSpd.Kp = 0.0022f;
//  Motor1.GainSpd.Ki = 0.00001f;
//  Motor1.GainSpd.Kaw = 1.0f / Motor1.GainSpd.Kp;
//
//  Motor1.GainFweak.Kp = 2.0f;
//  Motor1.GainFweak.Ki = 0.002f;
//
//  Motor1_Ang.PolePair = Motor1.Para.PolePair;
//  Motor1_Ang.EncPulse = 4096*4;     //  TLE5012B : 4096, JKONG : 1000
//  Motor1_Ang.AngleScale = 360.0f / (float)Motor1_Ang.EncPulse;
//  Motor1_Ang.SpeedScale = 1000.0f * 60.0f / (float)Motor1_Ang.EncPulse;
////  Motor1_Ang.AngleElecOffset = 37.0f;       // 24.01.05 -> Can be changed by sample modification, check every time!
//  Motor1_Ang.AngleElecOffset = RAM_Data.Motor1_Ang_AngleElecOffset; ///MOTOR1_ANG_ANGLEELCOFFSET
//
//  Motor1_Ang.MR.Max.Sin = 0;
//  Motor1_Ang.MR.Max.Cos = 0;
//  Motor1_Ang.MR.Min.Sin = 4095;
//  Motor1_Ang.MR.Min.Cos = 4095;
//  Motor1_Ang.MR.Mid.Sin = 0;
//  Motor1_Ang.MR.Mid.Cos = 0;
//  Motor1_Ang.MR.Gain.Sin = 1.0f;
//  Motor1_Ang.MR.Gain.Cos = 1.0f;
//
//  // Motor2 Parameter Settings
//  Motor2.Para.Pole = Motor1.Para.Pole;     // SAT : 4, JKONG : 8, BS-105-10 : 30
//  Motor2.Para.PolePair = Motor2.Para.Pole>>1;
//  Motor2.Para.Ld = 0.0f;
//  Motor2.Para.Lq = 0.0f;
//  Motor2.Para.Rs = 0.0f;
//  Motor2.Para.PhaiF = Motor1.Para.PhaiF;  // V/krpm
//
//  Motor2.GainCurD.Kp = Motor1.GainCurD.Kp;
//  Motor2.GainCurD.Ki = Motor1.GainCurD.Ki;
//  Motor2.GainCurD.Kaw = 1.0f / Motor2.GainCurD.Kp;
//  Motor2.GainCurQ.Kp = Motor1.GainCurQ.Kp;
//  Motor2.GainCurQ.Ki = Motor1.GainCurQ.Ki;
//  Motor2.GainCurQ.Kaw = 1.0f / Motor2.GainCurQ.Kp;
//
//  Motor2.GainSpd.Kp = Motor1.GainSpd.Kp;
//  Motor2.GainSpd.Ki = Motor1.GainSpd.Ki;
//  Motor2.GainSpd.Kaw = 1.0f / Motor2.GainSpd.Kp;
//
//  Motor2.GainFweak.Kp = Motor1.GainFweak.Kp;
//  Motor2.GainFweak.Ki = Motor1.GainFweak.Ki;
//
//  Motor2_Ang.PolePair = Motor2.Para.PolePair;
//  Motor2_Ang.EncPulse = Motor1_Ang.EncPulse;     //  TLE5012B : 4096, JKONG : 1000
//  Motor2_Ang.AngleScale = Motor1_Ang.AngleScale;
//  Motor2_Ang.SpeedScale = Motor1_Ang.SpeedScale;
////  Motor2_Ang.AngleElecOffset = 120.0f;       // SAT : 12, JKONG : #1 77 / #2 198.759857f -> inaccurate, check every time!
//  Motor2_Ang.AngleElecOffset = RAM_Data.Motor2_Ang_AngleElecOffset;
//
//  Motor2_Ang.MR.Max.Sin = 0;
//  Motor2_Ang.MR.Max.Cos = 0;
//  Motor2_Ang.MR.Min.Sin = 4095;
//  Motor2_Ang.MR.Min.Cos = 4095;
//  Motor2_Ang.MR.Mid.Sin = 0;
//  Motor2_Ang.MR.Mid.Cos = 0;
//  Motor2_Ang.MR.Gain.Sin = 1.0f;
//  Motor2_Ang.MR.Gain.Cos = 1.0f;
//
//
//  // Inverter Parameters
//  Inv.Para.HwAdcScale = 3.3f / 4095.0f;
//  Inv.Para.HwVdcScale = 1.0f / (30.0f + 1.0f);
//  Inv.Para.HwiScale = 0.002f * 15.0f / 1.0f;
//
//  Inv.Para.VdcScale = Inv.Para.HwAdcScale / Inv.Para.HwVdcScale;
//  Inv.Para.iScale = - Inv.Para.HwAdcScale / Inv.Para.HwiScale;
//
//
//#if   defined(TIM_8KHZ)
//  Inv.Para.Tsamp = 1.0f/8000.0f;
//#elif defined(TIM_10KHZ)
//  Inv.Para.Tsamp = 1.0f/10000.0f;
//#elif defined(TIM_16KHZ)
//  Inv.Para.Tsamp = 1.0f/16000.0f;
//#elif defined(TIM_20KHZ)
//  Inv.Para.Tsamp = 1.0f/20000.0f;
//#else
//  Inv.Para.Tsamp = 1.0f/10000.0f;
//#endif
//
//  Inv.Para.PWM_Max = TIM_PERIOD;
//  Inv.Para.PWM_Mid = ((TIM_PERIOD+1)>>1);
//
//  Inv.Para.VdcMargin = 0.90f;
//
//  LpfVdc.fc = 1000.0f;
//  LpfVdc.fs = 1.0f / Inv.Para.Tsamp;
//  LpfFACTOR(LpfVdc.fc, LpfVdc.fs, LpfVdc.fct);
//
//  LpfCur.fc = 10.0f;
//  LpfCur.fs = 1.0f / Inv.Para.Tsamp;
//  LpfFACTOR(LpfCur.fc, LpfCur.fs, LpfCur.fct);
//
//  LpfSpd.fc = 100.0f;
//  LpfSpd.fs = 1000.0f;
//  LpfFACTOR(LpfSpd.fc, LpfSpd.fs, LpfSpd.fct);
//
//  WP_ErrRef.IsensorOpen = 3908.0f;  // 50A에 해당하는 값
//  WP_ErrRef.IsensorShort = 186.0f;  // -50A에 해당하는 값
//  WP_ErrRef.OverCurrent = 30.0f;
//  WP_ErrRef.OverSpeed = 4000.0;
//  WP_ErrRef.OverVoltage = 70.0;
//  WP_ErrRef.UnderVoltage = 30.0;
//  WP_ErrRef.IsensorOffsetMax = 2048.0f + 50.0f;
//  WP_ErrRef.IsensorOffsetMin = 2048.0f - 50.0f;
//
//  Inv.Ctrl.VdcLimFweak = 49.0f;                               // Generation Field weak Pi Controller Output Limit
//  Inv.Ctrl.IlimFweak = WP_ErrRef.OverCurrent * 0.8f;          // Motoring Field weak Pi controller Output Limit
//
//  Motor1_Ang.RpmLim = WP_ErrRef.OverSpeed * 0.8f;
//  Motor1_Ang.SpdCtrlOutLim = WP_ErrRef.OverCurrent * 0.8f;    // Speed Control Pi Controller Output Limit
//
//  Motor2_Ang.RpmLim = WP_ErrRef.OverSpeed * 0.8f;
//  Motor2_Ang.SpdCtrlOutLim = WP_ErrRef.OverCurrent * 0.8f;    // Speed Control Pi Controller Output Limit
//}
//
//inline void Motor1Ctrl_VariSetZero(void)
//{
//  Motor1.Cmd.Icmd.d = 0.0f;
//  Motor1.Cmd.Icmd.q = 0.0f;
//  Motor1.Cmd.Iref.d = 0.0f;
//  Motor1.Cmd.Iref.q = 0.0f;
//  Motor1.Cmd.IsBeta = 0.0f;
//  Motor1.Cmd.IsCmd = 0.0f;
//  Motor1.Cmd.XbyF_Freq = 0.0f;
//  Motor1.Cmd.Vfcmd.d = 0.0f;
//  Motor1.Cmd.Vfcmd.q = 0.0f;
//
//  Motor1.PiCurD.Err = 0.0f;
//  Motor1.PiCurD.Pterm = 0.0f;
//  Motor1.PiCurD.Iterm = 0.0f;
//  Motor1.PiCurD.PIterm = 0.0f;
//  Motor1.PiCurD.Ff = 0.0f;
//  Motor1.PiCurD.AW = 0.0f;
//  Motor1.PiCurD.Out = 0.0f;
//
//  Motor1.PiCurQ.Err = 0.0f;
//  Motor1.PiCurQ.Pterm = 0.0f;
//  Motor1.PiCurQ.Iterm = 0.0f;
//  Motor1.PiCurQ.PIterm = 0.0f;
//  Motor1.PiCurQ.Ff = 0.0f;
//  Motor1.PiCurQ.AW = 0.0f;
//  Motor1.PiCurQ.Out = 0.0f;
//
//  Motor1.Fb.VeRef.d = 0.0f;
//  Motor1.Fb.VeRef.q = 0.0f;
//  Motor1.Fb.VeSet.d = 0.0f;
//  Motor1.Fb.VeSet.q = 0.0f;
//  Motor1.Fb.V3Set.U = 0.0f;
//  Motor1.Fb.V3Set.V = 0.0f;
//  Motor1.Fb.V3Set.W = 0.0f;
//  Motor1.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
//  Motor1.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
//  Motor1.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;
//
//  Motor1.PiSpd.Err = 0.0f;
//  Motor1.PiSpd.Pterm = 0.0f;
//  Motor1.PiSpd.Iterm = 0.0f;
//  Motor1.PiSpd.PIterm = 0.0f;
//  Motor1.PiSpd.Ff = 0.0f;
//  Motor1.PiSpd.AW = 0.0f;
//  Motor1.PiSpd.Out = 0.0f;
//}
//
//inline void Motor2Ctrl_VariSetZero(void)
//{
//  Motor2.Cmd.Icmd.d = 0.0f;
//  Motor2.Cmd.Icmd.q = 0.0f;
//  Motor2.Cmd.Iref.d = 0.0f;
//  Motor2.Cmd.Iref.q = 0.0f;
//  Motor2.Cmd.IsBeta = 0.0f;
//  Motor2.Cmd.IsCmd = 0.0f;
//  Motor2.Cmd.XbyF_Freq = 0.0f;
//  Motor2.Cmd.Vfcmd.d = 0.0f;
//  Motor2.Cmd.Vfcmd.q = 0.0f;
//
//  Motor2.PiCurD.Err = 0.0f;
//  Motor2.PiCurD.Pterm = 0.0f;
//  Motor2.PiCurD.Iterm = 0.0f;
//  Motor2.PiCurD.PIterm = 0.0f;
//  Motor2.PiCurD.Ff = 0.0f;
//  Motor2.PiCurD.AW = 0.0f;
//  Motor2.PiCurD.Out = 0.0f;
//
//  Motor2.PiCurQ.Err = 0.0f;
//  Motor2.PiCurQ.Pterm = 0.0f;
//  Motor2.PiCurQ.Iterm = 0.0f;
//  Motor2.PiCurQ.PIterm = 0.0f;
//  Motor2.PiCurQ.Ff = 0.0f;
//  Motor2.PiCurQ.AW = 0.0f;
//  Motor2.PiCurQ.Out = 0.0f;
//
//  Motor2.Fb.VeRef.d = 0.0f;
//  Motor2.Fb.VeRef.q = 0.0f;
//  Motor2.Fb.VeSet.d = 0.0f;
//  Motor2.Fb.VeSet.q = 0.0f;
//  Motor2.Fb.V3Set.U = 0.0f;
//  Motor2.Fb.V3Set.V = 0.0f;
//  Motor2.Fb.V3Set.W = 0.0f;
//  Motor2.Fb.PWMcnt[0] = Inv.Para.PWM_Mid;
//  Motor2.Fb.PWMcnt[1] = Inv.Para.PWM_Mid;
//  Motor2.Fb.PWMcnt[2] = Inv.Para.PWM_Mid;
//
//  Motor2.PiSpd.Err = 0.0f;
//  Motor2.PiSpd.Pterm = 0.0f;
//  Motor2.PiSpd.Iterm = 0.0f;
//  Motor2.PiSpd.PIterm = 0.0f;
//  Motor2.PiSpd.Ff = 0.0f;
//  Motor2.PiSpd.AW = 0.0f;
//  Motor2.PiSpd.Out = 0.0f;
//}
//
//inline void Motor1Ctrl_PwmStart(void)
//{
//  switch(WP_Ctrl.Motor1Pwm&0x01)
//  {
//    case 0:
//      HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_1);
//      HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_2);
//      HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_3);
//      HAL_TIMEx_PWMN_Start_IT(&htim1, TIM_CHANNEL_1);
//      HAL_TIMEx_PWMN_Start_IT(&htim1, TIM_CHANNEL_2);
//      HAL_TIMEx_PWMN_Start_IT(&htim1, TIM_CHANNEL_3);
//      WP_Ctrl.Motor1Pwm |= 0x01;
//      break;
//    default:
//      WP_Ctrl.Motor1Pwm = 0x01;
//      break;
//  }
//}
//
//inline void Motor2Ctrl_PwmStart(void)
//{
//  switch(WP_Ctrl.Motor2Pwm&0x01)
//  {
//    case 0:
//      HAL_TIM_PWM_Start_IT(&htim8, TIM_CHANNEL_1);
//      HAL_TIM_PWM_Start_IT(&htim8, TIM_CHANNEL_2);
//      HAL_TIM_PWM_Start_IT(&htim8, TIM_CHANNEL_3);
//      HAL_TIMEx_PWMN_Start_IT(&htim8, TIM_CHANNEL_1);
//      HAL_TIMEx_PWMN_Start_IT(&htim8, TIM_CHANNEL_2);
//      HAL_TIMEx_PWMN_Start_IT(&htim8, TIM_CHANNEL_3);
//      WP_Ctrl.Motor2Pwm |= 0x01;
//      break;
//    default:
//      WP_Ctrl.Motor2Pwm = 0x01;
//      break;
//  }
//}
//
//inline void Motor1Ctrl_PwmStop(void)
//{
//  switch((WP_Ctrl.Motor1Pwm>>1)&0x01)
//  {
//    case 0:
//    HAL_TIM_PWM_Stop_IT(&htim1,TIM_CHANNEL_1);
//    HAL_TIM_PWM_Stop_IT(&htim1,TIM_CHANNEL_2);
//    HAL_TIM_PWM_Stop_IT(&htim1,TIM_CHANNEL_3);
//    HAL_TIMEx_PWMN_Stop_IT(&htim1,TIM_CHANNEL_1);
//    HAL_TIMEx_PWMN_Stop_IT(&htim1,TIM_CHANNEL_2);
//    HAL_TIMEx_PWMN_Stop_IT(&htim1,TIM_CHANNEL_3);
//    WP_Ctrl.Motor1Pwm |= 0x02;
//    break;
//  default:
//    WP_Ctrl.Motor1Pwm = 0x02;
//    break;
//  }
//}
//
//inline void Motor2Ctrl_PwmStop(void)
//{
//  switch((WP_Ctrl.Motor2Pwm>>1)&0x01)
//  {
//    case 0:
//      HAL_TIM_PWM_Stop_IT(&htim8,TIM_CHANNEL_1);
//      HAL_TIM_PWM_Stop_IT(&htim8,TIM_CHANNEL_2);
//      HAL_TIM_PWM_Stop_IT(&htim8,TIM_CHANNEL_3);
//      HAL_TIMEx_PWMN_Stop_IT(&htim8,TIM_CHANNEL_1);
//      HAL_TIMEx_PWMN_Stop_IT(&htim8,TIM_CHANNEL_2);
//      HAL_TIMEx_PWMN_Stop_IT(&htim8,TIM_CHANNEL_3);
//      WP_Ctrl.Motor2Pwm |= 0x02;
//      break;
//    default:
//      WP_Ctrl.Motor2Pwm = 0x02;
//      break;
//  }
//}
//
//
///* ************************************************************************************
// *                             Motor Control Basic API                                *
// * ************************************************************************************/
//inline void WP_ClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)
//{
//  // use to match q axis to a phase,  abc -> dq
//  s->d = _3->U;
//  s->q = MT_1_OVR_SQ3*(_3->V - _3->W);
//
//  // abc -> d-q axis : PARK transform
//  e->d = s->q * Angle->Tr_Sin + s->d * Angle->Tr_Cos;
//  e->q = s->q * Angle->Tr_Cos - s->d * Angle->Tr_Sin;
////  ids = ias;
////  iqs = ONE_SQRT3*(ibs - ics);
////  ide = iqs*Sin + ids*Cos;
////  iqe = iqs*Cos - ids*Sin;
//}
//
//inline void WP_InvClarkeParkTrans(WP_Motor3PhaseTypeDef *_3, WP_MotorDQTypeDef *s, WP_MotorDQTypeDef *e, WP_MotorAngleTypeDef *Angle)
//{
//  // dq axis ->UVW axis : IPARK transform
//  s->d = -e->q * Angle->Tr_Sin + e->d * Angle->Tr_Cos;
//  s->q =  e->q * Angle->Tr_Cos + e->d * Angle->Tr_Sin;
//
//  // SPACE Vector PWM
//  _3->U =  s->d;
//  _3->V = -MT_1_OVR_2 * s->d + MT_SQ3_OVR_2 * s->q;
//  _3->W = -MT_1_OVR_2 * s->d - MT_SQ3_OVR_2 * s->q;
//
////  vdsRef = -vqeOut*Sin+ vdeOut*Cos;
////  vqsRef =  vqeOut*Cos+ vdeOut*Sin;
////  vasRef =  vdsRef;
////  vbsRef = -HALF*vdsRef + SQRT3_TWO*vqsRef;
////  vcsRef = -HALF*vdsRef - SQRT3_TWO*vqsRef;
//}
//
//inline void WP_InvMinMax(WP_Motor3PhaseTypeDef *_3in, WP_Motor3PhaseTypeDef *_3out, float Vlim)
//{
//  float a, b;
//
//  a = _3in->W - _3in->U;
//  b = ( (_3in->U - _3in->V) * a > 0 ) ? ( _3in->U ): ( (_3in->V - _3in->W) * a >=0  ? _3in->W : _3in->V );
//  b = b * 0.5f;
//
//  _3out->U = _3in->U + b;
//  _3out->V = _3in->V + b;
//  _3out->W = _3in->W + b;
//
//  _3out->U = SAT(_3out->U, Vlim);
//  _3out->V = SAT(_3out->V, Vlim);
//  _3out->W = SAT(_3out->W, Vlim);
//}
//
//inline void WP_InvDeadTimeComp(WP_Motor3PhaseTypeDef *_3I, WP_Motor3PhaseTypeDef *_3V, WP_Motor3PhaseTypeDef *_3dead, float VT_DEAD, float Vlim)
//{
//  if(_3I->U > 5.0f)        _3dead->U = VT_DEAD;
//  else if(_3I->U < -5.0f)  _3dead->U = -VT_DEAD;
//  else                      _3dead->U=(0.0f);
//
//  if(_3I->V > 5.0f)        _3dead->V = VT_DEAD;
//  else if(_3I->V < -5.0f)  _3dead->V = -VT_DEAD;
//  else                      _3dead->V=(0.0f);
//
//  if(_3I->W > 5.0f)        _3dead->W = VT_DEAD;
//  else if(_3I->W < -5.0f)  _3dead->W = -VT_DEAD;
//  else                      _3dead->W = (0.0f);
//
//  _3V->U += _3dead->U;
//  _3V->V += _3dead->V;
//  _3V->W += _3dead->W;
//
//  _3V->U = SAT(_3V->U, Vlim);
//  _3V->V = SAT(_3V->V, Vlim);
//  _3V->W = SAT(_3V->W, Vlim);
//}
//
//
//inline float WP_PIcontrol(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)
//{
//
//  PiCur->Err = Cmd - Fedb;
//  PiCur->Pterm = Gain->Kp * PiCur->Err;// Kp*e(k)
//  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err - PiCur->AW * Gain->Kaw) + PiCur->Iterm + PiCur->Ff;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)
//  PiCur->Out = SAT(PiCur->PIterm, VRefLim);
//  PiCur->Iterm += Gain->Ki * (PiCur->Err - PiCur->AW * Gain->Kaw);  //x(k)=x(k-1)+Ki*e(k)
//  PiCur->Iterm = SAT(PiCur->Iterm, VRefLim);
//
//  return PiCur->Out;
//}
//
//inline float WP_PIcontrolForFieldWeak(float Cmd, float Fedb, WP_PITypeDef *PiCur, WP_GainTypeDef *Gain, float VRefLim)
//{
//  PiCur->Err = Cmd - Fedb;
//  PiCur->Pterm = Gain->Kp * PiCur->Err;// Kp*e(k)
//  PiCur->PIterm = PiCur->Pterm + Gain->Ki*(PiCur->Err) + PiCur->Iterm;  //piout=Ki*e(k)+x(k-1)+Kp*e(k)
//  PiCur->Out = SAT_Fw(PiCur->PIterm, VRefLim);
//  PiCur->Iterm += Gain->Ki * (PiCur->Err);  //x(k)=x(k-1)+Ki*e(k)
//  PiCur->Iterm = SAT_Fw(PiCur->Iterm, VRefLim);
//
//  return PiCur->Out;
//}
//
//inline void WP_PwmScale(uint32_t* Duty, WP_Motor3PhaseTypeDef *_3V, float *Rt, WP_InverterTypeDef *inv)
//{
//  float Du[3];
//
//  *(Rt)   = _3V->U * inv->Ctrl._Vdc_2;
//  *(Rt+1) = _3V->V * inv->Ctrl._Vdc_2;
//  *(Rt+2) = _3V->W * inv->Ctrl._Vdc_2;
//  Du[0] = SAT(_3V->U * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);
//  Du[1] = SAT(_3V->V * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);
//  Du[2] = SAT(_3V->W * inv->Ctrl.PWM_Scale, inv->Para.PWM_Mid);
//  *(Duty)   = (uint32_t) SAT_U((Du[0] + inv->Para.PWM_Mid), inv->Para.PWM_Max);
//  *(Duty+1) = (uint32_t) SAT_U((Du[1] + inv->Para.PWM_Mid), inv->Para.PWM_Max);
//  *(Duty+2) = (uint32_t) SAT_U((Du[2] + inv->Para.PWM_Mid), inv->Para.PWM_Max);
//}
//
//inline WP_ErrorStatus WP_PWMout(uint8_t Mot, uint32_t* Duty)
//{
//  switch(Mot)
//  {
//    case MOTOR1:
//      PWM0_U = *(Duty);
//      PWM0_V = *(Duty+1);
//      PWM0_W = *(Duty+2);
//      return WP_OK;
//      break;
//    case MOTOR2:
//      PWM1_U = *(Duty);
//      PWM1_V = *(Duty+1);
//      PWM1_W = *(Duty+2);
//      return WP_OK;
//      break;
//    default:
//      return WP_ERR;
//      break;
//  }
//}
//
//inline void WP_RampUpDn(float *Cmd, float *Ref, float RampU, float RampD) // 단위 : 1A / 스위칭
//{
//  if(*Ref < *Cmd){
//    *Ref += RampU;
//    *Ref = (*Ref > *Cmd) ? *Cmd : *Ref;
//  }
//  else if(*Ref > *Cmd){
//    *Ref -= RampD;
//    *Ref = (*Ref < *Cmd) ? *Cmd : *Ref;
//  }
//}
//
//
//inline void WP_MrPos(WP_MotorAngleTypeDef *Ang)
//{
//  if(WP_Ctrl.MrOffsetFlg==1)
//  {
//    if(Ang->MR.Raw.Sin >= Ang->MR.Max.Sin) Ang->MR.Max.Sin = Ang->MR.Raw.Sin;
//    if(Ang->MR.Raw.Sin <= Ang->MR.Min.Sin) Ang->MR.Min.Sin = Ang->MR.Raw.Sin;
//    if(Ang->MR.Raw.Cos >= Ang->MR.Max.Cos) Ang->MR.Max.Cos = Ang->MR.Raw.Cos;
//    if(Ang->MR.Raw.Cos <= Ang->MR.Min.Cos) Ang->MR.Min.Cos = Ang->MR.Raw.Cos;
//
//    Ang->MR.Mid.Sin = (Ang->MR.Max.Sin + Ang->MR.Min.Sin)>>1;
//    Ang->MR.Mid.Cos = (Ang->MR.Max.Cos + Ang->MR.Min.Cos)>>1;
//
//    Ang->MR.Gain.Sin = 1.0f;
//    Ang->MR.Gain.Cos = (((float)Ang->MR.Max.Sin - Ang->MR.Mid.Sin))/(((float)Ang->MR.Max.Cos - Ang->MR.Mid.Cos));
//  }
//  else{}
//
//  Ang->MR.Scaled.Sin = ((float)Ang->MR.Raw.Sin - (float)Ang->MR.Mid.Sin) * Ang->MR.Gain.Sin;
//  Ang->MR.Scaled.Cos = ((float)Ang->MR.Raw.Cos - (float)Ang->MR.Mid.Cos) * Ang->MR.Gain.Cos;
//
//  Ang->AngleMech = TR_RAD2ANG * (atan2f(Ang->MR.Scaled.Sin, Ang->MR.Scaled.Cos)+PI);
//
//  if(Ang->AngleMech >= 360.0f)   Ang->AngleMech -= 360.0f;
//  else if(Ang->AngleMech < 0)    Ang->AngleMech += 360.0f;
//
//  Ang->AngleElec = Ang->AngleMech * (float)Ang->PolePair;
//  Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;
//
//  if(Ang->AngleElec >= 360.0f)   Ang->AngleElec -= 360.0f;
//  else if(Ang->AngleElec < 0)    Ang->AngleElec += 360.0f;
//}
//
//
//inline WP_ErrorStatus WP_EncPos(uint8_t Mot, WP_MotorAngleTypeDef *Ang)  // this function in 'stm32f4xx_it.c'
//{
//  if(Mot==MOTOR1)
//  {
////    Ang->Pulse = __HAL_TIM_GET_COUNTER(&htim2);
//    Ang->Pulse = -1 *__HAL_TIM_GET_COUNTER(&htim2);
//    //    cnt1 = tick*4;
////    Ang->diffL += ((Ang->Pulse - Ang->PulseOld)<<16)>>16;
//    Ang->diffL += ((Ang->PulseOld - Ang->Pulse)<<16)>>16;
////    Ang->diffF += (float)(((Ang->Pulse - Ang->PulseOld)<<16)>>16);
//    Ang->diffF += (float)(((Ang->PulseOld - Ang->Pulse)<<16)>>16);
//    if(Ang -> diffL > Ang->EncPulse){
//      Ang->diffL = Ang->diffL - Ang->EncPulse;
//    }
//    else if(Ang->diffL < 0){
//      Ang->diffL = Ang->diffL + Ang->EncPulse;
//    }
//    Ang->AngleMech = (float)Ang->diffL * Ang->AngleScale;
//    Ang->AngleMechAll = Ang->diffF * Ang->AngleScale;
//    Ang->PulseOld = Ang->Pulse;
//
//    Ang->AngleElec = Ang->AngleMech * (float)Ang->PolePair;
//    Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;
//
//    if(Ang->AngleElec >= 360.0f)   Ang->AngleElec -= 360.0f;
//    else if(Ang->AngleElec < 0)    Ang->AngleElec += 360.0f;
//
//    return WP_OK;
//  }
//  else if(Mot==MOTOR2)
//  {
//    Ang->Pulse = __HAL_TIM_GET_COUNTER(&htim4);
//
////    Ang->diffL += ((Ang->Pulse - Ang->PulseOld)<<16)>>16;
//    Ang->diffL += ((Ang->PulseOld - Ang->Pulse)<<16)>>16;
////    Ang->diffF += (float)(((Ang->Pulse - Ang->PulseOld)<<16)>>16);
//    Ang->diffF += (float)(((Ang->PulseOld - Ang->Pulse)<<16)>>16);
//    if(Ang -> diffL > Ang->EncPulse){
//      Ang->diffL = Ang->diffL - Ang->EncPulse;
//    }
//    else if(Ang->diffL < 0){
//      Ang->diffL = Ang->diffL + Ang->EncPulse;
//    }
//    Ang->AngleMech = (float)Ang->diffL * Ang->AngleScale;
//    Ang->AngleMechAll = Ang->diffF * Ang->AngleScale;
//    Ang->PulseOld = Ang->Pulse;
//
//    Ang->AngleElec = Ang->AngleMech * (float)Ang->PolePair;
//    Ang->AngleElec = Ang->AngleElec - (float)((uint16_t)(Ang->AngleElec * MT_1_OVR_360))*360.0f + Ang->AngleElecOffset;
//
//    if(Ang->AngleElec >= 360.0f)   Ang->AngleElec -= 360.0f;
//    else if(Ang->AngleElec < 0)    Ang->AngleElec += 360.0f;
//
//    return WP_OK;
//  }
//  else{
//    return WP_ERR;
//  }
//}
//
//inline WP_ErrorStatus WP_EncSpd(uint8_t Mot, WP_MotorAngleTypeDef *Ang)
//{
//#if   defined(TIM_8KHZ)
//  uint8_t CntRef=8;
//#elif defined(TIM_10KHZ)
//  uint8_t CntRef=10;
//#elif defined(TIM_16KHZ)
//  uint8_t CntRef=16;
//#elif defined(TIM_20KHZ)
//  uint8_t CntRef=20;
//#else
//  uint8_t CntRef=10;
//#endif
//
//  if(Mot==MOTOR1)
//  {
//    Ang->_1msCnt++;
//    if(Ang->_1msCnt > CntRef)
//    {
////      Ang->mnew = __HAL_TIM_GET_COUNTER(&htim2);
//      Ang->mnew = -1 * __HAL_TIM_GET_COUNTER(&htim2);
//
////      Ang->diffM = ((Ang->mnew - Ang->mold)<<16)>>16;
//      Ang->diffM = ((Ang->mold - Ang->mnew)<<16)>>16;
//
//      Ang->Rpm = (float)Ang->diffM * Ang->SpeedScale * 0.3333; // 기어비 반영
//      Ang->mold = Ang->mnew;
//
//      LPF(Ang->RpmFil, Ang->Rpm, LpfSpd.fct);
//      Ang->WeFil = Ang->RpmFil * TR_RPM2RAD;
//      Ang->_1msCnt = 0;
//      Ang->_1msFlg = 1;
//    }
//    else{}
//
//    return WP_OK;
//  }
//  else if(Mot==MOTOR2)
//  {
//    Ang->_1msCnt++;
//    if(Ang->_1msCnt > CntRef)
//    {
//      Ang->mnew = __HAL_TIM_GET_COUNTER(&htim4);
//
////      Ang->diffM = ((Ang->mnew - Ang->mold)<<16)>>16;
//      Ang->diffM = ((Ang->mold - Ang->mnew)<<16)>>16;
//
//      Ang->Rpm = (float)Ang->diffM * Ang->SpeedScale * 0.3333; // 기어비 반영
//      Ang->mold = Ang->mnew;
//
//      LPF(Ang->RpmFil, Ang->Rpm, LpfSpd.fct);
//      Ang->WeFil = Ang->RpmFil * TR_RPM2RAD;
//      Ang->_1msCnt = 0;
//      Ang->_1msFlg = 1;
//    }
//    else{}
//    return WP_OK;
//  }
//  else{
//    return WP_ERR;
//  }
//}
////################################################################################
////
////    you must change the "HAL_GPIO_EXTI_Callback" function (see below)
////    the "HAL_GPIO_EXTI_Callback" function is in the "gpio.c" file
////    If it is not existed, add code like below
////
////################################################################################
///*
//void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
//{
//  if(GPIO_Pin==GPIO_PIN_15){
//    Motor2_Ang.diffL = 0;
//  }
//  else if(GPIO_Pin==GPIO_PIN_0){
//    Motor1_Ang.diffL = 0;
//  }
//}
//*/