pmsm-control/test_sw/cmd_proc.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4
   5 #include "cmd_proc.h"
   6
   7 #define PRUM_PROUD      2061
   8 #define PRUM_SOUC       6183
   9
  10 static char doPrint = 1;
  11
  12 /*
  13  * \brief
  14  * Help
  15  */
  16 static void printHelp(){
  17         doPrint=0;
  18         puts("start - Pripravi rizeni, zapne enable bity pwm.");
  19         puts("stop - Vypne komutaci, pwm a rizeni.");
  20         puts("0 - Vypne komutaci, pwm a rizeni.");
  21         puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
  22         puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
  23         puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
  24
  25         puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
  26         puts("exit - Bezpecne ukonci program.");
  27 }
  28
  29
  30 /*
  31  * \brief
  32  * V prikazech je hodnota od zneni prikazu delena dvojteckou
  33  * tato funkce dvojtecku nahradi mezerou
  34  */
  35 static void delCol(char * txt){
  36         unsigned i=0;
  37         while(txt[i]!='\0'){
  38                 if (txt[i]==':') txt[i]=' ';
  39                 i++;
  40         }
  41 }
  42
  43 /*
  44  * Nastavi enable bity na pwm,
  45  * zapne komutaci
  46  */
  47 static void start(struct rpi_state* state){
  48         sem_wait(&state->thd_par_sem);
  49         state->test=0x70;       /*konfiguracni byte*/
  50         sem_post(&state->thd_par_sem);
  51 }
  52
  53 /*
  54  * \brief
  55  * Zastavi komutaci, vypne pwm
  56  */
  57 static void stop(struct rpi_state* state){
  58         sem_wait(&state->thd_par_sem);
  59         state->test=0;          /*konfiguracni byte*/
  60         state->commutate=0;
  61         state->pwm1=0;
  62         state->pwm2=0;
  63         state->pwm3=0;
  64         sem_post(&state->thd_par_sem);
  65 }
  66
  67 /*
  68  * \brief
  69  * Nastavi pevnou sirku plneni
  70  */
  71 static void dutySet(struct rpi_state* state, int duty){
  72         sem_wait(&state->thd_par_sem);
  73         if (duty>512) duty=512;
  74         if (duty<-512) duty=-512;/*paranoia*/
  75         state->duty=duty;
  76         state->commutate=1;
  77         state->pos_reg_ena=0;
  78         sem_post(&state->thd_par_sem);
  79 }
  80
  81 /*
  82  * \brief
  83  * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu
  84  */
  85 static void goAbsolute(struct rpi_state* state, int pos){
  86         sem_wait(&state->thd_par_sem);
  87         state->pos_reg_ena=1;
  88         state->commutate=1;
  89         state->desired_pos=pos;
  90         sem_post(&state->thd_par_sem);
  91 }
  92
  93 /*
  94  * \brief
  95  * Zapne nebo vypne pravidelne vypisovani hodnot.
  96  */
  97 static void changePrint(){
  98         doPrint=!doPrint;
  99 }
 100
 101 /*
 102  * \brief
 103  * Bezpecne ukonci program.
 104  */
 105 static void exitApp(struct rpi_state* state){
 106         stop(state);
 107         /* Note: atexit() is set before*/
 108         exit(0);
 109 }
 110
 111 /**
 112  * \brief
 113  * Commands detection.
 114  */
 115 void poll_cmd(struct rpi_state* state){
 116         unsigned int tmp;
 117         char buff[50];
 118         char cmd[30];
 119         int val;
 120
 121         while(1){
 122                 scanf("%49s",buff);
 123                 delCol(buff);
 124                 sscanf(buff,"%s %d",cmd,&val);
 125
 126
 127                 if (!strcmp(cmd,"start")){
 128                         start(state);
 129                 }else if (!strcmp(cmd,"0")){
 130                         stop(state);
 131                 }else if (!strcmp(cmd,"stop")){
 132                         stop(state);
 133                 }else if (!strcmp(cmd,"ga")){
 134                         goAbsolute(state,val);
 135                 }else if (!strcmp(cmd,"duty")){
 136                         dutySet(state,val);
 137                 }else if (!strcmp(cmd,"help")){
 138                         printHelp();
 139                 }else if (!strcmp(cmd,"print")){
 140                         changePrint();
 141                 }else if (!strcmp(cmd,"exit")){
 142                         exitApp(state);
 143                 }
 144         }
 145
 146 }
 147
 148 /*
 149  * pocita procentualni odchylku od prumerneho proudu
 150  */
 151 float diff_p(float value){
 152         return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
 153 }
 154 /*
 155  * pocita procentualni odchylku od prumerneho souctu proudu
 156  */
 157 float diff_s(float value){
 158         return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
 159 }
 160
 161 /*
 162  * tiskne potrebna data
 163  */
 164 void printData(struct rpi_state* state){
 165         if (!doPrint) return;
 166
 167         struct rpi_in data_p;
 168         struct rpi_state s;     /*state*/
 169         float cur0, cur1, cur2;
 170         int i;
 171         signed long int speed;
 172         /* copy the data */
 173         sem_wait(&state->thd_par_sem);
 174         data_p = *state->spi_dat;
 175         s=*state;
 176         sem_post(&state->thd_par_sem);
 177
 178         speed=data_p.pozice-s.old_pos;
 179         if (data_p.adc_m_count){
 180                 cur0=data_p.ch0/data_p.adc_m_count;
 181                 cur1=data_p.ch1/data_p.adc_m_count;
 182                 cur2=data_p.ch2/data_p.adc_m_count;
 183         }
 184         for (i = 0; i < 16; i++) {
 185                         if (!(i % 6))
 186                                 puts("");
 187                         printf("%.2X ", data_p.debug_rx[i]);
 188         }
 189         puts("");
 190         printf("\npozice=%u\n",data_p.pozice);
 191         printf("rychlost=%ld\n",speed);
 192         printf("chtena pozice=%d\n",s.desired_pos);
 193         printf("transfer count=%u\n",s.tf_count);
 194         printf("raw_pozice=%u\n",data_p.pozice_raw);
 195         printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
 196         printf("index position=%u\n",data_p.index_position);
 197         printf("distance to index=%u\n",s.index_dist);
 198         printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
 199         printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
 200         printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
 201         printf("  PWM1=%u   PWM2=%u   PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3);
 202         printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
 203         printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
 204         printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
 205         printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
 206         printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0));
 207         printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
 208         printf("duty=%d\n",s.duty);
 209         if (s.index_ok) printf("index ok\n");
 210         if (s.commutate) printf("commutation in progress\n");
 211 }