1 | // ********************* Variablen ***********************************
|
2 | unsigned int f0=303;
|
3 | volatile float f, fc, Match;
|
4 | unsigned char Fahrt_Flag;
|
5 | unsigned long Pos_x=0, Pos_P1=0, a, fm=18000;
|
6 |
|
7 | // ********************* Prototypen ***********************************
|
8 | void ATOUL(unsigned char *DFeld, int feldlaenge); // Prototype Arry to unsigned long
|
9 | void ULTOA(unsigned long Value);
|
10 | static unsigned long wurzel(unsigned long x);
|
11 |
|
12 | // ***********************ISR zum Zählen der Schritte *******************
|
13 | ISR(TIMER1_COMPB_vect) // ISR Zählen der Matches
|
14 | {
|
15 | PORTB |= (1<<PB2); // Motor-Takt AN
|
16 |
|
17 | OCR1A=Match;
|
18 | // ----------- Zählen der Schritte in beiden Richtungen -------------
|
19 | Pos_ist++;
|
20 | if(Rechtsfahrt_Flag==0)
|
21 | {
|
22 | Pos_akt--;
|
23 | }
|
24 | else
|
25 | Pos_akt++;
|
26 |
|
27 | // ----------- Timer ausschalten ---------------------------------
|
28 | if((Pos_soll == Pos_ist) || (MotorAN_ADC == 0) || (MotorAN_LS == 0))
|
29 | {
|
30 | TIMSK1=0; // Timer Interrupt COMPARE MATCH B aus
|
31 | }
|
32 |
|
33 | PORTB &=~ (1<<PB2); // Motor-Takt AUS
|
34 | }
|
35 |
|
36 | static unsigned long wurzel(unsigned long x)
|
37 | {
|
38 | register unsigned long xr; // result register
|
39 | register unsigned long q2; // scan-bit register
|
40 | register unsigned char f; // flag (one bit)
|
41 |
|
42 | xr = 0; // clear result
|
43 | q2 = 0x40000000L; // higest possible result bit
|
44 | do
|
45 | {
|
46 | if((xr + q2) <= x)
|
47 | {
|
48 | x -= xr + q2;
|
49 | f = 1; // set flag
|
50 | }
|
51 | else{
|
52 | f = 0; // clear flag
|
53 | }
|
54 | xr >>= 1;
|
55 | if(f){
|
56 | xr += q2; // test flag
|
57 | }
|
58 | } while(q2 >>= 2); // shift twice
|
59 | if(xr < x){
|
60 | return xr +1; // add for rounding
|
61 | }
|
62 | else{
|
63 | return xr;
|
64 | }
|
65 | }
|
66 |
|
67 | void schrittmotor(unsigned char *DFeld,int feldlaenge) // SUB-Befehl Schrittmotor
|
68 | {
|
69 | MotorAN_ADC=1; // Flag
|
70 | MotorAN_LS=1; // Flag
|
71 | Fahrt_Flag=1; // Flag
|
72 |
|
73 | f=f0;
|
74 | a=10000;
|
75 | Pos_P1=10200;
|
76 |
|
77 | Match=0;
|
78 | OCR1A=0;
|
79 |
|
80 | // *********************** Konvertierung der Daten in long int ***************
|
81 | if(Schrittmotor_flag == 0)
|
82 | {
|
83 | ATOUL(DFeld,feldlaenge);
|
84 | }
|
85 |
|
86 | // -------- Timer Init -------------------------------------------------------------------
|
87 | if(Schrittmotor_flag==1)
|
88 | {
|
89 | TIMSK1 = 0b00000100; // Timer Interrupts COMPARE MATCH B = bit2
|
90 | TCCR1A = 0b00000000; // Pin disconnect
|
91 | TCCR1B = 0b00001001; // Timer Start: Mode 4 = bit3, Teiler 1 = bit1
|
92 | PORTD &= ~(1<<PD0); // Motor an ( low aktiv )
|
93 | }
|
94 |
|
95 | while(Schrittmotor_flag==1)
|
96 | {
|
97 | // -------- Aufsteigend -----------------
|
98 | if(Fahrt_Flag==1) // 1 = Aufsteigend
|
99 | {
|
100 | f = wurzel(2*Pos_ist*a);
|
101 | Match= F_CPU/f;
|
102 |
|
103 | if(Pos_ist >= Pos_P1)
|
104 | {
|
105 | fc=f;
|
106 | Fahrt_Flag=2;
|
107 | }
|
108 | }
|
109 |
|
110 | // -------- Konstant -----------------
|
111 | if(Fahrt_Flag==2) // 2 = Konstant
|
112 | {
|
113 | Match= F_CPU/fc;
|
114 | PORTB |=(1<<PB4); // LED gelb AN
|
115 |
|
116 | if(Pos_ist>=(Pos_soll-Pos_P1))
|
117 | {
|
118 | Fahrt_Flag=3;
|
119 | }
|
120 | }
|
121 |
|
122 | // -------- Absteigend -----------------
|
123 | if(Fahrt_Flag==3) // 3 = Absteigend
|
124 | {
|
125 | PORTB &= ~(1<<PB4); // LED gelb AUS
|
126 |
|
127 | f = wurzel(2*a*(Pos_soll-Pos_ist));
|
128 | Match= F_CPU/f;
|
129 |
|
130 | if(Pos_ist >= Pos_soll)
|
131 | {
|
132 | PORTD |= (1<<PD0); // Motor aus
|
133 | }
|
134 | }
|
135 | } // Ende while
|
136 | Schrittmotor_flag=0;
|
137 | } // Ende Schrittmotor
|
