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