-inline
-int sin_commutator(int duty){
- #define DEGREE_60 715827883
- #define DEGREE_120 1431655765
- #define DEGREE_180 2147483648
- #define DEGREE_240 2863311531
- #define DEGREE_300 3579139413
- uint32_t j,pos;
- int32_t sin;
- uint16_t pwm;
- pos=rps.index_dist*4294967;
- if (duty>=0){ /*clockwise rotation*/
- /* 1st phase */
- sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm1=pwm;
-
- /* 2nd phase */
- sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm2=pwm;
-
- /* 3rd phase */
- sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm3=pwm;
- }else{
- duty=-duty;
-
- /* 1st phase */
- sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm1=pwm;
-
- /* 2nd phase */
- sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm2=pwm;
-
- /* 3rd phase */
- sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
- pwm=0;
- for(j=0;j!=11;j++){
- /* multiplicate as if maximum sinus value was unity */
- pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
- }
- rps.pwm3=pwm;
- }
- return 0;
-}
-/*
- * \brief
- * Test function to be placed in controll loop.
- * Switches PWM's at point where they produce same force.
- * This points are found thanks to IRC position,
- */
-inline
-void simple_ind_dist_commutator(int duty){
- if (duty>=0){ /* clockwise - so that position increase */
- /* pwm3 */
- if ((rps.index_dist>=45 && rps.index_dist<=373) ||
- (rps.index_dist>=1048 && rps.index_dist<=1377)){
- rps.pwm1=0;
- rps.pwm2=0;
- rps.pwm3=duty;
- /* pwm1 */
- }else if ((rps.index_dist>=373 && rps.index_dist<=711) ||
- (rps.index_dist>=1377 && rps.index_dist<=1711)){
- rps.pwm1=duty;
- rps.pwm2=0;
- rps.pwm3=0;
- /* pwm2 */
- }else if ((rps.index_dist>=0 && rps.index_dist<=45) ||
- (rps.index_dist>=711 && rps.index_dist<=1048) ||
- (rps.index_dist>=1711 && rps.index_dist<=1999)){
- rps.pwm1=0;
- rps.pwm2=duty;
- rps.pwm3=0;
- }
- }else{ /*counter-clockwise - position decrease */
- /* pwm3 */
- if ((rps.index_dist>=544 && rps.index_dist<=881) ||
- (rps.index_dist>=1544 && rps.index_dist<=1878)){
- rps.pwm1=0;
- rps.pwm2=0;
- rps.pwm3=-duty;
- /* pwm1 */
- }else if ((rps.index_dist>=0 && rps.index_dist<=211) ||
- (rps.index_dist>=881 && rps.index_dist<=1210) ||
- (rps.index_dist>=1878 && rps.index_dist<=1999)){
- rps.pwm1=-duty;
- rps.pwm2=0;
- rps.pwm3=0;
- /* pwm2 */
- }else if ((rps.index_dist>=211 && rps.index_dist<=544) ||
- (rps.index_dist>=1210 && rps.index_dist<=1544)){
- rps.pwm1=0;
- rps.pwm2=-duty;
- rps.pwm3=0;
- }
- }
-}
-/*
- * \brief
- * Test function to be placed in controll loop.
- * Switches PWM's at point where they produce same force
- */
-inline void simple_hall_commutator(int duty){
- if (duty>=0){ /* clockwise - so that position increase */
- /* pwm3 */
- if (data.hal2 && !data.hal3){
- rps.pwm1=0;
- rps.pwm2=0;
- rps.pwm3=duty;
- /* pwm1 */
- }else if (data.hal1 && !data.hal2){
- rps.pwm1=duty;
- rps.pwm2=0;
- rps.pwm3=0;
- /* pwm2 */
- }else if (!data.hal1 && data.hal3){
- rps.pwm1=0;
- rps.pwm2=duty;
- rps.pwm3=0;
- }
- }else{ /*counter-clockwise - position decrease */
- /* pwm3 */
- if (!data.hal2 && data.hal3){
- rps.pwm1=0;
- rps.pwm2=0;
- rps.pwm3=-duty;
- /* pwm1 */
- }else if (!data.hal1 && data.hal2){
- rps.pwm1=-duty;
- rps.pwm2=0;
- rps.pwm3=0;
- /* pwm2 */
- }else if (data.hal1 && !data.hal3){
- rps.pwm1=0;
- rps.pwm2=-duty;
- rps.pwm3=0;
- }
- }
-}
-/**
- * \brief
- * Computation of distance to index.
- *
- * K dispozici je 12-bit index, to umoznuje ulozit 4096 ruznych bodu
- * Je nutne vyjadrit 1999 bodu proti i posmeru h.r. od indexu -
- * to je 3999 bodu
- * =>12 bitu je dostacujicich, pokud nikdy nedojde ke ztrate
- * signalu indexu
- */
-void comIndDist(){
- uint16_t pos_12 = 0x0FFF & data.pozice_raw;
- if (data.index_position<2048){
- if (pos_12<data.index_position){
- /*proti smeru h.r. od indexu*/
- rps.index_dist=pos_12+2000-data.index_position;
- }else if (pos_12<=data.index_position+2048){
- /*po smeru h.r. od indexu*/
- rps.index_dist=pos_12-data.index_position;
- }else{
- /*proti smeru h.r. od indexu - podtecena pozice*/
- rps.index_dist=pos_12-data.index_position-2096;
- }
- }else{
- if (pos_12<data.index_position-2048){
- /*po smeru h.r. od indexu - pretecena pozice*/
- rps.index_dist=4096+pos_12-data.index_position;
- }else if (pos_12<data.index_position){
- /*proti smeru h.r. od indexu*/
- rps.index_dist=pos_12+2000-data.index_position;
- }else{
- /*po smeru h.r. od indexu*/
- rps.index_dist=pos_12-data.index_position;
- }
-
- }
-}
-/*
- * \brief
- * Very simple PID regulator.
- * Now only with P-part so that the error doesnt go to zero.
- * TODO: add anti-wind up and I and D parts
- */
-inline void pid(){
- int duty_tmp;
- duty_tmp = PID_P*(rps.desired_pos - (int32_t)data.pozice);
- if (duty_tmp>MAX_DUTY){
- rps.duty=MAX_DUTY;
- }else if (duty_tmp<-MAX_DUTY){
- rps.duty=-MAX_DUTY;
- }else{
- rps.duty = duty_tmp;
- }
-}