Correct typo in rpi-mc-1 mapping to Ti AM437x based RICO board.
[fpga/rpi-motor-control.git] / pmsm-control / test_sw / main_pmsm.c
1 /**
2  * \file main_pmsm.c
3  * \author Martin Prudek
4  * \brief Mainfile pro pmsm control.
5  */
6
7 #ifndef NULL
8 #define NULL (void*) 0
9 #endif /*NULL*/
10
11
12 #include <stdlib.h>     /*exit*/
13 #include <signal.h>     /*signal handler Ctrl+C*/
14 #include <stdio.h>      /*printf*/
15 #include <sched.h>      /*sheduler*/
16 #include <unistd.h>     /*usleep*/
17 #include <pthread.h>    /*threads*/
18 #include <time.h>       /*nanosleep*/
19 #include <string.h>
20
21 #include "rpin.h"       /*gpclk*/
22 #include "rp_spi.h"     /*spi*/
23 #include "misc.h"       /*structure for priorities*/
24 #include "pmsm_state.h"
25 #include "cmd_proc.h"
26 #include "controllers.h"
27 #include "commutators.h"
28 #include "comp.h"
29 #include "logs.h"
30
31 #define PRIOR_KERN      50
32 #define PRIOR_HIGH      49
33 #define PRIOR_LOW       20
34
35 #define THREAD_SHARED   0
36 #define INIT_VALUE      1       /*init value for semaphor*/
37
38
39 #define NSEC_PER_SEC    (1000000000) /* The number of nsecs per sec. */
40
41
42 struct rpi_in data;
43 struct rpi_state rps={
44         //.MAX_DUTY=170,
45         .spi_dat=&data,
46         .test=0,
47         .pwm1=0,.pwm2=0, .pwm3=0,
48         .pwm1=0, .t_pwm2=0, .t_pwm3=0,
49         .commutate=0,
50         .duty=0,                        /* duty cycle of pwm */
51         .index_dist=0,          /* distance to index position */
52         .index_ok=0,
53         .tf_count=0,            /*number of transfer*/
54         .desired_pos=0,         /* desired position */
55         .pos_reg_ena=0,
56         .desired_spd=0,
57         .spd_reg_ena=0,
58         .old_pos={0},
59         .spd_err_sum=0,
60         .log_col_count=0,       /* pocet radku zaznamu */
61         .log_col=0,
62         .doLogs=0,
63         .alpha_offset=960,
64         .main_commutator=zero_commutator,       /* komutace vypnuta */
65         .main_controller=zero_controller        /*rizeni vypnuto */
66 };
67
68
69 /**
70  * \brief Initilizes GPCLK.
71  */
72 int clk_init(){
73         initialise(); /*namapovani gpio*/
74         initClock(PLLD_500_MHZ, 10, 0);
75         gpioSetMode(4, FSEL_ALT0);
76         return 0;
77 }
78
79 /**
80  * \brief Signal handler pro Ctrl+C
81  */
82 void appl_stop(){
83         uint8_t tx[16];
84         sem_wait(&rps.thd_par_sem);
85
86         memset(tx,0,16*sizeof(int));
87         rps.pwm1=0;
88         rps.pwm2=0;
89         rps.pwm3=0;
90         spi_read(&rps);
91
92         spi_disable();
93         termClock(0);
94         freeLogs(&rps);
95         /*muzeme zavrit semafor*/
96         sem_destroy(&rps.thd_par_sem);
97         printf("\nprogram bezpecne ukoncen\n");
98 }
99
100 /**
101  * Funkce pravidelne vypisuje posledni zjistenou pozici lokalniho motoru
102  */
103 void * pos_monitor(void* param){
104         while(1){
105                 printData(&rps);
106                 usleep(1000000);        /*1 Hz*/
107         }
108         return (void*)0;
109 }
110
111 /*
112  * \brief
113  * Feedback loop.
114  */
115 void * control_loop(void* param){
116         int i;
117         struct rpi_in pocatek;
118         struct rpi_state poc={
119                 .spi_dat=&pocatek,
120                 .test=0,
121                 .pwm1=0, .pwm1=0, .pwm3=0
122         };
123         struct timespec t;
124         int interval = 1000000; /* 1ms ~ 1kHz*/
125         char first=1;
126         uint16_t last_index;                            /*we have index up-to date*/
127         spi_read(&poc);         /*pocatecni informace*/
128         clock_gettime(CLOCK_MONOTONIC ,&t);
129                 while(1){
130                         /* wait until next shot */
131                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
132                         sem_wait(&rps.thd_par_sem);             /*---take semaphore---*/
133
134                         /*old positions*/
135                         rps.old_pos[rps.tf_count%OLD_POS_NUM]=rps.spi_dat->pozice;
136                         spi_read(&rps);                         /*exchange data*/
137                         rps.tf_count++;
138                         substractOffset(&data,poc.spi_dat);     /*subtract initiate postion */
139                         compSpeed(&rps);                        /*spocita rychlost*/
140
141                         if (!rps.index_ok){
142                                 if (first){
143                                         last_index=data.index_position;
144                                         first=0;
145                                 }else if (last_index!=data.index_position){
146                                         setIndexOK(&rps);
147                                         comIndDist(&rps);       /*vypocet vzdalenosti indexu*/
148                                 }
149                         }else{ /*index je v poradku*/
150                                 comIndDist(&rps);               /*vypocet vzdalenosti indexu*/
151                         }
152                         last_index=data.index_position;         /*preulozime stary index */
153
154                         /* pocitame sirku plneni podle potreb rizeni*/
155                         rps.main_controller(&rps);
156
157                         /* sirku plneni prepocteme na jednotlive pwm */
158                         rps.main_commutator(&rps);
159
160                         /*zalogujeme hodnoty*/
161                         if (rps.doLogs && !(rps.tf_count%LOG_PERIOD)){
162                                 makeLog(&rps);
163                         }
164
165                         sem_post(&rps.thd_par_sem);             /*--post semaphore---*/
166
167                         /* calculate next shot */
168                         t.tv_nsec += interval;
169
170                         while (t.tv_nsec >= NSEC_PER_SEC) {
171                                 t.tv_nsec -= NSEC_PER_SEC;
172                                 t.tv_sec++;
173                         }
174
175                 }
176 }
177
178 /**
179  * \brief Main function.
180  */
181 int main(){
182         pthread_t base_thread_id;
183         clk_init();             /* inicializace gpio hodin */
184         spi_init();             /* iniicializace spi*/
185
186         /*semafor pro detekci zpracovani parametru vlaken*/
187         sem_init(&rps.thd_par_sem,THREAD_SHARED,INIT_VALUE);
188         setup_environment();
189
190         base_thread_id=pthread_self();
191
192         /*main control loop*/
193         create_rt_task(&base_thread_id,PRIOR_HIGH,control_loop,NULL);
194
195         /*monitor of current state*/
196         create_rt_task(&base_thread_id,PRIOR_LOW,pos_monitor,NULL);
197
198         /*wait for commands*/
199         poll_cmd(&rps);
200
201         return 0;
202 }