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