Added simple commutator.

[fpga/rpi-motor-control.git] / pmsm-control / test_sw / main_pmsm.c

diff --git a/pmsm-control/test_sw/main_pmsm.c b/pmsm-control/test_sw/main_pmsm.c
index 6219d69859f2d4f16e4ff485e4be4c168d0d1d7d..65c62007655b88a85efa5dd33e788bd5e1bdff9f 100644 (file)
--- a/pmsm-control/test_sw/main_pmsm.c
+++ b/pmsm-control/test_sw/main_pmsm.c
@@ -36,6 +36,8 @@ struct rpi_in data;
 
 uint8_t test;
 uint16_t pwm1, pwm2, pwm3;
+char commutate;
+uint16_t simple_hall_duty;
 
 
 /**
@@ -67,6 +69,7 @@ void sighnd_fnc(){
 }
 
 void substractOffset(struct rpi_in* data, struct rpi_in* offset){
+       data->pozice_raw=data->pozice;
        data->pozice-=offset->pozice;
        return;
 }
@@ -100,6 +103,9 @@ void printData(struct rpi_in data){
        }
        puts("");
        printf("\npozice=%d\n",(int32_t)data.pozice);
+       printf("raw_pozice=%d\n",(int32_t)data.pozice_raw);
+       printf("raw_pozice last11=%u\n",(data.pozice_raw&0x7FF));
+       printf("index position=%d\n",(int16_t)data.index_position);
        printf("hal1=%d, hal2=%d, hal3=%d\n",data.hal1,data.hal2,data.hal3);
        printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&test),!!(0x20&test),!!(0x10&test));
        printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&test),!!(0x04&test),!!(0x02&test));
@@ -111,6 +117,8 @@ void printData(struct rpi_in data){
        printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch2,cur2,diff_p(cur2));
        printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data.ch0,cur0,diff_p(cur0));
        printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
+       printf("duty=%u\n",simple_hall_duty);
+       if (commutate) printf("commutation in progress\n");
 }
 void prepare_tx(uint8_t * tx){
 
@@ -181,7 +189,29 @@ void * pos_monitor(void* param){
        }
        return (void*)0;
 }
-
+/*
+ * \brief
+ * Test function to be placed in controll loop.
+ * Clockwise rotation with PWM with 64 out of 2048 duty cycle.
+ */
+inline void simple_hall_commutator(struct rpi_in data){
+       /* pwm3 */
+       if (data.hal2 && !data.hal3){
+               pwm1=0;
+               pwm2=0;
+               pwm3=simple_hall_duty;
+       /* pwm1 */
+       }else if (data.hal1 && !data.hal2){
+               pwm1=simple_hall_duty;
+               pwm2=0;
+               pwm3=0;
+       /* pwm2 */
+       }else if (!data.hal1 && data.hal3){
+               pwm1=0;
+               pwm2=simple_hall_duty;
+               pwm3=0;
+       }
+}
 /**
  * Funkce pravidelne vycita data z motoru
  */
@@ -195,6 +225,9 @@ void * read_data(void* param){
                        prepare_tx(tx);
                        data = spi_read(tx);
                        substractOffset(&data,&pocatek);
+                       if (commutate){
+                               simple_hall_commutator(data);
+                       }
                        usleep(1000);                           /*1kHz*/
                }
 }
@@ -204,7 +237,7 @@ void * read_data(void* param){
  */
 
 int main(){
-       uint16_t tmp;
+       unsigned int tmp;
 
        /*nastaveni priorit vlaken*/
        struct thread_param tsp;
@@ -238,22 +271,41 @@ int main(){
 
        /*muzeme zavrit semafor*/
        sem_destroy(&thd_par_sem);
-
+       /*
+        * Note:
+        * pri pouziti scanf("%u",&simple_hall_duty); dochazelo
+        * k preukladani hodnot na promenne test. Dost divne.
+        */
        while (1){
                scanf("%u",&tmp);
-               printf("volba=%x\n",tmp);
+               printf("volba=%u\n",tmp);
                switch (tmp){
                case 1:
-                       scanf("%u",&pwm1);
+                       scanf("%u",&tmp);
+                       pwm1=tmp&0xFFF;
                        break;
                case 2:
-                       scanf("%u",&pwm2);
+                       scanf("%u",&tmp);
+                       pwm2=tmp&0xFFF;
                        break;
                case 3:
-                       scanf("%u",&pwm3);
+                       scanf("%u",&tmp);
+                       pwm3=tmp&0xFFF;
                        break;
                case 4:
-                       scanf("%u",&test);
+                       scanf("%u",&tmp);
+                       test=tmp&0xFF;
+                       break;
+               case 5:
+                       commutate=!commutate;
+                       /* switch off pwms at the end of commutation */
+                       pwm1&=commutate*0xFFFF;
+                       pwm2&=commutate*0xFFFF;
+                       pwm3&=commutate*0xFFFF;
+                       break;
+               case 6:
+                       scanf("%u",&tmp);
+                       simple_hall_duty=tmp&0xFFF;
                        break;
 
                default: