Subversion Repositories FlightCtrl

Rev

Rev 839 | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 839 Rev 966
1
/*############################################################################
1
/*############################################################################
2
############################################################################*/
2
############################################################################*/
3
 
3
 
4
#include "main.h"
4
#include "main.h"
5
#include "kafi.h"
5
#include "kafi.h"
6
 
6
 
7
unsigned char twi_state = 0;
7
unsigned char twi_state = 0;
8
unsigned char motor = 0;
8
unsigned char motor = 0;
9
unsigned char motorread = 0;
9
unsigned char motorread = 0;
10
unsigned char motor_rx[8];
10
unsigned char motor_rx[8];
11
 
11
 
12
unsigned int TriggerMagneticHeading = 0;
12
unsigned int TriggerMagneticHeading = 0;
13
unsigned int TriggerSonicReading = 0;
13
unsigned int TriggerSonicReading = 0;
14
 
14
 
15
 
15
 
16
//############################################################################
16
//############################################################################
17
//Initzialisieren der I2C (TWI) Schnittstelle
17
//Initzialisieren der I2C (TWI) Schnittstelle
18
void i2c_init(void)
18
void i2c_init(void)
19
//############################################################################
19
//############################################################################
20
{
20
{
21
        TWSR = 0;
21
  TWSR = 0;
22
        TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
22
  TWBR = ((SYSCLK/SCL_CLOCK)-16)/2;
23
}
23
}
24
 
24
 
25
//############################################################################
25
//############################################################################
26
//Start I2C
26
//Start I2C
27
char i2c_start(void)
27
char i2c_start(void)
28
//############################################################################
28
//############################################################################
29
{
29
{
30
    TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
30
  TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);
31
    return(0);
31
  return(0);
32
}
32
}
33
 
33
 
34
//############################################################################
34
//############################################################################
35
//Start I2C
35
//Start I2C
36
void i2c_stop(void)
36
void i2c_stop(void)
37
//############################################################################
37
//############################################################################
38
{
38
{
39
    TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
39
  TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);
40
}
40
}
41
 
41
 
42
void i2c_reset(void)
42
void i2c_reset(void)
43
//############################################################################
43
//############################################################################
44
{
44
{
45
        i2c_stop();                
45
  i2c_stop();                
46
        twi_state = 0;
46
  twi_state = 0;
47
        motor = TWDR;
47
  motor = TWDR;
48
        motor = 0;
48
  motor = 0;
49
        TWCR = 0x80;
49
  TWCR = 0x80;
50
        TWAMR = 0;
50
  TWAMR = 0;
51
        TWAR = 0;
51
  TWAR = 0;
52
        TWDR = 0;
52
  TWDR = 0;
53
        TWSR = 0;
53
  TWSR = 0;
54
        TWBR = 0;
54
  TWBR = 0;
55
        i2c_init();
55
  i2c_init();
56
        i2c_start();
56
  i2c_start();
57
        i2c_write_byte(0);
57
  i2c_write_byte(0);
58
}
58
}
59
 
59
 
60
//############################################################################
60
//############################################################################
61
//Start I2C
61
//Start I2C
62
char i2c_write_byte(char byte)
62
char i2c_write_byte(char byte)
63
//############################################################################
63
//############################################################################
64
{
64
{
65
    TWSR = 0x00;
65
  TWSR = 0x00;
66
    TWDR = byte;
66
  TWDR = byte;
67
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
67
  TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
68
   
68
 
69
    return(0);    
69
  return(0);    
70
}
70
}
71
 
71
 
72
//############################################################################
72
//############################################################################
73
//Start I2C
73
//Start I2C
74
SIGNAL (TWI_vect)
74
SIGNAL (TWI_vect)
75
//############################################################################
75
//############################################################################
76
{
76
{
77
#if 0   
77
#if 0 
78
        int motorwert = 0;
78
  int motorwert = 0;
79
        int scale_p = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 4.F)  / 6;
79
  int scale_p = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 4.F)  / 6;
80
    int scale_d = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 4.F)  / 6;
80
  int scale_d = (int) (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 4.F)  / 6;
81
#endif
81
#endif
82
       
-
 
83
    switch (twi_state++)
82
  switch (twi_state++)
84
        {
83
  {
85
        case 0:
84
  case 0:
86
                i2c_write_byte(0x52+(motor*2));
85
    i2c_write_byte(0x52+(motor*2));
87
                break;
86
    break;
88
        case 1:
87
  case 1:
89
                switch(motor++)
88
    switch(motor++)
90
                {
89
    {
91
                case 0:
90
    case 0:
92
#if 0           
91
#if 0  
93
                        pd_ergebnis = (int) (scale_p* DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;                            
92
      pd_ergebnis = (int) (scale_p* DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;    
94
                        if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
93
      if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
95
                        {
94
      {
96
                                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
95
        pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
97
                        }
96
      }
98
                        if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
97
      if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
99
                        {
98
      {
100
                                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
99
        pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
101
                        }
100
      }
102
                        /* M o t o r   V o r n */      
101
      /* M o t o r   V o r n */
103
                        motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
102
      motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;
104
                        if ((motorwert < 0))
103
      if ((motorwert < 0))
105
                        {
104
      {
106
                                motorwert = 0;
105
        motorwert = 0;
107
                        }      
106
      }
108
                        else if(motorwert > MAX_GAS)
107
      else if(motorwert > MAX_GAS)
109
                        {
108
      {
110
                                motorwert = MAX_GAS;
109
        motorwert = MAX_GAS;
111
                        }
110
      }
112
                        if (motorwert < MIN_GAS)
111
      if (motorwert < MIN_GAS)
113
                        {
112
      {
114
                                motorwert = MIN_GAS;
113
        motorwert = MIN_GAS;
115
                        }
114
      }
116
                        Motor_Vorne = motorwert;         
115
      Motor_Vorne = motorwert;  
117
                        if(!MotorenEin)
116
      if(!MotorenEin)
118
                        {
117
      {
119
                                Motor_Vorne = 0;
118
        Motor_Vorne = 0;
120
                                if(MotorTest[0]) Motor_Vorne = MotorTest[0];
119
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
121
                        }
120
      }
122
#endif  
121
#endif 
123
                        i2c_write_byte(Motor_Vorne);
122
      i2c_write_byte(Motor_Vorne);
124
                        break;
123
      break;
125
                case 1:
124
    case 1:
126
#if 0           
125
#if 0  
127
                        pd_ergebnis = (scale_p * DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;                         
126
      pd_ergebnis = (scale_p * DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10;    
128
                        if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
127
      if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
129
                        {
128
      {
130
                                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
129
        pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
131
                        }
130
      }
132
                        if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
131
      if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
133
                        {
132
      {
134
                                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
133
        pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
135
                        }
134
      }
136
                        /* M o t o r   H e c k */
135
      /* M o t o r   H e c k */
137
                        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
136
      motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
138
                        if ((motorwert < 0))
137
      if ((motorwert < 0))
139
                        {
138
      {
140
                                motorwert = 0;
139
        motorwert = 0;
141
                        }
140
      }
142
                        else if(motorwert > MAX_GAS)
141
      else if(motorwert > MAX_GAS)
143
                        {
142
      {
144
                                motorwert = MAX_GAS;
143
        motorwert = MAX_GAS;
145
                        }
144
      }
146
                        if (motorwert < MIN_GAS)
145
      if (motorwert < MIN_GAS)
147
                        {
146
      {
148
                                motorwert = MIN_GAS;
147
        motorwert = MIN_GAS;
149
                        }
148
      }
150
                        Motor_Hinten = motorwert;      
149
      Motor_Hinten = motorwert;
151
                       
150
     
152
                        if(!MotorenEin)
151
      if(!MotorenEin)
153
                        {
152
      {
154
                                Motor_Hinten = 0;
153
        Motor_Hinten = 0;
155
                                if(MotorTest[1]) Motor_Hinten = MotorTest[1];
154
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
156
                        }                                      
155
      }    
157
#endif                                                  
156
#endif       
158
                        i2c_write_byte(Motor_Hinten);
157
      i2c_write_byte(Motor_Hinten);
159
                        break;
158
      break;
160
                case 2:
159
    case 2:
161
#if 0           
160
#if 0  
162
                        pd_ergebnis = (scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
161
      pd_ergebnis = (scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
163
                        if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
162
      if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
164
                        {
163
      {
165
                                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
164
        pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
166
                        }
165
      }
167
                        if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
166
      if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
168
                        {
167
      {
169
                                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
168
        pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
170
                        }
169
      }
171
                        /* M o t o r   R e c h t s */
170
      /* M o t o r   R e c h t s */
172
                        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;    
171
      motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
173
                        if (motorwert < 0)
172
      if (motorwert < 0)
174
                        {
173
      {
175
                                motorwert = 0;
174
        motorwert = 0;
176
                        }
175
      }
177
                        else if(motorwert > MAX_GAS)
176
      else if(motorwert > MAX_GAS)
178
                        {
177
      {
179
                                motorwert = MAX_GAS;
178
        motorwert = MAX_GAS;
180
                        }
179
      }
181
                        if (motorwert < MIN_GAS)
180
      if (motorwert < MIN_GAS)
182
                        {
181
      {
183
                                motorwert = MIN_GAS;   
182
        motorwert = MIN_GAS;
184
                        }
183
      }
185
                        Motor_Rechts = motorwert;      
184
      Motor_Rechts = motorwert;
186
                        if(!MotorenEin)
185
      if(!MotorenEin)
187
                        {
186
      {
188
                                Motor_Rechts = 0;
187
        Motor_Rechts = 0;
189
                                if(MotorTest[3]) Motor_Rechts = MotorTest[3];
188
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
190
                        }
189
      }
191
#endif                                                                  
190
#endif         
192
                        i2c_write_byte(Motor_Rechts);
191
      i2c_write_byte(Motor_Rechts);
193
                        break;
192
      break;
194
                case 3:        
193
    case 3:  
195
#if 0
194
#if 0
196
                        pd_ergebnis =  (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
195
      pd_ergebnis =  (scale_p* DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10;
197
                        if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
196
      if(pd_ergebnis >  (GasMischanteil + abs(GierMischanteil)))
198
                        {
197
      {
199
                                pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
198
        pd_ergebnis =  (GasMischanteil + abs(GierMischanteil));
200
                        }
199
      }
201
                        if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
200
      if(pd_ergebnis < -(GasMischanteil + abs(GierMischanteil)))
202
                        {
201
      {
203
                                pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
202
        pd_ergebnis = -(GasMischanteil + abs(GierMischanteil));
204
                        }
203
      }
205
                        /* M o t o r   L i n k s */
204
      /* M o t o r   L i n k s */
206
                        motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
205
      motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
207
                        if (motorwert < 0)
206
      if (motorwert < 0)
208
                        {
207
      {
209
                                motorwert = 0;
208
        motorwert = 0;
210
                        }
209
      }
211
                        else if(motorwert > MAX_GAS)
210
      else if(motorwert > MAX_GAS)
212
                        {
211
      {
213
                                motorwert = MAX_GAS;
212
        motorwert = MAX_GAS;
214
                        }
213
      }
215
                        if (motorwert < MIN_GAS)
214
      if (motorwert < MIN_GAS)
216
                        {
215
      {
217
                                motorwert = MIN_GAS;
216
        motorwert = MIN_GAS;
218
                        }
217
      }
219
                        Motor_Links = motorwert;                       
218
      Motor_Links = motorwert;  
220
                        if(!MotorenEin)
219
      if(!MotorenEin)
221
                        {
220
      {
222
                                Motor_Links = 0;
221
        Motor_Links = 0;
223
                                if(MotorTest[2]) Motor_Links = MotorTest[2];
222
        if(MotorTest[2]) Motor_Links = MotorTest[2];
224
                        }
223
      }
225
#endif                          
224
#endif    
226
                        i2c_write_byte(Motor_Links);
225
      i2c_write_byte(Motor_Links);
227
                        break;
226
      break;
228
                    }
227
    }
229
                                        break;
228
    break;
230
        case 2:
229
    case 2:
231
                        i2c_stop();
230
      i2c_stop();
232
                        if (motor<4) twi_state = 0;
231
      if (motor<4) twi_state = 0;
233
                        else motor = 0;
232
      else motor = 0;
234
                        i2c_start();  
233
      i2c_start();  
235
                        break;
234
      break;
236
                       
235
     
237
                        //Liest Daten von Motor
236
      //Liest Daten von Motor
238
        case 3:
237
    case 3:
239
                        i2c_write_byte(0x53+(motorread*2));
238
      i2c_write_byte(0x53+(motorread*2));
240
                        break;
239
      break;
241
        case 4:
240
    case 4:
242
                        switch(motorread)
241
      switch(motorread)
243
                        {
242
      {
244
                        case 0:
243
      case 0:
245
                                i2c_write_byte(Motor_Vorne);
244
        i2c_write_byte(Motor_Vorne);
246
                                break;
245
        break;
247
                        case 1:
246
      case 1:
248
                                i2c_write_byte(Motor_Hinten);
247
        i2c_write_byte(Motor_Hinten);
249
                                break;
248
        break;
250
                        case 2:
249
      case 2:
251
                                i2c_write_byte(Motor_Rechts);
250
        i2c_write_byte(Motor_Rechts);
252
                                break;
251
        break;
253
                        case 3:
252
      case 3:
254
                                i2c_write_byte(Motor_Links);
253
        i2c_write_byte(Motor_Links);
255
                                break;
254
        break;
256
                        }
255
      }
257
                        break;
256
      break;
258
                        case 5: //1 Byte vom Motor lesen       
257
      case 5: //1 Byte vom Motor lesen       
259
                motor_rx[motorread] = TWDR;
258
        motor_rx[motorread] = TWDR;
260
                               
259
       
261
                        case 6:
260
      case 6:
262
                switch(motorread)
261
        switch(motorread)
263
                                {
262
        {
264
                                case 0:
263
        case 0:
265
                                        i2c_write_byte(Motor_Vorne);
264
          i2c_write_byte(Motor_Vorne);
266
                                        break;
265
          break;
267
                                case 1:
266
        case 1:
268
                                        i2c_write_byte(Motor_Hinten);
267
          i2c_write_byte(Motor_Hinten);
269
                                        break;
268
          break;
270
                                case 2:
269
        case 2:
271
                                        i2c_write_byte(Motor_Rechts);
270
          i2c_write_byte(Motor_Rechts);
272
                                        break;
271
          break;
273
                                case 3:
272
        case 3:
274
                                        i2c_write_byte(Motor_Links);
273
          i2c_write_byte(Motor_Links);
275
                                        break;
274
          break;
276
                                }
275
        }
277
                break;  
276
        break;  
278
                                case 7: //2 Byte vom Motor lesen       
277
        case 7: //2 Byte vom Motor lesen       
279
                                        motor_rx[motorread+4] = TWDR;
278
          motor_rx[motorread+4] = TWDR;
280
                                        motorread++;
279
          motorread++;
281
                                        if (motorread>3) motorread=0;
280
          if (motorread>3) motorread=0;
282
                                        i2c_stop();
281
          i2c_stop();
283
                                        I2CTimeout = 10;
282
          I2CTimeout = 10;
284
                                        twi_state = 0;
283
          twi_state = 0;
285
}
284
        }
286
                        #if 0
285
#if 0        
287
                        break; 
286
        break;
288
                        TriggerMagneticHeading++;
287
        TriggerMagneticHeading++;
289
                        TriggerMagneticHeading %= 50;
288
        TriggerMagneticHeading %= 50;
290
                        TriggerSonicReading++;
289
        TriggerSonicReading++;
291
                        TriggerSonicReading %= 200;
290
        TriggerSonicReading %= 200;
292
 
291
       
293
                        if (TriggerMagneticHeading == 1)
292
        if (TriggerMagneticHeading == 1)
294
                        {
293
        {
295
                                // Get Magnetic Heading 
294
          // Get Magnetic Heading 
296
                                i2c_start();  
295
          i2c_start();  
297
                                twi_state = 8;
296
          twi_state = 8;
298
                                break;
297
          break;
299
                        }
298
        }
300
 
299
       
301
                        if (TriggerSonicReading == 1)
300
        if (TriggerSonicReading == 1)
302
                        {
301
        {
303
                                // Get Ultrasonic Reading
302
          // Get Ultrasonic Reading
304
                                i2c_start();  
303
          i2c_start();  
305
                                twi_state = 20;
304
          twi_state = 20;
306
                                break;           
305
          break;  
307
                        }
306
        }
308
                        else if (TriggerSonicReading == 190)
307
        else if (TriggerSonicReading == 190)
309
                        {
308
        {
310
                                // Get Ultrasonic Reading
309
          // Get Ultrasonic Reading
311
                                i2c_start();  
310
          i2c_start();  
312
                                twi_state = 30;
311
          twi_state = 30;
313
                                break;           
312
          break;  
314
                        }
313
        }
315
                        else
314
        else
316
                        {              
315
        {  
317
                                I2CTimeout = 10;
316
          I2CTimeout = 10;
318
                                twi_state = 0;
317
          twi_state = 0;
319
                                break;
318
          break;
320
                        }
319
        }
321
 
320
       
322
                                case 8:
321
        case 8:
323
                                        i2c_write_byte(0xC0);
322
          i2c_write_byte(0xC0);
324
                                        break;
323
          break;
325
                                case 9:
324
        case 9:
326
                                        i2c_write_byte(0x02);
325
          i2c_write_byte(0x02);
327
                                        break;
326
          break;
328
                                case 10:
327
        case 10:
329
                                        i2c_start();
328
          i2c_start();
330
                                        break;
329
          break;
331
                                case 11:
330
        case 11:
332
                                        i2c_write_byte(0xC1);
331
          i2c_write_byte(0xC1);
333
                                        break;                 
332
          break;  
334
                                case 12:
333
        case 12:
335
                                        i2c_write_byte(MagneticHeading);
334
          i2c_write_byte(MagneticHeading);
336
                                        break;                 
335
          break;  
337
                                case 13:
336
        case 13:
338
                                        MagneticHeading = (0x0F & TWDR)  << 8;
337
          MagneticHeading = (0x0F & TWDR)  << 8;
339
                                        i2c_stop();
338
          i2c_stop();
340
                                        i2c_start();  
339
          i2c_start();  
341
                                        break;
340
          break;
342
                                case 14:
341
        case 14:
343
                                        i2c_write_byte(0xC0);
342
          i2c_write_byte(0xC0);
344
                                        break;
343
          break;
345
                                case 15:
344
        case 15:
346
                                        i2c_write_byte(0x03);
345
          i2c_write_byte(0x03);
347
                                        break;
346
          break;
348
                                case 16:
347
        case 16:
349
                                        i2c_start();
348
          i2c_start();
350
                                        break;
349
          break;
351
                                case 17:
350
        case 17:
352
                                        i2c_write_byte(0xC1);
351
          i2c_write_byte(0xC1);
353
                                        break;                 
352
          break;  
354
                                case 18:
353
        case 18:
355
                                        i2c_write_byte(MagneticHeading);
354
          i2c_write_byte(MagneticHeading);
356
                                        break;                 
355
          break;  
357
                                case 19:
356
        case 19:
358
                                        MagneticHeading += TWDR;                               
357
          MagneticHeading += TWDR;    
359
                                        i2c_stop();
358
          i2c_stop();
360
                                        I2CTimeout = 22;
359
          I2CTimeout = 22;
361
                                        twi_state = 0;
360
          twi_state = 0;
362
                                        break;
361
          break;
363
                                       
362
         
364
                                        /* Trigger Ultrasonic Ping */
363
          /* Trigger Ultrasonic Ping */
365
                                case 20:  /* Ping */
364
        case 20:  /* Ping */
366
                                        i2c_write_byte(0xE0);  /* Send I2C Adress */
365
          i2c_write_byte(0xE0);  /* Send I2C Adress */
367
                                        break;
366
          break;
368
                                case 21:
367
        case 21:
369
                                        i2c_write_byte(0x00);  /* Send I2C Register */
368
          i2c_write_byte(0x00);  /* Send I2C Register */
370
                                        break;
369
          break;
371
                                case 22:
370
        case 22:
372
                                        i2c_write_byte(82); /* Send I2C Value */
371
          i2c_write_byte(82); /* Send I2C Value */
373
                                        break;
372
          break;
374
                                case 24:
373
        case 24:
375
                                        VersionID = TWDR;
374
          VersionID = TWDR;
376
                                        i2c_stop();
375
          i2c_stop();
377
                                        I2CTimeout = 22;
376
          I2CTimeout = 22;
378
                                        twi_state = 0;
377
          twi_state = 0;
379
                                        UltrasonicPingCnt++;
378
          UltrasonicPingCnt++;
380
                                        break;
379
          break;
381
                                case 30:
380
        case 30:
382
                                        i2c_write_byte(0xE0);/* Send I2C Adress */
381
          i2c_write_byte(0xE0);/* Send I2C Adress */
383
                                        break;
382
          break;
384
                                case 31:
383
        case 31:
385
                                        i2c_write_byte(0x02); /* Send I2C Register */
384
          i2c_write_byte(0x02); /* Send I2C Register */
386
                                        break;
385
          break;
387
                                case 32:
386
        case 32:
388
                                        i2c_start();
387
          i2c_start();
389
                                        break;
388
          break;
390
                                case 33:
389
        case 33:
391
                                        i2c_write_byte(0xE1);
390
          i2c_write_byte(0xE1);
392
                                        break;                 
391
          break;  
393
                                case 34:
392
        case 34:
394
                                        i2c_write_byte(255);
393
          i2c_write_byte(255);
395
                                        break;                 
394
          break;  
396
                                case 35:
395
        case 35:
397
                                        UltrasonicRange = TWDR  << 8;/* Read I2C Value */
396
          UltrasonicRange = TWDR  << 8;/* Read I2C Value */
398
                                        UltrasonicRangeHigh = TWDR;
397
          UltrasonicRangeHigh = TWDR;
399
                                        i2c_stop();
398
          i2c_stop();
400
                                        i2c_start();  
399
          i2c_start();  
401
                                        break;
400
          break;
402
                                case 36:
401
        case 36:
403
                                        i2c_write_byte(0xE0);
402
          i2c_write_byte(0xE0);
404
                                        break;
403
          break;
405
                                case 37:
404
        case 37:
406
                                        i2c_write_byte(0x03);
405
          i2c_write_byte(0x03);
407
                                        break;
406
          break;
408
                                case 38:
407
        case 38:
409
                                        i2c_start();
408
          i2c_start();
410
                                        break;
409
          break;
411
                                case 39:
410
        case 39:
412
                                        i2c_write_byte(0xE1);
411
          i2c_write_byte(0xE1);
413
                                        break;                 
412
          break;  
414
                                case 40:
413
        case 40:
415
                                        i2c_write_byte(UltrasonicRangeLow);
414
          i2c_write_byte(UltrasonicRangeLow);
416
                                        break;                 
415
          break;  
417
                                case 41:
416
        case 41:
418
                                        UltrasonicRange += TWDR;               
417
          UltrasonicRange += TWDR;  
419
                                        UltrasonicRangeLow = TWDR;             
418
          UltrasonicRangeLow = TWDR;  
420
                                        i2c_stop();
419
          i2c_stop();
421
                                        I2CTimeout = 22;
420
          I2CTimeout = 22;
422
                                        twi_state = 0;
421
          twi_state = 0;
423
                                        break;         
422
          break;
424
    }
423
    }
425
#endif
424
#endif
426
        TWCR |= 0x80;
425
    TWCR |= 0x80;
427
 }
426
 }
428
 
427