1 |
//******** Mrakomer2 - stepper motor control ***************** |
1 |
//******** Mrakomer2 - stepper motor control ***************** |
2 |
#define VERSION "3.0" // Special version for BART |
2 |
#define VERSION "3.0" // Special version for BART |
3 |
#define ID "$Id: irmrak.c 420 2006-12-29 21:43:11Z kakl $" |
3 |
#define ID "$Id: irmrak.c 1586 2010-08-22 08:35:33Z kakl $" |
4 |
//************************************************************ |
4 |
//************************************************************ |
5 |
|
5 |
|
6 |
#include "irmrak.h" |
6 |
#include "irmrak.h" |
7 |
#include <string.h> |
7 |
#include <string.h> |
8 |
|
8 |
|
9 |
char VER[4]=VERSION; |
9 |
char VER[4]=VERSION; |
10 |
char REV[50]=ID; |
10 |
char REV[50]=ID; |
11 |
|
11 |
|
12 |
#bit CREN = 0x18.4 // USART registers |
12 |
#bit CREN = 0x18.4 // USART registers |
13 |
#bit SPEN = 0x18.7 |
13 |
#bit SPEN = 0x18.7 |
14 |
#bit OERR = 0x18.1 |
14 |
#bit OERR = 0x18.1 |
15 |
#bit FERR = 0x18.2 |
15 |
#bit FERR = 0x18.2 |
16 |
|
16 |
|
17 |
#define HALL PIN_A4 // Hallova sonda pro zjisteni natoceni dolu |
17 |
#define HALL PIN_A4 // Hallova sonda pro zjisteni natoceni dolu |
18 |
// vykonovy FET je na RB3 (vystup PWM) |
18 |
// vykonovy FET je na RB3 (vystup PWM) |
19 |
|
19 |
|
20 |
int port; // stav brany B pro krokove motory |
20 |
int port; // stav brany B pro krokove motory |
21 |
int j; // pro synchronisaci fazi |
21 |
int j; // pro synchronisaci fazi |
22 |
unsigned int8 uhel; // prijaty znak |
22 |
unsigned int8 uhel; // prijaty znak |
23 |
unsigned int8 i; // pro cyklus for |
23 |
unsigned int8 i; // pro cyklus for |
24 |
|
24 |
|
25 |
// --- Kroky krokoveho motoru --- |
25 |
// --- Kroky krokoveho motoru --- |
26 |
void krok(int n) |
26 |
void krok(int n) |
27 |
{ |
27 |
{ |
28 |
while((n--)>0) |
28 |
while((n--)>0) |
29 |
{ |
29 |
{ |
30 |
if (1==(j&1)) {port^=0b11000000;} else {port^=0b00110000;}; |
30 |
if (1==(j&1)) {port^=0b11000000;} else {port^=0b00110000;}; |
31 |
output_B(port); |
31 |
output_B(port); |
32 |
delay_ms(20); // Nutno nastavit podle dynamiky systemu. |
32 |
delay_ms(20); // Nutno nastavit podle dynamiky systemu. |
33 |
j++; |
33 |
j++; |
34 |
} |
34 |
} |
35 |
} |
35 |
} |
36 |
|
36 |
|
37 |
// --- Dojet dolu magnetem na cidlo --- |
37 |
// --- Dojet dolu magnetem na cidlo --- |
38 |
void dolu() |
38 |
void dolu() |
39 |
{ |
39 |
{ |
40 |
unsigned int8 err; // pocitadlo pro zjisteni zaseknuti otaceni |
40 |
unsigned int8 err; // pocitadlo pro zjisteni zaseknuti otaceni |
41 |
|
41 |
|
42 |
err=0; |
42 |
err=0; |
43 |
while(!input(HALL)) // otoceni trubky dolu az na hall |
43 |
while(!input(HALL)) // otoceni trubky dolu az na hall |
44 |
{ |
44 |
{ |
45 |
krok(1); |
45 |
krok(1); |
46 |
err++; |
46 |
err++; |
47 |
if(40==err) // do 40-ti kroku by se melo podarit otocit dolu |
47 |
if(40==err) // do 40-ti kroku by se melo podarit otocit dolu |
48 |
{ |
48 |
{ |
49 |
output_B(0); // vypnuti motoru |
49 |
output_B(0); // vypnuti motoru |
50 |
printf("E"); // Hlasime chybu |
50 |
printf("E"); // Hlasime chybu |
51 |
reset_cpu(); |
51 |
reset_cpu(); |
52 |
} |
52 |
} |
53 |
}; |
53 |
}; |
54 |
delay_ms(500); // cas na ustaleni trubky |
54 |
delay_ms(500); // cas na ustaleni trubky |
55 |
output_B(0); // vypnuti motoru |
55 |
output_B(0); // vypnuti motoru |
56 |
} |
56 |
} |
57 |
|
57 |
|
58 |
// --- Najeti na vychozi polohu dole --- |
58 |
// --- Najeti na vychozi polohu dole --- |
59 |
void nula() |
59 |
void nula() |
60 |
{ |
60 |
{ |
61 |
port=0b10010000; // vychozi nastaveni fazi pro rizeni motoru |
61 |
port=0b10010000; // vychozi nastaveni fazi pro rizeni motoru |
62 |
output_B(port); |
62 |
output_B(port); |
63 |
j=1; // smer dolu |
63 |
j=1; // smer dolu |
64 |
delay_ms(500); |
64 |
delay_ms(500); |
65 |
} |
65 |
} |
66 |
|
66 |
|
67 |
|
67 |
|
68 |
//------------------------------------------------ |
68 |
//------------------------------------------------ |
69 |
void main() |
69 |
void main() |
70 |
{ |
70 |
{ |
71 |
setup_adc_ports(NO_ANALOGS|VSS_VDD); |
71 |
setup_adc_ports(NO_ANALOGS|VSS_VDD); |
72 |
setup_adc(ADC_OFF); |
72 |
setup_adc(ADC_OFF); |
73 |
setup_spi(SPI_SS_DISABLED); |
73 |
setup_spi(SPI_SS_DISABLED); |
74 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
74 |
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); |
75 |
setup_timer_1(T1_DISABLED); |
75 |
setup_timer_1(T1_DISABLED); |
76 |
setup_timer_2(T2_DISABLED,0,1); |
76 |
setup_timer_2(T2_DISABLED,0,1); |
77 |
setup_ccp1(CCP_OFF); |
77 |
setup_ccp1(CCP_OFF); |
78 |
setup_comparator(NC_NC_NC_NC); |
78 |
setup_comparator(NC_NC_NC_NC); |
79 |
setup_vref(FALSE); |
79 |
setup_vref(FALSE); |
80 |
setup_oscillator(OSC_8MHZ|OSC_INTRC); |
80 |
setup_oscillator(OSC_8MHZ|OSC_INTRC); |
81 |
|
81 |
|
82 |
output_B(0); // vypnuti motoru a topeni |
82 |
output_B(0); // vypnuti motoru a topeni |
83 |
set_tris_B(0b00000111); // faze a topeni jako vystupy |
83 |
set_tris_B(0b00000111); // faze a topeni jako vystupy |
84 |
|
84 |
|
85 |
nula(); |
85 |
nula(); |
86 |
dolu(); // otoc trubku do vychozi pozice dolu |
86 |
dolu(); // otoc trubku do vychozi pozice dolu |
87 |
|
87 |
|
88 |
while(true) |
88 |
while(true) |
89 |
{ |
89 |
{ |
90 |
CREN=0; CREN=1; // Reinitialise USART |
90 |
CREN=0; CREN=1; // Reinitialise USART |
91 |
|
91 |
|
92 |
while(!kbhit()) |
92 |
while(!kbhit()) |
93 |
{ |
93 |
{ |
94 |
if (!input(HALL)) // znovuotoceni trubky dolu, kdyby ji vitr otocil |
94 |
if (!input(HALL)) // znovuotoceni trubky dolu, kdyby ji vitr otocil |
95 |
{ |
95 |
{ |
96 |
dolu(); |
96 |
dolu(); |
97 |
} |
97 |
} |
98 |
}; // pokracuj dal, kdyz prisel po RS232 znak |
98 |
}; // pokracuj dal, kdyz prisel po RS232 znak |
99 |
|
99 |
|
100 |
|
100 |
|
101 |
uhel=getc(); // prijmi znak |
101 |
uhel=getc(); // prijmi znak |
102 |
if ('m'==uhel) // standardni mereni ve trech polohach |
102 |
if ('m'==uhel) // standardni mereni ve trech polohach |
103 |
{ |
103 |
{ |
104 |
nula(); |
104 |
nula(); |
105 |
j++; // reverz, nahoru |
105 |
j++; // reverz, nahoru |
106 |
|
106 |
|
107 |
krok(18); |
107 |
krok(18); |
108 |
printf("A"); // mereni teploty 45° nad obzorem |
108 |
printf("A"); // mereni teploty 45° nad obzorem |
109 |
delay_ms(300); |
109 |
delay_ms(300); |
110 |
krok(7); |
110 |
krok(7); |
111 |
printf("B"); // mereni teploty v zenitu |
111 |
printf("B"); // mereni teploty v zenitu |
112 |
delay_ms(300); |
112 |
delay_ms(300); |
113 |
krok(7); |
113 |
krok(7); |
114 |
printf("C"); // mereni teploty 45° nad obzorem na druhou stranu |
114 |
printf("C"); // mereni teploty 45° nad obzorem na druhou stranu |
115 |
delay_ms(300); |
115 |
delay_ms(300); |
116 |
|
116 |
|
117 |
j++; // reverz |
117 |
j++; // reverz |
118 |
dolu(); |
118 |
dolu(); |
119 |
printf("G"); // mereni teploty Zeme (<G>round) |
119 |
printf("G"); // mereni teploty Zeme (<G>round) |
120 |
|
120 |
|
121 |
continue; |
121 |
continue; |
122 |
} |
122 |
} |
123 |
|
123 |
|
124 |
if ('i'==uhel) {printf("I"); continue;} // Predani prikazu pro Info |
124 |
if ('i'==uhel) {printf("I"); continue;} // Predani prikazu pro Info |
125 |
if ('h'==uhel) {printf("H"); continue;} // Predani prikazu pro Topeni |
125 |
if ('h'==uhel) {printf("H"); continue;} // Predani prikazu pro Topeni |
126 |
if ('f'==uhel) {printf("F"); continue;} // Predani prikazu pro vypnuti topeni |
126 |
if ('f'==uhel) {printf("F"); continue;} // Predani prikazu pro vypnuti topeni |
127 |
if ('x'==uhel) // Zjisteni verze FW |
127 |
if ('x'==uhel) // Zjisteni verze FW |
128 |
{ |
128 |
{ |
129 |
printf("Mrakomer - Motor V%s (C) 2006 KAKL\n\r", VER); |
129 |
printf("Mrakomer - Motor V%s (C) 2006 KAKL\n\r", VER); |
130 |
printf("%s\r\n", REV); |
130 |
printf("%s\r\n", REV); |
131 |
} |
131 |
} |
132 |
|
132 |
|
133 |
if ((uhel>='0') && (uhel<='@')) // mereni v pozadovanem uhlu [0..;]=(0..11) |
133 |
if ((uhel>='0') && (uhel<='@')) // mereni v pozadovanem uhlu [0..;]=(0..11) |
134 |
{ |
134 |
{ |
135 |
uhel-='0'; |
135 |
uhel-='0'; |
136 |
}; |
136 |
}; |
137 |
|
137 |
|
138 |
if(uhel>11) continue; // ochrana, abysme neukroutili draty |
138 |
if(uhel>11) continue; // ochrana, abysme neukroutili draty |
139 |
|
139 |
|
140 |
nula(); |
140 |
nula(); |
141 |
j++; // reverz, nahoru |
141 |
j++; // reverz, nahoru |
142 |
|
142 |
|
143 |
krok(12); // odkrokuj do roviny |
143 |
krok(12); // odkrokuj do roviny |
144 |
for(i=0; i<uhel; i++) // dale odkrokuj podle pozadovaneho uhlu |
144 |
for(i=0; i<uhel; i++) // dale odkrokuj podle pozadovaneho uhlu |
145 |
{ |
145 |
{ |
146 |
krok(2); |
146 |
krok(2); |
147 |
}; |
147 |
}; |
148 |
printf("S"); |
148 |
printf("S"); |
149 |
delay_ms(300); |
149 |
delay_ms(300); |
150 |
|
150 |
|
151 |
j++; // reverz |
151 |
j++; // reverz |
152 |
dolu(); |
152 |
dolu(); |
153 |
printf("G"); // mereni teploty Zeme (<G>round) |
153 |
printf("G"); // mereni teploty Zeme (<G>round) |
154 |
} |
154 |
} |
155 |
} |
155 |
} |
156 |
|
156 |
|