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