Rev Author Line No. Line
615 kaklik 1 // Features:
2 // - Complete PID loop
3 // - Optional proportional feed-forward for open-loop control and wind-up minimization
4 // - Wind-up limiting under overload conditions
5 // - Torque throttling under overload
6 // - TWI interface with PnP support
7 // - Parameter storage in EEPROM including TWI address
8 // - Acceleration, deceleration limits
9 // - Speed integration to estimate travelled distance
10 // - Add 'go to distance' mode with trapezoid speed-profiles - Needs testing
11 // - Added 32-bit and 16-bit atomic reads and writes. Note: 8-bit atomic writes are NOT supported
12 // - Added support for duty cycle throttling (basically torque-throttling)
13 // - Added current reading and max current detection
14 // - Added per cycle over-current detection and early-termination of cycle
15 // - Added servo-type operation with pot-based position feedback
16  
17 // TODO:
18 // - Add current integration for power usage estimation
19 // - Add serial interface
20 // - Add servo-type (PWM) interface
21 // - Add optical encoder support
22 // - When sampling for fast-collapse high-side and during the on-state: we only need two states - the high-side should be on already
23 // - Switching between high- and low-collapse modes to equalize catch-diode load - make this user-selectable
24 // - Detect continous-current mode and do something about it!
25  
26 // TODO TEST:
27 // - Servo mode in all four modes
28 // - Freewheeling and back-EMF in all four modes
29 // - Braking
30  
31 // BUGS:
32 // - back-EMF measurment is different in fast-collapse and low-collapse modes. This make control leading to different speeds in the two modes.
33  
34 #include <avr/pgmspace.h>
35 #include <avr/interrupt.h> #include <avr/sleep.h>
36
37  
38 #include "opled.h"
39 #include "usart.h"
40 #include "twi_aap.h"
41 #include "eeprom.h"
42
43  
44
45  
46
47  
48 namespace PWM1 {
49 enum eClkSrc {
50 ClkNone = 0,
51 Clk = (1 << CS10),
52 ClkDiv1 = (1 << CS10),
53 ClkDiv8 = (1 << CS11),
54 ClkDiv64 = (1 << CS11) | (1 << CS10),
55 ClkDiv256 = (1 << CS12),
56 ClkDiv1024 = (1 << CS12) | (1 << CS10),
57 ClkExtFall = (1 << CS12) | (1 << CS11),
58 ClkExtRise = (1 << CS12) | (1 << CS11) | (1 << CS10)
59 };
60
61  
62 SETBIT(DDRB,0x02|0x04); // Preprare PortB to output to handle disables
63 CLRBIT(PORTB,0x02|0x04); // Set port bits to 0 so that disable will work correctly
64 CLRBIT(PRR,PRTIM0);
65 TCCR1A = (1 << WGM10);
66 TCCR1B = aClkSrc | (1 << WGM12);
67 OCR1A = 0x0000;
68 OCR1B = 0x0000;
69 ICR1 = 0x0000;
70 TCNT1 = 0x0000;
71 }
72 void inline DisableChA() { CLRBIT(TCCR1A,(1 << COM1A1)); }
73 void inline DisableChB() { CLRBIT(TCCR1A,(1 << COM1B1)); }
74 void inline DisableChAB() { CLRBIT(TCCR1A,(1 << COM1A1) | (1 << COM1B1)); }
75 void inline EnableChA() { SETBIT(TCCR1A,(1 << COM1A1)); }
76 void inline EnableChB() { SETBIT(TCCR1A,(1 << COM1B1)); }
77 void inline EnableChAB() { SETBIT(TCCR1A,(1 << COM1A1) | (1 << COM1B1)); }
78 bool inline IsChAEnabled() { return TESTBIT(TCCR1A,(1 << COM1A1)) != 0; }
79 bool inline IsChBEnabled() { return TESTBIT(TCCR1A,(1 << COM1B1)) != 0; }
80 bool inline IsChAOrBEnabled() { return TESTBIT(TCCR1A,(1 << COM1A1) | (1 << COM1B1)) != 0; }
81 // We never set the high bits to anything but 0, so the high-byte in the TEMP register doesn't need to be set.
82 void inline SetChannelA(uint8_t aValue) { OCR1AL = aValue; }
83 uint8_t inline GetChannelA() { return OCR1AL; }
84 void inline SetChannelB(uint8_t aValue) { OCR1BL = aValue; }
85 uint8_t inline GetChannelB() { return OCR1BL; }
86 void inline EnableIRQ_A() { SETBIT(TIMSK1,(1 << OCIE1A)); }
87 void inline EnableIRQ_B() { SETBIT(TIMSK1,(1 << OCIE1B)); }
88 void inline EnableIRQ_AB() { SETBIT(TIMSK1,(1 << OCIE1A) | (1 << OCIE1B)); }
89 void inline EnableIRQ_Overflow() { SETBIT(TIMSK1,(1 << TOIE1)); }
90 void inline EnableOnlyIRQ_A() { TIMSK1 = (1 << OCIE1A); }
91 void inline EnableOnlyIRQ_B() { TIMSK1 = (1 << OCIE1B); }
92 void inline EnableOnlyIRQ_Overflow() { TIMSK1 = (1 << TOIE1); }
93 void inline DisableIRQ_A() { CLRBIT(TIMSK1,(1 << OCIE1A)); }
94 void inline DisableIRQ_B() { CLRBIT(TIMSK1,(1 << OCIE1B)); }
95 void inline DisableIRQ_Overflow() { CLRBIT(TIMSK1,(1 << TOIE1)); }
96 void inline DisableIRQ_All() { TIMSK1 = 0; }
97 bool inline IsPendingIRQ_A() { return (TESTBIT(TIFR1,(1 << OCF1A)) != 0); }
98 bool inline IsPendingIRQ_B() { return (TESTBIT(TIFR1,(1 << OCF1B)) != 0); }
99 bool inline IsPendingIRQ_Overflow() { return (TESTBIT(TIFR1,(1 << TOV1)) != 0); }
100 void inline ClearPendingIRQ_A() { TIFR1 = (1 << OCF1A); }
101 void inline ClearPendingIRQ_B() { TIFR1 = (1 << OCF1B); }
102 void inline ClearPendingIRQ_Overflow() { TIFR1 = (1 << TOV1); }
103 void inline ClearPendingIRQ_All() { TIFR1 = (1 << OCF1A) | (1 << OCF1B) | (1 << TOV1); }
104 }
105
106  
107 enum eRef {
108 RefInt = (1 << REFS0) | (1 << REFS1),
109 RefVcc = (1 << REFS0),
110 RefExt = 0
111 };
112 enum eAdjust {
113 LeftAdjust = ADLAR,
114 RightAdjust = 0
115 };
116 #if defined MEGA_BRIDGE
117 const uint8_t Ch_MotorA = 7;
118 const uint8_t Ch_MotorB = 2;
119 const uint8_t Ch_MotorCurrent = 6;
120 const uint8_t Ch_ServoPot = 3;
121 const uint8_t Ch_Battery = 1;
122
123  
124 const eRef Ref_MotorB = RefVcc;
125 const eRef Ref_MotorCurrent = RefInt;
126 const eRef Ref_ServoPot = RefVcc;
127 const eRef Ref_Battery = RefVcc;
128 #elif defined H_BRIDGE
129 const uint8_t Ch_MotorA = 7;
130 const uint8_t Ch_MotorB = 2;
131 const uint8_t Ch_MotorCurrent = 6;
132 const uint8_t Ch_ServoPot = 3;
133
134  
135 const eRef Ref_MotorB = RefInt;
136 const eRef Ref_MotorCurrent = RefInt;
137 const eRef Ref_ServoPot = RefVcc;
138 const eRef Ref_Battery = RefVcc;
139 #elif defined SERVO_BRAIN
140 const uint8_t Ch_MotorA = 0;
141 const uint8_t Ch_MotorB = 2;
142 const uint8_t Ch_MotorCurrent = 6;
143 const uint8_t Ch_ServoPot = 3;
144
145  
146 const eRef Ref_MotorB = RefVcc;
147 const eRef Ref_MotorCurrent = RefVcc;
148 const eRef Ref_ServoPot = RefVcc;
149 const eRef Ref_Battery = RefVcc;
150 #else
151 #error HW version is not specified
152 #endif
153
154  
155 static inline void Init() {
156 ADMUX = (1<< REFS0) | 15; // AVCC is the default reference
157 ADCSRA = ADCSRA_BaseValue;
158 ADCSRB = 0; // Free-running mode
159 DIDR0 = (1 << Ch_MotorA) | (1 << Ch_MotorB) | (1 << Ch_MotorCurrent);
160 }
161 static inline void SetChannel(uint8_t aChannel) {
162 ADMUX = (ADMUX & 0xf0) | (aChannel & 0x0f);
163 }
164 static inline void SetChannel(uint8_t aChannel,eRef aRef) {
165 ADMUX = (ADMUX & 0x20) | (aChannel & 0x0f) | aRef;
166 }
167 static inline void SetChannel(uint8_t aChannel,eRef aRef,eAdjust aAdjust) {
168 ADMUX = (aChannel & 0x0f) | aRef | aAdjust;
169 }
170 static inline void StartConversion(uint8_t aChannel) {
171 SetChannel(aChannel);
172 ADCSRA = ADCSRA_BaseValue | (1 << ADSC) | (1 << ADIE);
173 }
174 static inline void StartConversion(uint8_t aChannel,eRef aRef) {
175 SetChannel(aChannel,aRef);
176 ADCSRA = ADCSRA_BaseValue | (1 << ADSC) | (1 << ADIE);
177 }
178 static inline void StartConversion(uint8_t aChannel,eRef aRef, eAdjust aAdjust) {
179 SetChannel(aChannel,aRef,aAdjust);
180 ADCSRA = ADCSRA_BaseValue | (1 << ADSC) | (1 << ADIE);
181 }
182 static inline void StopConversion() {
183 CLRBIT(ADCSRA,(1 << ADEN)); // This might delete the pending IRQ request, but we don't really care
184 }
185 static inline uint8_t GetChannel() {
186 return ADMUX & 0x0f;
187 }
188 static uint16_t GetSample() {
189 while((ADCSRA & (1 << ADIF)) == 0); // Wait until conversion finishes
190 uint16_t RetVal = ADC;
191 SETBIT(ADCSRA,ADIF); // Clear the pending interrupt request (though we're polling)
192 return RetVal;
193 }
194 static inline uint16_t FastGetSample() {
195 return ADC;
196 }
197 }
198
199  
200 const uint16_t DataRecord_Ofs = 0x00; // size is sizeof(HBridge::DataRecord)
201 const uint16_t DataValid_Ofs = 0xfe;
202 }
203
204  
205 template<typename T> inline T max(T aA,T aB) { return (aA>aB)?aA:aB; }
206
207  
208
209  
210 enum eSampleStates {
211 SampleState_PreFastCollapse = 0,
212 SampleState_FastCollapse,
213 SampleState_PostFastCollapse,
214
215  
216 SampleState_PreSampleBase2,
217 SampleState_SampleBase,
218 SampleState_PreSearchMax,
219 SampleState_PreSamplePot,
220 SampleState_SamplePot,
221 SampleState_SearchMax,
222 SampleState_SearchMin,
223 SampleState_PreCurrentSample,
224 SampleState_CurrentSample1,
225 SampleState_CurrentSample2,
226 SampleState_CurrentSample3,
227 // off-band battery sampling for fast-high-side collapse mode
228 SampleState_PreSampleBat,
229 SampleState_SampleBat
230 };
231 enum eOperatingModes {
232 OperatingMode_Speed = 0,
233 OperatingMode_Servo = 1
234 };
235
236  
237 const int16_t RequestBrake = 0x4001;
238
239  
240
241  
242 const uint8_t ControlTime = 255 - 10;
243 #if defined MEGA_BRIDGE
244 const uint8_t LoBMask = (0x01 << 2);
245 const uint8_t LoAMask = (0x01 << 1);
246 const uint8_t HiBMask = (0x01 << 5);
247 const uint8_t HiAMask = (0x01 << 6);
248 const eOperatingModes DefaultOperatingMode = OperatingMode_Speed;
249
250  
251 const int16_t Def_PFactor = 0;
252 const int16_t Def_DFactor = 0;
253 const int16_t Def_PFFactor = 0x0100;
254 #elif defined H_BRIDGE
255 const uint8_t LoBMask = (0x01 << 2);
256 const uint8_t LoAMask = (0x01 << 1);
257 const uint8_t HiBMask = (0x01 << 5);
258 const uint8_t HiAMask = (0x01 << 6);
259 const eOperatingModes DefaultOperatingMode = OperatingMode_Speed;
260
261  
262 const int16_t Def_PFactor = 0;
263 const int16_t Def_DFactor = 0;
264 const int16_t Def_PFFactor = 0x0100;
265 #elif defined SERVO_BRAIN
266 const uint8_t LoBMask = (0x01 << 2);
267 const uint8_t LoAMask = (0x01 << 1);
268 const uint8_t HiBMask = (0x01 << 6);
269 const uint8_t HiAMask = (0x01 << 5);
270 const eOperatingModes DefaultOperatingMode = OperatingMode_Servo;
271
272  
273 const int16_t Def_PFactor = 0x0200;
274 const int16_t Def_DFactor = 0;
275 const int16_t Def_PFFactor = 0;
276 #else
277 #error No HW version is specified!
278 #endif
279
280  
281 FastCollapseHighSide = 0,
282 FastCollapseLowSide,
283 SlowCollapseHighSide,
284 SlowCollapseLowSide,
285 CollapseStateMask = 0x0f,
286 CollapseStateAutoCycle = 0x10
287 };
288
289  
290 struct PublicData {
291 // The layout of this record IS important.
292 // This is the public interface that is accessible through the TWI interface
293
294  
295 // 0x4000 is freewheeling and 0x4001 is braking
296 int16_t RequestValue;
297
298  
299 int16_t IFactor;
300 int16_t PFactor;
301 int16_t DFactor;
302 int16_t PFFactor;
303 int16_t SampleOffset;
304
305  
306 int16_t MaxPositiveChange;
307 int16_t MaxNegativeChange;
308
309  
310 int32_t Distance;
311
312  
313 int32_t FwDistanceLimit;
314 int32_t BwDistanceLimit;
315
316  
317 int32_t DistanceToStop;
318
319  
320 int16_t CurrentRequest;
321
322  
323 int16_t Command;
324
325  
326 int16_t IValue;
327 int16_t LastError;
328 int16_t Error;
329
330  
331 int16_t VoltageSample;
332
333  
334 int16_t BaseValue;
335
336  
337 int16_t MinValue_Snapshot;
338
339  
340 int16_t MinValue;
341 int16_t MaxValue;
342 int16_t SampleCnt;
343
344  
345
346  
347 uint8_t DutyCycleThrottle;
348 uint8_t NewData;
349 uint8_t IsForward;
350
351  
352 uint16_t CurrentDelta;
353 uint16_t CurrentTemp;
354 uint16_t CurrentMaxSearch;
355
356  
357 uint8_t ADBufferEnableHost;
358
359  
360
361  
362 uint8_t CollapseState;
363
364  
365 } DataRecord;
366
367  
368 inline uint16_t* GetDataRecord16(uint8_t aOfs) { return (uint16_t *)(((uint8_t *)&DataRecord)+aOfs); }
369 inline uint32_t* GetDataRecord32(uint8_t aOfs) { return (uint32_t *)(((uint8_t *)&DataRecord)+aOfs); }
370 const inline size_t GetDataRecordSize() { return sizeof(DataRecord); }
371 uint8_t GetDataElementSize(uint8_t aOfs) {
372 #define FIELD_OFFSET(aField) ((uint8_t)((size_t)&(HBridge::DataRecord.aField)-(size_t)&HBridge::DataRecord))
373 #define OFFSET_ENTRY(aField) if (aOfs >= FIELD_OFFSET(aField)) return sizeof(DataRecord.aField); else
374 #define OFFSET_ENTRY_2(aField,aSize) if (aOfs >= FIELD_OFFSET(aField)) return (aSize); else
375 OFFSET_ENTRY_2(ADBuffer,sizeof(DataRecord.ADBuffer[0]))
376 OFFSET_ENTRY(CollapseState)
377 OFFSET_ENTRY(OperatingMode)
378 OFFSET_ENTRY(CurrentLimit)
379 OFFSET_ENTRY(ADBufferEnableHost)
380 OFFSET_ENTRY(ADBufferEnable)
381 OFFSET_ENTRY(CurrentMaxSearch)
382 OFFSET_ENTRY(CurrentTemp)
383 OFFSET_ENTRY(CurrentDelta)
384 OFFSET_ENTRY(CurrentMax)
385 OFFSET_ENTRY(IsForward)
386 OFFSET_ENTRY(NewData)
387 OFFSET_ENTRY(DutyCycleThrottle)
388 OFFSET_ENTRY(SampleState)
389 OFFSET_ENTRY(OriginalRequestValue)
390 OFFSET_ENTRY(SampleCnt)
391 OFFSET_ENTRY(MaxValue)
392 OFFSET_ENTRY(MinValue)
393 OFFSET_ENTRY(MinValue_Snapshot)
394 OFFSET_ENTRY(SampleCnt_Snapshot)
395 OFFSET_ENTRY(BaseValue)
396 OFFSET_ENTRY(VoltageSample)
397 OFFSET_ENTRY(Error)
398 OFFSET_ENTRY(LastError)
399 OFFSET_ENTRY(IValue)
400 OFFSET_ENTRY(Command)
401 OFFSET_ENTRY(CurrentRequest)
402 OFFSET_ENTRY(DistanceToStop)
403 OFFSET_ENTRY(BwDistanceLimit)
404 OFFSET_ENTRY(FwDistanceLimit)
405 OFFSET_ENTRY(Distance)
406 OFFSET_ENTRY(MaxNegativeChange)
407 OFFSET_ENTRY(MaxPositiveChange)
408 OFFSET_ENTRY(SampleOffset)
409 OFFSET_ENTRY(PFFactor)
410 OFFSET_ENTRY(DFactor)
411 OFFSET_ENTRY(PFactor)
412 OFFSET_ENTRY(IFactor)
413 OFFSET_ENTRY(RequestValue)
414 return 1;
415 #undef FIELD_OFFSET
416 #undef OFFSET_ENTRY
417 #undef OFFSET_ENTRY2
418 }
419
420  
421
422  
423 uint8_t *Data = (uint8_t *)&DataRecord;
424 for(uint8_t i=EEPROM_layout::DataRecord_Ofs;i<EEPROM_layout::DataRecord_Ofs+sizeof(DataRecord);++i,++Data) {
425 EEPROM::SetByte(i,*Data);
426 }
427 EEPROM::SetByte(EEPROM_layout::DataValid_Ofs,EEPROMDataValid);
428 EEPROM::Wait();
429 }
430
431  
432 // Init to all channels off - this is the free-wheeling state
433 PWM1::Init(PWM1::ClkDiv256); // Set clock to clkI/O / 256 -> full cycle is around 120Hz with an 8MHz clock
434 //PWM1::Init(PWM1::ClkDiv64); // Set clock to clkI/O / 64 -> full cycle is around 500Hz with an 8MHz clock
435 CLRBIT(PORTD,HiAMask);
436 CLRBIT(PORTD,HiBMask);
437 CLRBIT(PORTB,LoBMask);
438 CLRBIT(PORTB,LoBMask);
439 SETBIT(DDRB,LoBMask);
440 SETBIT(DDRB,LoAMask);
441 SETBIT(DDRD,HiBMask);
442 SETBIT(DDRD,HiAMask);
443
444  
445 DataRecord.IFactor = Def_IFactor;
446 DataRecord.PFactor = Def_PFactor;
447 DataRecord.DFactor = Def_DFactor;
448 DataRecord.PFFactor = Def_PFFactor;
449 DataRecord.SampleOffset = 0;
450 DataRecord.CollapseState = FastCollapseHighSide;
451 DataRecord.OperatingMode = DefaultOperatingMode;
452 // No acceleration control - this is the default.
453 DataRecord.MaxPositiveChange = 0x7fff;
454 DataRecord.MaxNegativeChange = 0x7fff;
455
456  
457 uint8_t *Data = (uint8_t *)&DataRecord;
458 for(uint8_t i=EEPROM_layout::DataRecord_Ofs;i<EEPROM_layout::DataRecord_Ofs+sizeof(DataRecord);++i,++Data) {
459 *Data = EEPROM::GetByte(i);
460 }
461 }
462
463  
464 DataRecord.Error = 0;
465 DataRecord.LastError = 0;
466 DataRecord.RequestValue = RequestFreewheel;
467 DataRecord.OriginalRequestValue = RequestFreewheel;
468 DataRecord.IsForward = true;
469 DataRecord.NewData = false;
470 DataRecord.Distance = 0;
471 DataRecord.FwDistanceLimit = 0x7fffffff;
472 DataRecord.BwDistanceLimit = 0x80000000;
473 DataRecord.DutyCycleThrottle = GuardTime;
474 DataRecord.CurrentLimit = 0xffff; // Anything over 0x3ff is OFF
475 for(uint8_t i = 0;i<sizeof(DataRecord.ADBuffer)/sizeof(DataRecord.ADBuffer[0]);++i) {
476 DataRecord.ADBuffer[i] = 0;
477 }
478 ADSampleIdx = 0;
479 DataRecord.ADBufferEnable = 0;
480 DataRecord.ADBufferEnableHost = true;
481
482  
483 PWM1::EnableIRQ_AB();
484
485  
486 ADConv::StartConversion(ADConv::Ch_MotorCurrent);
487 }
488 // If made inline GCC generates invalid code
489 void Forward(uint8_t aSpeed) {
490 if (aSpeed > DataRecord.DutyCycleThrottle) aSpeed = DataRecord.DutyCycleThrottle;
491 if (aSpeed > GuardTime) aSpeed = GuardTime;
492 // Allways clear first, than set
493 CLRBIT(PORTB,LoBMask);
494 CLRBIT(PORTB,LoAMask);
495 CLRBIT(PORTD,HiBMask);
496 SETBIT(PORTD,HiAMask);
497 PWM1::SetChannelB(aSpeed);
498 PWM1::SetChannelA(ControlTime);
499 PWM1::DisableChAB();
500 if (aSpeed > 0) PWM1::EnableChB();
501 DataRecord.IsForward = true;
502 }
503 void Backward(uint8_t aSpeed) {
504 if (aSpeed > DataRecord.DutyCycleThrottle) aSpeed = DataRecord.DutyCycleThrottle;
505 if (aSpeed > GuardTime) aSpeed = GuardTime;
506 // Allways clear first, than set
507 CLRBIT(PORTB,LoBMask);
508 CLRBIT(PORTB,LoAMask);
509 CLRBIT(PORTD,HiAMask);
510 SETBIT(PORTD,HiBMask);
511 PWM1::SetChannelA(aSpeed);
512 PWM1::SetChannelB(ControlTime);
513 PWM1::DisableChAB();
514 if (aSpeed > 0) PWM1::EnableChA();
515 DataRecord.IsForward = false;
516 }
517 inline void FastFieldCollapseHighSide() {
518 if (PWM1::IsChAEnabled()) {
519 CLRBIT(PORTD,HiBMask);
520 CLRBIT(PORTD,HiAMask);
521 SETBIT(PORTD,HiAMask);
522 } else if (PWM1::IsChBEnabled()) {
523 CLRBIT(PORTD,HiAMask);
524 CLRBIT(PORTD,HiBMask);
525 SETBIT(PORTD,HiBMask);
526 }
527 }
528 inline void FastFieldCollapseLowSide() {
529 if (PWM1::IsChAEnabled()) {
530 CLRBIT(PORTD,HiAMask);
531 CLRBIT(PORTD,HiBMask);
532 SHOOT_THROUGH_DELAY;
533 CLRBIT(PORTB,LoAMask);
534 PWM1::DisableChAB();
535 SETBIT(PORTB,LoBMask);
536 } else if (PWM1::IsChBEnabled()) {
537 CLRBIT(PORTD,HiAMask);
538 CLRBIT(PORTD,HiBMask);
539 SHOOT_THROUGH_DELAY;
540 CLRBIT(PORTB,LoBMask);
541 PWM1::DisableChAB();
542 SETBIT(PORTB,LoAMask);
543 }
544 }
545 inline void SlowFieldCollapseHighSide() {
546 }
547 inline void SlowFieldCollapseLowSide() {
548 if (PWM1::IsChAEnabled()) {
549 CLRBIT(PORTD,HiAMask);
550 CLRBIT(PORTD,HiBMask);
551 SHOOT_THROUGH_DELAY;
552 CLRBIT(PORTB,LoBMask);
553 PWM1::DisableChAB();
554 SETBIT(PORTB,LoAMask);
555 } else if (PWM1::IsChBEnabled()) {
556 CLRBIT(PORTD,HiAMask);
557 CLRBIT(PORTD,HiBMask);
558 SHOOT_THROUGH_DELAY;
559 CLRBIT(PORTB,LoAMask);
560 PWM1::DisableChAB();
561 SETBIT(PORTB,LoBMask);
562 }
563 }
564 inline void ResetAfterFastCollapse() {
565 if (TESTBIT(PORTD,HiAMask) != 0) {
566 CLRBIT(PORTD,HiAMask);
567 SETBIT(PORTD,HiBMask);
568 } else if (TESTBIT(PORTD,HiBMask) != 0) {
569 CLRBIT(PORTD,HiBMask);
570 SETBIT(PORTD,HiAMask);
571 } else if (TESTBIT(PORTB,LoAMask) != 0) {
572 CLRBIT(PORTB,LoAMask);
573 SETBIT(PORTB,LoBMask);
574 } else if (TESTBIT(PORTB,LoBMask) != 0) {
575 CLRBIT(PORTB,LoBMask);
576 SETBIT(PORTB,LoAMask);
577 }
578 }
579
580  
581 switch (DataRecord.CollapseState & CollapseStateMask) {
582 case FastCollapseHighSide:
583 FastFieldCollapseHighSide();
584 break;
585 case FastCollapseLowSide:
586 FastFieldCollapseLowSide();
587 break;
588 default:
589 case SlowCollapseHighSide:
590 SlowFieldCollapseHighSide();
591 break;
592 case SlowCollapseLowSide:
593 SlowFieldCollapseLowSide();
594 break;
595 }
596 }
597
598  
599 switch (DataRecord.CollapseState & CollapseStateMask) {
600 case FastCollapseHighSide:
601 if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {
602 DataRecord.CollapseState = CollapseStateAutoCycle | FastCollapseLowSide;
603 }
604 break;
605 case FastCollapseLowSide:
606 if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {
607 DataRecord.CollapseState = CollapseStateAutoCycle | FastCollapseHighSide;
608 }
609 break;
610 default:
611 case SlowCollapseHighSide:
612 if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {
613 DataRecord.CollapseState = CollapseStateAutoCycle | SlowCollapseLowSide;
614 } else {
615 DataRecord.CollapseState = SlowCollapseHighSide;
616 }
617 break;
618 case SlowCollapseLowSide:
619 if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {
620 DataRecord.CollapseState = CollapseStateAutoCycle | SlowCollapseHighSide;
621 }
622 break;
623 }
624 }
625 inline void ResetHighSide() {
626 if (TESTBIT(PORTD,HiBMask)) {
627 CLRBIT(PORTD,HiBMask);
628 SETBIT(PORTD,HiBMask);
629 }
630 if (TESTBIT(PORTD,HiAMask)) {
631 CLRBIT(PORTD,HiAMask);
632 SETBIT(PORTD,HiAMask);
633 }
634 }
635 void FreeWheel() {
636 // Disable everything
637 CLRBIT(PORTB,LoBMask);
638 CLRBIT(PORTB,LoAMask);
639 CLRBIT(PORTD,HiBMask);
640 CLRBIT(PORTD,HiAMask);
641 PWM1::DisableChAB();
642 // Set up interrupts to some reasonable values
643 if (!DataRecord.IsForward) {
644 PWM1::SetChannelA(0x10);
645 PWM1::SetChannelB(ControlTime);
646 } else {
647 PWM1::SetChannelB(0x10);
648 PWM1::SetChannelA(ControlTime);
649 }
650 }
651 void Brake() {
652 // Allways clear first, than set
653 CLRBIT(PORTB,LoBMask);
654 CLRBIT(PORTB,LoAMask);
655 PWM1::DisableChAB();
656 PWM1::SetChannelB(0x10); // Set it to some reasonable value
657 PWM1::SetChannelA(ControlTime);
658 DataRecord.IsForward = true;
659 SETBIT(PORTD,HiAMask);
660 SETBIT(PORTD,HiBMask);
661 }
662 void HandleOverload() {
663 // Turn off both low-side FETs - this will remove the load for the rest of the cycle
664 CLRBIT(PORTB,LoBMask);
665 CLRBIT(PORTB,LoAMask);
666 PWM1::DisableChAB();
667 }
668
669  
670  
671 return (((int32_t)aA * (int32_t)aB) >> 8);
672 }
673
674  
675 // Control acceleration
676 // Note: DoControl will not be called if RequestValue is Freewheel or Braking
677 int16_t SpeedDiff = DataRecord.RequestValue - DataRecord.CurrentRequest;
678 if (SpeedDiff > DataRecord.MaxPositiveChange) {
679 DataRecord.CurrentRequest += DataRecord.MaxPositiveChange;
680 } else if (SpeedDiff < -DataRecord.MaxNegativeChange) {
681 DataRecord.CurrentRequest -= DataRecord.MaxNegativeChange;
682 } else {
683 DataRecord.CurrentRequest = DataRecord.RequestValue;
684 }
685
686  
687 // so stop will be smooth. Since we estimage the time required to stop this also implements
688 // the go-to-distance functionality.
689 int16_t Change = (DataRecord.OriginalRequestValue > 0)?DataRecord.MaxNegativeChange:-DataRecord.MaxPositiveChange;
690 DataRecord.DistanceToStop = ((int32_t)DataRecord.VoltageSample * (int32_t)DataRecord.VoltageSample / (int32_t)Change) << 3;
691 int32_t StopPosition = DataRecord.Distance + DataRecord.DistanceToStop;
692 if (DataRecord.OriginalRequestValue > 0) {
693 if (StopPosition > DataRecord.FwDistanceLimit) {
694 DataRecord.RequestValue = DataRecord.CurrentRequest - min(DataRecord.MaxNegativeChange,DataRecord.CurrentRequest);
695 }
696 } else {
697 if (StopPosition < DataRecord.BwDistanceLimit) {
698 DataRecord.RequestValue = DataRecord.CurrentRequest + min(DataRecord.MaxPositiveChange,-DataRecord.CurrentRequest);
699 }
700 }
701
702  
703 int16_t ScaledRequest = DataRecord.CurrentRequest >> 4;
704 DataRecord.LastError = DataRecord.Error;
705 DataRecord.Error = DataRecord.VoltageSample - ScaledRequest;
706 int16_t DValue = DataRecord.Error - DataRecord.LastError;
707 DataRecord.Command = ScaledMult(ScaledRequest,DataRecord.PFFactor) + ScaledMult(DataRecord.IValue,DataRecord.IFactor) + ScaledMult(DataRecord.Error,DataRecord.PFactor) + ScaledMult(DValue,DataRecord.DFactor);
708 // Limit command to valid range and limit IValue growth as well
709 if (DataRecord.Command >= 0x100) {
710 DataRecord.Command = 0xffL;
711 // In an overflow case allow integrator value updates if it works against the overflow (sign bits are differenet)
712 if (((DataRecord.IValue ^ DataRecord.Error) & 0x8000) != 0) DataRecord.IValue += DataRecord.Error;
713 } else if (DataRecord.Command <= -0x100) {
714 DataRecord.Command = -0xffL;
715 // In an overflow case allow integrator value updates if it works against the overflow (sign bits are differenet)
716 if (((DataRecord.IValue ^ DataRecord.Error) & 0x8000) != 0) DataRecord.IValue += DataRecord.Error;
717 } else {
718 // Use saturated arithmetics to avoid roll-over in the accumulator
719 int32_t TempIValue = (int32_t)DataRecord.IValue + (int32_t)DataRecord.Error;
720 if (TempIValue > 0x7fff) {
721 DataRecord.IValue = 0x7fffL;
722 } else if (TempIValue < -0x7fffL) {
723 DataRecord.IValue = -0x7fff;
724 } else {
725 DataRecord.IValue = TempIValue;
726 }
727 }
728 if (DataRecord.Command > 0) Forward(DataRecord.Command); else Backward((-DataRecord.Command));
729
730  
731 }
732
733  
734
735  
736 uint8_t OnTime = (DataRecord.IsForward)?PWM1::GetChannelB():PWM1::GetChannelA();
737 return (OnTime > 128);
738 }
739
740  
741 void SetADChannelMotorPositive() {
742 if (DataRecord.IsForward) {
743 ADConv::SetChannel(ADConv::Ch_MotorA,ADConv::Ref_MotorA,ADConv::RightAdjust);
744 } else {
745 ADConv::SetChannel(ADConv::Ch_MotorB,ADConv::Ref_MotorB,ADConv::RightAdjust);
746 }
747 }
748
749  
750 void SetADChannelMotorNegative() {
751 if (DataRecord.IsForward) {
752 ADConv::SetChannel(ADConv::Ch_MotorB,ADConv::Ref_MotorB,ADConv::RightAdjust);
753 } else {
754 ADConv::SetChannel(ADConv::Ch_MotorA,ADConv::Ref_MotorA,ADConv::RightAdjust);
755 }
756 }
757
758  
759 // Start new measurements
760 switch (DataRecord.CollapseState & CollapseStateMask) {
761 case FastCollapseLowSide:
762 SetADChannelMotorNegative();
763 if (aWasChAOrBEnabled) {
764 DataRecord.SampleState = SampleState_PreFastCollapse;
765 } else {
766 DataRecord.SampleState = SampleState_PostFastCollapse;
767 }
768 break;
769 case FastCollapseHighSide:
770 SetADChannelMotorPositive();
771 if (aWasChAOrBEnabled) {
772 DataRecord.SampleState = SampleState_PreFastCollapse;
773 } else {
774 DataRecord.SampleState = SampleState_PostFastCollapse;
775 }
776 break;
777 case SlowCollapseLowSide:
778 SetADChannelMotorNegative();
779 DataRecord.SampleState = SampleState_PreSampleBase;
780 break;
781 case SlowCollapseHighSide:
782 SetADChannelMotorPositive();
783 DataRecord.SampleState = SampleState_PreSampleBase;
784 break;
785 }
786 }
787
788  
789 switch (DataRecord.CollapseState & CollapseStateMask) {
790 case FastCollapseLowSide:
791 ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);
792 DataRecord.SampleState = SampleState_PreCurrentSample;
793 break;
794 case FastCollapseHighSide:
795 if (BatSampleWhileOn()) {
796 SetADChannelMotorPositive();
797 DataRecord.SampleState = SampleState_PreSampleBat;
798 SampleStateCnt = 0;
799 } else {
800 ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);
801 DataRecord.SampleState = SampleState_PreCurrentSample;
802 }
803 break;
804 case SlowCollapseLowSide:
805 ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);
806 DataRecord.SampleState = SampleState_PreCurrentSample;
807 break;
808 case SlowCollapseHighSide:
809 ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);
810 DataRecord.SampleState = SampleState_PreCurrentSample;
811 break;
812 }
813 bool InvertSample = !DataRecord.IsForward;
814 switch (DataRecord.OperatingMode) {
815 default:
816 case OperatingMode_Speed: {
817 int16_t LocalBaseValue = DataRecord.BaseValue;
818 switch (DataRecord.CollapseState & CollapseStateMask) {
819 case FastCollapseHighSide:
820 case SlowCollapseHighSide:
821 break;
822 case FastCollapseLowSide:
823 case SlowCollapseLowSide:
824 InvertSample = !InvertSample;
825 LocalBaseValue = 0; // These are ground-based measurements...
826 break;
827 }
828 if (!InvertSample) {
829 DataRecord.VoltageSample = LocalBaseValue - DataRecord.MinValue;
830 } else {
831 DataRecord.VoltageSample = DataRecord.MinValue - LocalBaseValue;
832 }
833 }
834 break;
835 case OperatingMode_Servo:
836 DataRecord.VoltageSample = DataRecord.MaxValue;
837 break;
838 }
839
840  
841 DataRecord.SampleCnt_Snapshot = DataRecord.SampleCnt;
842 DataRecord.MinValue_Snapshot = DataRecord.MinValue;
843 }
844
845  
846 const uint8_t BlinkInterval = 50;
847
848  
849 if (DataRecord.IsForward == IsIRQA) {
850 // Almost at the end: grab back-EMF data from sampling,
851 // do the control, and start sampling for current
852 StartOnPhase();
853
854  
855 case RequestFreewheel:
856 FreeWheel();
857 DataRecord.CurrentRequest = 0;
858 break;
859 case RequestBrake:
860 Brake();
861 DataRecord.CurrentRequest = 0;
862 break;
863 default:
864 // Update the travalled distance:
865 DataRecord.Distance += DataRecord.VoltageSample;
866 DoControl();
867 break;
868 }
869 SwitchCollapseType();
870 // Toggle user LED
871 ++BlinkCnt;
872 if (BlinkCnt > BlinkInterval) {
873 BlinkCnt = 0;
874 OpLed::Toggle();
875 }
876 } else {
877 // At the end of the on-part: reverse voltage across motor to fast-discharge it.
878 bool WasChAOrBEnabled = PWM1::IsChAOrBEnabled();
879 CollapseField();
880
881  
882
883  
884 DataRecord.ADBufferEnable = 1;
885 }
886 if (DataRecord.ADBufferEnableHost) {
887 DataRecord.ADBufferEnableHost = false;
888 DataRecord.ADBufferEnable = 0;
889 }
890
891  
892 StartOffPhase(WasChAOrBEnabled);
893 ResetHighSide();
894 }
895 }
896
897  
898 const uint8_t CollapseSearchBlank = 1;
899 const uint16_t CollapseSearchLowLimit = 0x30;
900 const uint16_t CollapseSearchHighLimit = 0x90;
901
902  
903 int16_t CurData = ADConv::FastGetSample();
904
905  
906 if (DataRecord.ADBufferEnable == 0) {
907 if (ADSampleIdx < sizeof(DataRecord.ADBuffer)/sizeof(DataRecord.ADBuffer[0])) {
908 int16_t ADSampleData = (int16_t)ADConv::GetChannel() << 10;
909 ADSampleData |= CurData;
910 DataRecord.ADBuffer[ADSampleIdx] = ADSampleData;
911 ++ADSampleIdx;
912 if (ADSampleIdx < sizeof(DataRecord.ADBuffer)/sizeof(DataRecord.ADBuffer[0])) {
913 DataRecord.ADBuffer[ADSampleIdx] = 0;
914 }
915 TOGGLEBIT(PORTD,0x10);
916 }
917 }
918
919  
920 switch (DataRecord.SampleState) {
921 case SampleState_PreFastCollapse:
922 DataRecord.SampleState = SampleState_FastCollapse;
923 // Update max current from the search field
924 DataRecord.CurrentMax = DataRecord.CurrentMaxSearch;
925 // TODO: If DataRecord.CurrentTemp isn't DataRecord.CurrentMaxSearch (more or less)
926 // than the over-current protection must have been activated.
927 SampleStateCnt = 0;
928 break;
929 case SampleState_FastCollapse:
930 // Wait in this state until turn-off transient is over
931 switch (DataRecord.CollapseState & CollapseStateMask) {
932 case FastCollapseLowSide:
933 if (CurData < CollapseSearchLowLimit && SampleStateCnt > CollapseSearchBlank) {
934 ResetAfterFastCollapse();
935 DataRecord.SampleState = SampleState_PostFastCollapse;
936 } else if (CurData > CollapseSearchHighLimit) {
937 SampleStateCnt = CollapseSearchBlank;
938 }
939 ++SampleStateCnt;
940 break;
941 case FastCollapseHighSide:
942 if (CurData > CollapseSearchHighLimit && SampleStateCnt > CollapseSearchBlank) {
943 ResetAfterFastCollapse();
944 DataRecord.SampleState = SampleState_PostFastCollapse;
945 } else if (CurData < CollapseSearchLowLimit) {
946 SampleStateCnt = CollapseSearchBlank;
947 }
948 ++SampleStateCnt;
949 break;
950 }
951 break;
952 case SampleState_PostFastCollapse:
953 switch (DataRecord.OperatingMode) {
954 default:
955 case OperatingMode_Speed:
956 switch (DataRecord.CollapseState & CollapseStateMask) {
957 case FastCollapseLowSide:
958 SetADChannelMotorPositive();
959 DataRecord.BaseValue = CurData;
960 DataRecord.SampleState = SampleState_PreSearchMax;
961 break;
962 case FastCollapseHighSide:
963 if (BatSampleWhileOn()) {
964 SetADChannelMotorNegative();
965 DataRecord.SampleState = SampleState_PreSearchMax;
966 } else {
967 SetADChannelMotorPositive();
968 DataRecord.SampleState = SampleState_PreSampleBat;
969 SampleStateCnt = 0;
970 }
971 break;
972 }
973 break;
974 case OperatingMode_Servo:
975 ADConv::SetChannel(ADConv::Ch_ServoPot,ADConv::Ref_ServoPot,ADConv::RightAdjust);
976 DataRecord.SampleState = SampleState_PreSamplePot;
977 break;
978 }
979 break;
980 case SampleState_PreSampleBase:
981 // Throw away the data, but the next one is for real!
982 DataRecord.SampleState = SampleState_PreSampleBase2;
983 switch (DataRecord.CollapseState & CollapseStateMask) {
984 case SlowCollapseLowSide:
985 SetADChannelMotorNegative();
986 break;
987 case SlowCollapseHighSide:
988 SetADChannelMotorPositive();
989 break;
990 }
991 // Update max current from the search field
992 DataRecord.CurrentMax = DataRecord.CurrentMaxSearch;
993 // TODO: If DataRecord.CurrentTemp isn't DataRecord.CurrentMaxSearch (more or less)
994 // than the over-current protection must have been activated.
995 SampleStateCnt = 0;
996 break;
997 case SampleState_PreSampleBase2:
998 // throw away this data, but the next one is for real!
999 DataRecord.SampleState = SampleState_SampleBase;
1000 break;
1001 case SampleState_SampleBase:
1002 switch (DataRecord.OperatingMode) {
1003 default:
1004 case OperatingMode_Speed:
1005 switch (DataRecord.CollapseState & CollapseStateMask) {
1006 case SlowCollapseLowSide:
1007 SetADChannelMotorPositive();
1008 DataRecord.BaseValue = CurData;
1009 DataRecord.SampleState = SampleState_PreSearchMax;
1010 break;
1011 case SlowCollapseHighSide:
1012 SetADChannelMotorNegative();
1013 DataRecord.BaseValue = CurData;
1014 DataRecord.SampleState = SampleState_PreSearchMax;
1015 break;
1016 }
1017 break;
1018 case OperatingMode_Servo:
1019 ADConv::SetChannel(ADConv::Ch_ServoPot,ADConv::Ref_ServoPot,ADConv::RightAdjust);
1020 DataRecord.SampleState = SampleState_PreSamplePot;
1021 break;
1022 }
1023 break;
1024 case SampleState_PreSamplePot:
1025 DataRecord.SampleState = SampleState_SamplePot;
1026 break;
1027 case SampleState_SamplePot:
1028 DataRecord.MaxValue = CurData;
1029 DataRecord.SampleCnt = 1;
1030 break;
1031 case SampleState_PreSearchMax:
1032 DataRecord.SampleState = SampleState_SearchMax;
1033 DataRecord.MaxValue = 0x0000;
1034 DataRecord.SampleCnt = 0;
1035 break;
1036 case SampleState_SearchMax:
1037 switch (DataRecord.CollapseState & CollapseStateMask) {
1038 case FastCollapseHighSide:
1039 case SlowCollapseLowSide:
1040 if (CurData <= DataRecord.MaxValue) {
1041 DataRecord.MaxValue = CurData;
1042 } else {
1043 DataRecord.SampleState = SampleState_SearchMin;
1044 DataRecord.MinValue = DataRecord.MaxValue;
1045 }
1046 break;
1047 case FastCollapseLowSide:
1048 case SlowCollapseHighSide:
1049 if (CurData >= DataRecord.MaxValue) {
1050 DataRecord.MaxValue = CurData;
1051 } else {
1052 DataRecord.SampleState = SampleState_SearchMin;
1053 DataRecord.MinValue = DataRecord.MaxValue;
1054 }
1055 break;
1056 }
1057 break;
1058 case SampleState_SearchMin:
1059 switch (DataRecord.CollapseState & CollapseStateMask) {
1060 case FastCollapseHighSide:
1061 case SlowCollapseLowSide:
1062 if (CurData > DataRecord.MinValue) {
1063 DataRecord.MinValue = CurData;
1064 }
1065 DataRecord.SampleCnt++;
1066 break;
1067 case FastCollapseLowSide:
1068 case SlowCollapseHighSide:
1069 if (CurData < DataRecord.MinValue) {
1070 DataRecord.MinValue = CurData;
1071 }
1072 DataRecord.SampleCnt++;
1073 break;
1074 }
1075 break;
1076 case SampleState_PreSampleBat:
1077 if (SampleStateCnt > 4)
1078 DataRecord.SampleState = SampleState_SampleBat;
1079 ++SampleStateCnt;
1080 break;
1081 case SampleState_SampleBat:
1082 DataRecord.BaseValue = CurData;
1083 if (BatSampleWhileOn()) {
1084 ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);
1085 DataRecord.SampleState = SampleState_PreCurrentSample;
1086 } else {
1087 SetADChannelMotorNegative();
1088 DataRecord.SampleState = SampleState_PreSearchMax;
1089 }
1090 break;
1091 case SampleState_PreCurrentSample:
1092 ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);
1093 CurrentSampleCnt = 0;
1094 DataRecord.SampleState = SampleState_CurrentSample1;
1095 break;
1096 case SampleState_CurrentSample1:
1097 if (CurData > DataRecord.CurrentLimit) HandleOverload();
1098 if (CurrentSampleCnt == 0) {
1099 DataRecord.CurrentTemp = CurData;
1100 DataRecord.CurrentMaxSearch = CurData;
1101 }
1102 ++CurrentSampleCnt;
1103 if (CurData > DataRecord.CurrentMaxSearch) DataRecord.CurrentMaxSearch = CurData;
1104 if (CurrentSampleCnt == 3) DataRecord.SampleState = SampleState_CurrentSample2;
1105 break;
1106 case SampleState_CurrentSample2:
1107 if (CurData > DataRecord.CurrentLimit) HandleOverload();
1108 // Sample delta-current. It is related to the back-EMF voltage, though
1109 // this measurement is not precise enough to base control off of it
1110 DataRecord.CurrentDelta = CurData - DataRecord.CurrentTemp;
1111 DataRecord.CurrentTemp = CurData;
1112 if (CurData > DataRecord.CurrentMaxSearch) DataRecord.CurrentMaxSearch = CurData;
1113 DataRecord.SampleState = SampleState_CurrentSample3;
1114 break;
1115 case SampleState_CurrentSample3:
1116 // We'll stay in this state until the on-phase ends...
1117 if (CurData > DataRecord.CurrentLimit) HandleOverload();
1118 DataRecord.CurrentTemp = CurData;
1119 if (CurData > DataRecord.CurrentMaxSearch) DataRecord.CurrentMaxSearch = CurData;
1120 break;
1121 }
1122 }
1123 }
1124
1125  
1126 SIGNAL(SIG_OUTPUT_COMPARE1B) {HBridge::HandleIRQ(false);}
1127 SIGNAL(SIG_ADC) {HBridge::Sample();}
1128
1129  
1130 uint8_t Address;
1131
1132  
1133 DevClassHBridge,
1134 #if defined MEGA_BRIDGE
1135 DevUmHBridge
1136 #elif defined H_BRIDGE
1137 DevUmHBridge
1138 #elif defined SERVO_BRAIN
1139 DevUmServoBrain
1140 #else
1141 #error No HW version is specified!
1142 #endif
1143 };
1144
1145  
1146 US_ReceiveAddr = TWI::US_Base,
1147 US_ReceiveData,
1148 US_SendData
1149 };
1150
1151  
1152 State = US_ReceiveAddr;
1153 }
1154
1155  
1156 uint8_t UInt8[4];
1157 uint16_t UInt16[2];
1158 uint32_t UInt32[1];
1159 } TransmitBuffer;
1160
1161  
1162
1163  
1164 switch (HBridge::GetDataElementSize(Address)) {
1165 default:
1166 case 1:
1167 TWIPrevData = *HBridge::GetDataRecord8(Address);
1168 break;
1169 case 2:
1170 if ((Address & 1) == 0) {
1171 TransmitBuffer.UInt16[0] = *HBridge::GetDataRecord16(Address);
1172 }
1173 TWIPrevData = TransmitBuffer.UInt8[Address & 1];
1174 break;
1175 case 4:
1176 if ((Address & 3) == 0) {
1177 TransmitBuffer.UInt32[0] = *HBridge::GetDataRecord32(Address);
1178 }
1179 TWIPrevData = TransmitBuffer.UInt8[Address & 3];
1180 break;
1181 }
1182 TWDR = TWIPrevData;
1183 ++Address;
1184 if (Address >= HBridge::GetDataRecordSize()) Address = 0; // Wrap around
1185 }
1186
1187  
1188 switch (HBridge::GetDataElementSize(Address)) {
1189 default:
1190 case 1:
1191 TransmitBuffer.UInt8[0] = TWIData;
1192 *HBridge::GetDataRecord8(Address) = TransmitBuffer.UInt8[0];
1193 break;
1194 case 2:
1195 TransmitBuffer.UInt8[Address & 1] = TWIData;
1196 if ((Address & 1) != 0) {
1197 // High-byte: store the whole word
1198 *HBridge::GetDataRecord16(Address & (~0x01)) = TransmitBuffer.UInt16[0];
1199 // Special-case request value, we have to save that in another spot as well...
1200 if (Address == 1) {
1201 HBridge::DataRecord.OriginalRequestValue = TransmitBuffer.UInt16[0];
1202 }
1203 }
1204 break;
1205 case 4:
1206 TransmitBuffer.UInt8[Address & 3] = TWIData;
1207 if ((Address & 3) != 0) {
1208 // Highest byte: store the whole dword
1209 *HBridge::GetDataRecord32(Address & (~0x03)) = TransmitBuffer.UInt32[0];
1210 }
1211 break;
1212 }
1213 ++Address;
1214 if (Address >= HBridge::GetDataRecordSize()) Address = 0; // Wrap around
1215 }
1216
1217  
1218 SendData();
1219 State = US_SendData;
1220 }
1221
1222  
1223 switch (State) {
1224 case US_ReceiveAddr:
1225 switch (TWIStatus) {
1226 case TW_SR_DATA_ACK:
1227 // TODO: handle different command codes here -> check the data written to this address!
1228 if (TWIData == 0xff) {
1229 HBridge::SaveSettings();
1230 ResetTWI();
1231 } else {
1232 //if ((TWIData & 0x7f) >= HBridge::GetDataRecordSize()) Address = HBridge::GetDataRecordSize() - 1; else Address = TWIData;
1233 if (TWIData >= HBridge::GetDataRecordSize()) Address = HBridge::GetDataRecordSize() - 1; else Address = TWIData;
1234 State = US_ReceiveData;
1235 }
1236 break;
1237 default:
1238 ResetTWI();
1239 break;
1240 }
1241 break;
1242 case US_ReceiveData:
1243 switch (TWIStatus) {
1244 case TW_SR_DATA_ACK:
1245 GetData();
1246 // We stay in this state for any optional additional data
1247 break;
1248 default:
1249 ResetTWI();
1250 break;
1251 }
1252 break;
1253 case US_SendData:
1254 switch (TWIStatus) {
1255 case TW_ST_DATA_ACK:
1256 SendData();
1257 SETBIT(TWIControl,(1 << TWEA)); // require ACK
1258 break;
1259 default:
1260 ResetTWI();
1261 break;
1262 }
1263 break;
1264 default:
1265 // This really REALLY shouldn't happen...
1266 ResetTWI();
1267 break;
1268 }
1269 }
1270 }
1271
1272  
1273 // Debug interfaces
1274 namespace UsartComm {
1275 inline void Init(uint16_t aBaudSetting) {
1276 USART0::Init(aBaudSetting);
1277 }
1278 void HandleInput() {
1279 uint8_t CurData = USART0::FastReceiveData();
1280 OpLed::Toggle();
1281 }
1282 }
1283
1284  
1285
1286  
1287 cli();
1288 int16_t VoltageSample = HBridge::DataRecord.VoltageSample;
1289 uint16_t SampleCnt = HBridge::DataRecord.SampleCnt_Snapshot;
1290 int16_t MinValue = HBridge::DataRecord.MinValue_Snapshot;
1291 int16_t CurrentRequest = HBridge::DataRecord.CurrentRequest;
1292 int16_t Error = HBridge::DataRecord.Error;
1293 int16_t IValue = HBridge::DataRecord.IValue;
1294 int16_t Command = HBridge::DataRecord.Command;
1295 sei();
1296
1297  
1298 USART0::SendData(' '); USART0::SendData('S'); USART0::SendData(':'); USART0::SendHexData(SampleCnt);
1299 USART0::SendData(' '); USART0::SendData('M'); USART0::SendData(':'); USART0::SendHexData(MinValue);
1300 USART0::SendData(' '); USART0::SendData('R'); USART0::SendData(':'); USART0::SendHexData(CurrentRequest);
1301 USART0::SendData(' '); USART0::SendData('E'); USART0::SendData(':'); USART0::SendHexData(Error);
1302 USART0::SendData(' '); USART0::SendData('I'); USART0::SendData(':'); USART0::SendHexData(IValue);
1303 USART0::SendData(' '); USART0::SendData('C'); USART0::SendData(':'); USART0::SendHexData(Command);
1304 USART0::SendData('\r'); USART0::SendData('\n');
1305 }
1306 #endif // USE_SERIAL_DEBUG
1307
1308  
1309 cli();
1310 EEPROM::Init();
1311 OpLed::Init();
1312 HBridge::BlinkCnt = 0;
1313 #ifdef USE_SERIAL_DEBUG
1314 UsartComm::Init(USART0::baud57600_8MHz);
1315 #endif // USE_SERIAL_DEBUG
1316 TCCR0A = 0;
1317 TWI::Init();
1318 ADConv::Init();
1319 HBridge::Init();
1320 SETBIT(DDRD,0x08|0x10);
1321 sei();
1322
1323  
1324 // here but some optional debugging
1325 while (true) {
1326 #ifdef USE_SERIAL_DEBUG
1327 cli();
1328 if (HBridge::DataRecord.NewData) {
1329 HBridge::DataRecord.NewData = false;
1330 DebugStat();
1331 } else {
1332 sei();
1333 }
1334 #endif // USE_SERIAL_DEBUG
1335 }
1336 return 0;
1337 }
1338