// Features:// - Complete PID loop// - Optional proportional feed-forward for open-loop control and wind-up minimization// - Wind-up limiting under overload conditions// - Torque throttling under overload// - TWI interface with PnP support// - Parameter storage in EEPROM including TWI address// - Acceleration, deceleration limits// - Speed integration to estimate travelled distance// - Add 'go to distance' mode with trapezoid speed-profiles - Needs testing// - Added 32-bit and 16-bit atomic reads and writes. Note: 8-bit atomic writes are NOT supported// - Added support for duty cycle throttling (basically torque-throttling)// - Added current reading and max current detection// - Added per cycle over-current detection and early-termination of cycle// - Added servo-type operation with pot-based position feedback// TODO:// - Add current integration for power usage estimation// - Add serial interface// - Add servo-type (PWM) interface// - Add optical encoder support// - When sampling for fast-collapse high-side and during the on-state: we only need two states - the high-side should be on already// - Switching between high- and low-collapse modes to equalize catch-diode load - make this user-selectable// - Detect continous-current mode and do something about it!// TODO TEST:// - Servo mode in all four modes// - Freewheeling and back-EMF in all four modes// - Braking// BUGS:// - back-EMF measurment is different in fast-collapse and low-collapse modes. This make control leading to different speeds in the two modes.#define BOARD_umHBridge#define H_BRIDGE#include <avr/pgmspace.h>#include <avr/interrupt.h> #include <avr/sleep.h>#include "common.h"#include "opled.h"#include "usart.h"#include "twi_aap.h"#include "eeprom.h"void DebugStat();// This is an 8-bit PWMnamespace PWM1 {enum eClkSrc {ClkNone = 0,Clk = (1 << CS10),ClkDiv1 = (1 << CS10),ClkDiv8 = (1 << CS11),ClkDiv64 = (1 << CS11) | (1 << CS10),ClkDiv256 = (1 << CS12),ClkDiv1024 = (1 << CS12) | (1 << CS10),ClkExtFall = (1 << CS12) | (1 << CS11),ClkExtRise = (1 << CS12) | (1 << CS11) | (1 << CS10)};void inline Init(eClkSrc aClkSrc) {SETBIT(DDRB,0x02|0x04); // Preprare PortB to output to handle disablesCLRBIT(PORTB,0x02|0x04); // Set port bits to 0 so that disable will work correctlyCLRBIT(PRR,PRTIM0);TCCR1A = (1 << WGM10);TCCR1B = aClkSrc | (1 << WGM12);OCR1A = 0x0000;OCR1B = 0x0000;ICR1 = 0x0000;TCNT1 = 0x0000;}void inline DisableChA() { CLRBIT(TCCR1A,(1 << COM1A1)); }void inline DisableChB() { CLRBIT(TCCR1A,(1 << COM1B1)); }void inline DisableChAB() { CLRBIT(TCCR1A,(1 << COM1A1) | (1 << COM1B1)); }void inline EnableChA() { SETBIT(TCCR1A,(1 << COM1A1)); }void inline EnableChB() { SETBIT(TCCR1A,(1 << COM1B1)); }void inline EnableChAB() { SETBIT(TCCR1A,(1 << COM1A1) | (1 << COM1B1)); }bool inline IsChAEnabled() { return TESTBIT(TCCR1A,(1 << COM1A1)) != 0; }bool inline IsChBEnabled() { return TESTBIT(TCCR1A,(1 << COM1B1)) != 0; }bool inline IsChAOrBEnabled() { return TESTBIT(TCCR1A,(1 << COM1A1) | (1 << COM1B1)) != 0; }// We never set the high bits to anything but 0, so the high-byte in the TEMP register doesn't need to be set.void inline SetChannelA(uint8_t aValue) { OCR1AL = aValue; }uint8_t inline GetChannelA() { return OCR1AL; }void inline SetChannelB(uint8_t aValue) { OCR1BL = aValue; }uint8_t inline GetChannelB() { return OCR1BL; }void inline EnableIRQ_A() { SETBIT(TIMSK1,(1 << OCIE1A)); }void inline EnableIRQ_B() { SETBIT(TIMSK1,(1 << OCIE1B)); }void inline EnableIRQ_AB() { SETBIT(TIMSK1,(1 << OCIE1A) | (1 << OCIE1B)); }void inline EnableIRQ_Overflow() { SETBIT(TIMSK1,(1 << TOIE1)); }void inline EnableOnlyIRQ_A() { TIMSK1 = (1 << OCIE1A); }void inline EnableOnlyIRQ_B() { TIMSK1 = (1 << OCIE1B); }void inline EnableOnlyIRQ_Overflow() { TIMSK1 = (1 << TOIE1); }void inline DisableIRQ_A() { CLRBIT(TIMSK1,(1 << OCIE1A)); }void inline DisableIRQ_B() { CLRBIT(TIMSK1,(1 << OCIE1B)); }void inline DisableIRQ_Overflow() { CLRBIT(TIMSK1,(1 << TOIE1)); }void inline DisableIRQ_All() { TIMSK1 = 0; }bool inline IsPendingIRQ_A() { return (TESTBIT(TIFR1,(1 << OCF1A)) != 0); }bool inline IsPendingIRQ_B() { return (TESTBIT(TIFR1,(1 << OCF1B)) != 0); }bool inline IsPendingIRQ_Overflow() { return (TESTBIT(TIFR1,(1 << TOV1)) != 0); }void inline ClearPendingIRQ_A() { TIFR1 = (1 << OCF1A); }void inline ClearPendingIRQ_B() { TIFR1 = (1 << OCF1B); }void inline ClearPendingIRQ_Overflow() { TIFR1 = (1 << TOV1); }void inline ClearPendingIRQ_All() { TIFR1 = (1 << OCF1A) | (1 << OCF1B) | (1 << TOV1); }}namespace ADConv {enum eRef {RefInt = (1 << REFS0) | (1 << REFS1),RefVcc = (1 << REFS0),RefExt = 0};enum eAdjust {LeftAdjust = ADLAR,RightAdjust = 0};#if defined MEGA_BRIDGEconst uint8_t Ch_MotorA = 7;const uint8_t Ch_MotorB = 2;const uint8_t Ch_MotorCurrent = 6;const uint8_t Ch_ServoPot = 3;const uint8_t Ch_Battery = 1;const eRef Ref_MotorA = RefVcc;const eRef Ref_MotorB = RefVcc;const eRef Ref_MotorCurrent = RefInt;const eRef Ref_ServoPot = RefVcc;const eRef Ref_Battery = RefVcc;#elif defined H_BRIDGEconst uint8_t Ch_MotorA = 7;const uint8_t Ch_MotorB = 2;const uint8_t Ch_MotorCurrent = 6;const uint8_t Ch_ServoPot = 3;const eRef Ref_MotorA = RefInt;const eRef Ref_MotorB = RefInt;const eRef Ref_MotorCurrent = RefInt;const eRef Ref_ServoPot = RefVcc;const eRef Ref_Battery = RefVcc;#elif defined SERVO_BRAINconst uint8_t Ch_MotorA = 0;const uint8_t Ch_MotorB = 2;const uint8_t Ch_MotorCurrent = 6;const uint8_t Ch_ServoPot = 3;const eRef Ref_MotorA = RefVcc;const eRef Ref_MotorB = RefVcc;const eRef Ref_MotorCurrent = RefVcc;const eRef Ref_ServoPot = RefVcc;const eRef Ref_Battery = RefVcc;#else#error HW version is not specified#endifconst uint8_t ADCSRA_BaseValue = (1 << ADEN) | (1 << ADATE) | (1 << ADPS2) | (1 << ADPS1);static inline void Init() {ADMUX = (1<< REFS0) | 15; // AVCC is the default referenceADCSRA = ADCSRA_BaseValue;ADCSRB = 0; // Free-running modeDIDR0 = (1 << Ch_MotorA) | (1 << Ch_MotorB) | (1 << Ch_MotorCurrent);}static inline void SetChannel(uint8_t aChannel) {ADMUX = (ADMUX & 0xf0) | (aChannel & 0x0f);}static inline void SetChannel(uint8_t aChannel,eRef aRef) {ADMUX = (ADMUX & 0x20) | (aChannel & 0x0f) | aRef;}static inline void SetChannel(uint8_t aChannel,eRef aRef,eAdjust aAdjust) {ADMUX = (aChannel & 0x0f) | aRef | aAdjust;}static inline void StartConversion(uint8_t aChannel) {SetChannel(aChannel);ADCSRA = ADCSRA_BaseValue | (1 << ADSC) | (1 << ADIE);}static inline void StartConversion(uint8_t aChannel,eRef aRef) {SetChannel(aChannel,aRef);ADCSRA = ADCSRA_BaseValue | (1 << ADSC) | (1 << ADIE);}static inline void StartConversion(uint8_t aChannel,eRef aRef, eAdjust aAdjust) {SetChannel(aChannel,aRef,aAdjust);ADCSRA = ADCSRA_BaseValue | (1 << ADSC) | (1 << ADIE);}static inline void StopConversion() {CLRBIT(ADCSRA,(1 << ADEN)); // This might delete the pending IRQ request, but we don't really care}static inline uint8_t GetChannel() {return ADMUX & 0x0f;}static uint16_t GetSample() {while((ADCSRA & (1 << ADIF)) == 0); // Wait until conversion finishesuint16_t RetVal = ADC;SETBIT(ADCSRA,ADIF); // Clear the pending interrupt request (though we're polling)return RetVal;}static inline uint16_t FastGetSample() {return ADC;}}namespace EEPROM_layout {const uint16_t DataRecord_Ofs = 0x00; // size is sizeof(HBridge::DataRecord)const uint16_t DataValid_Ofs = 0xfe;}template<typename T> inline T min(T aA,T aB) { return (aA>aB)?aB:aA; }template<typename T> inline T max(T aA,T aB) { return (aA>aB)?aA:aB; }#define SHOOT_THROUGH_DELAY for(int i=0;i<10000;++i);namespace HBridge {enum eSampleStates {SampleState_PreFastCollapse = 0,SampleState_FastCollapse,SampleState_PostFastCollapse,SampleState_PreSampleBase,SampleState_PreSampleBase2,SampleState_SampleBase,SampleState_PreSearchMax,SampleState_PreSamplePot,SampleState_SamplePot,SampleState_SearchMax,SampleState_SearchMin,SampleState_PreCurrentSample,SampleState_CurrentSample1,SampleState_CurrentSample2,SampleState_CurrentSample3,// off-band battery sampling for fast-high-side collapse modeSampleState_PreSampleBat,SampleState_SampleBat};enum eOperatingModes {OperatingMode_Speed = 0,OperatingMode_Servo = 1};const int16_t RequestFreewheel = 0x4000;const int16_t RequestBrake = 0x4001;const uint8_t EEPROMDataValid = 0x01;const uint8_t GuardTime = 255 - 47; // Maximum allowed duty cycleconst uint8_t ControlTime = 255 - 10;#if defined MEGA_BRIDGEconst uint8_t LoBMask = (0x01 << 2);const uint8_t LoAMask = (0x01 << 1);const uint8_t HiBMask = (0x01 << 5);const uint8_t HiAMask = (0x01 << 6);const eOperatingModes DefaultOperatingMode = OperatingMode_Speed;const int16_t Def_IFactor = 0;const int16_t Def_PFactor = 0;const int16_t Def_DFactor = 0;const int16_t Def_PFFactor = 0x0100;#elif defined H_BRIDGEconst uint8_t LoBMask = (0x01 << 2);const uint8_t LoAMask = (0x01 << 1);const uint8_t HiBMask = (0x01 << 5);const uint8_t HiAMask = (0x01 << 6);const eOperatingModes DefaultOperatingMode = OperatingMode_Speed;const int16_t Def_IFactor = 0;const int16_t Def_PFactor = 0;const int16_t Def_DFactor = 0;const int16_t Def_PFFactor = 0x0100;#elif defined SERVO_BRAINconst uint8_t LoBMask = (0x01 << 2);const uint8_t LoAMask = (0x01 << 1);const uint8_t HiBMask = (0x01 << 6);const uint8_t HiAMask = (0x01 << 5);const eOperatingModes DefaultOperatingMode = OperatingMode_Servo;const int16_t Def_IFactor = 0;const int16_t Def_PFactor = 0x0200;const int16_t Def_DFactor = 0;const int16_t Def_PFFactor = 0;#else#error No HW version is specified!#endifenum CollapseStates {FastCollapseHighSide = 0,FastCollapseLowSide,SlowCollapseHighSide,SlowCollapseLowSide,CollapseStateMask = 0x0f,CollapseStateAutoCycle = 0x10};uint8_t ADSampleIdx;struct PublicData {// The layout of this record IS important.// This is the public interface that is accessible through the TWI interface// Speed control request signal R/W (Scaled between -0x3fff and 0x3fff// 0x4000 is freewheeling and 0x4001 is brakingint16_t RequestValue;// PID control loop parameters R/Wint16_t IFactor;int16_t PFactor;int16_t DFactor;int16_t PFFactor;int16_t SampleOffset;// Request change limits (acceleration limits) R/Wint16_t MaxPositiveChange;int16_t MaxNegativeChange;// Travel (distance counter) R/Wint32_t Distance;// Travel cutoff R/Wint32_t FwDistanceLimit;int32_t BwDistanceLimit;// Current estimated distance required to stop (R/O)int32_t DistanceToStop;// Modified request (R/O)int16_t CurrentRequest;// Command given to the H-bridge R/Oint16_t Command;// PID loop working set R/Oint16_t IValue;int16_t LastError;int16_t Error;// Last Back-EMF sample R/Oint16_t VoltageSample;// Back-EMF sampling code working set R/Oint16_t BaseValue;int16_t SampleCnt_Snapshot;int16_t MinValue_Snapshot;// Private members: not part of the communication interface, but added to this structure for potential debuggingint16_t MinValue;int16_t MaxValue;int16_t SampleCnt;int16_t OriginalRequestValue;uint8_t SampleState;uint8_t DutyCycleThrottle;uint8_t NewData;uint8_t IsForward;uint16_t CurrentMax;uint16_t CurrentDelta;uint16_t CurrentTemp;uint16_t CurrentMaxSearch;uint8_t ADBufferEnable;uint8_t ADBufferEnableHost;uint16_t CurrentLimit;uint8_t OperatingMode;uint8_t CollapseState;uint16_t ADBuffer[80];} DataRecord;inline uint8_t* GetDataRecord8(uint8_t aOfs) { return ((uint8_t *)&DataRecord)+aOfs; }inline uint16_t* GetDataRecord16(uint8_t aOfs) { return (uint16_t *)(((uint8_t *)&DataRecord)+aOfs); }inline uint32_t* GetDataRecord32(uint8_t aOfs) { return (uint32_t *)(((uint8_t *)&DataRecord)+aOfs); }const inline size_t GetDataRecordSize() { return sizeof(DataRecord); }uint8_t GetDataElementSize(uint8_t aOfs) {#define FIELD_OFFSET(aField) ((uint8_t)((size_t)&(HBridge::DataRecord.aField)-(size_t)&HBridge::DataRecord))#define OFFSET_ENTRY(aField) if (aOfs >= FIELD_OFFSET(aField)) return sizeof(DataRecord.aField); else#define OFFSET_ENTRY_2(aField,aSize) if (aOfs >= FIELD_OFFSET(aField)) return (aSize); elseOFFSET_ENTRY_2(ADBuffer,sizeof(DataRecord.ADBuffer[0]))OFFSET_ENTRY(CollapseState)OFFSET_ENTRY(OperatingMode)OFFSET_ENTRY(CurrentLimit)OFFSET_ENTRY(ADBufferEnableHost)OFFSET_ENTRY(ADBufferEnable)OFFSET_ENTRY(CurrentMaxSearch)OFFSET_ENTRY(CurrentTemp)OFFSET_ENTRY(CurrentDelta)OFFSET_ENTRY(CurrentMax)OFFSET_ENTRY(IsForward)OFFSET_ENTRY(NewData)OFFSET_ENTRY(DutyCycleThrottle)OFFSET_ENTRY(SampleState)OFFSET_ENTRY(OriginalRequestValue)OFFSET_ENTRY(SampleCnt)OFFSET_ENTRY(MaxValue)OFFSET_ENTRY(MinValue)OFFSET_ENTRY(MinValue_Snapshot)OFFSET_ENTRY(SampleCnt_Snapshot)OFFSET_ENTRY(BaseValue)OFFSET_ENTRY(VoltageSample)OFFSET_ENTRY(Error)OFFSET_ENTRY(LastError)OFFSET_ENTRY(IValue)OFFSET_ENTRY(Command)OFFSET_ENTRY(CurrentRequest)OFFSET_ENTRY(DistanceToStop)OFFSET_ENTRY(BwDistanceLimit)OFFSET_ENTRY(FwDistanceLimit)OFFSET_ENTRY(Distance)OFFSET_ENTRY(MaxNegativeChange)OFFSET_ENTRY(MaxPositiveChange)OFFSET_ENTRY(SampleOffset)OFFSET_ENTRY(PFFactor)OFFSET_ENTRY(DFactor)OFFSET_ENTRY(PFactor)OFFSET_ENTRY(IFactor)OFFSET_ENTRY(RequestValue)return 1;#undef FIELD_OFFSET#undef OFFSET_ENTRY#undef OFFSET_ENTRY2}void FreeWheel();inline void SaveSettings() {uint8_t *Data = (uint8_t *)&DataRecord;for(uint8_t i=EEPROM_layout::DataRecord_Ofs;i<EEPROM_layout::DataRecord_Ofs+sizeof(DataRecord);++i,++Data) {EEPROM::SetByte(i,*Data);}EEPROM::SetByte(EEPROM_layout::DataValid_Ofs,EEPROMDataValid);EEPROM::Wait();}inline void Init() {// Init to all channels off - this is the free-wheeling statePWM1::Init(PWM1::ClkDiv256); // Set clock to clkI/O / 256 -> full cycle is around 120Hz with an 8MHz clock//PWM1::Init(PWM1::ClkDiv64); // Set clock to clkI/O / 64 -> full cycle is around 500Hz with an 8MHz clockCLRBIT(PORTD,HiAMask);CLRBIT(PORTD,HiBMask);CLRBIT(PORTB,LoBMask);CLRBIT(PORTB,LoBMask);SETBIT(DDRB,LoBMask);SETBIT(DDRB,LoAMask);SETBIT(DDRD,HiBMask);SETBIT(DDRD,HiAMask);// No control loop - this is the default.DataRecord.IFactor = Def_IFactor;DataRecord.PFactor = Def_PFactor;DataRecord.DFactor = Def_DFactor;DataRecord.PFFactor = Def_PFFactor;DataRecord.SampleOffset = 0;DataRecord.CollapseState = FastCollapseHighSide;DataRecord.OperatingMode = DefaultOperatingMode;// No acceleration control - this is the default.DataRecord.MaxPositiveChange = 0x7fff;DataRecord.MaxNegativeChange = 0x7fff;if (EEPROM::GetByte(EEPROM_layout::DataValid_Ofs) == EEPROMDataValid) {uint8_t *Data = (uint8_t *)&DataRecord;for(uint8_t i=EEPROM_layout::DataRecord_Ofs;i<EEPROM_layout::DataRecord_Ofs+sizeof(DataRecord);++i,++Data) {*Data = EEPROM::GetByte(i);}}DataRecord.IValue = 0;DataRecord.Error = 0;DataRecord.LastError = 0;DataRecord.RequestValue = RequestFreewheel;DataRecord.OriginalRequestValue = RequestFreewheel;DataRecord.IsForward = true;DataRecord.NewData = false;DataRecord.Distance = 0;DataRecord.FwDistanceLimit = 0x7fffffff;DataRecord.BwDistanceLimit = 0x80000000;DataRecord.DutyCycleThrottle = GuardTime;DataRecord.CurrentLimit = 0xffff; // Anything over 0x3ff is OFFfor(uint8_t i = 0;i<sizeof(DataRecord.ADBuffer)/sizeof(DataRecord.ADBuffer[0]);++i) {DataRecord.ADBuffer[i] = 0;}ADSampleIdx = 0;DataRecord.ADBufferEnable = 0;DataRecord.ADBufferEnableHost = true;FreeWheel();PWM1::EnableIRQ_AB();DataRecord.SampleState = SampleState_CurrentSample3;ADConv::StartConversion(ADConv::Ch_MotorCurrent);}// If made inline GCC generates invalid codevoid Forward(uint8_t aSpeed) {if (aSpeed > DataRecord.DutyCycleThrottle) aSpeed = DataRecord.DutyCycleThrottle;if (aSpeed > GuardTime) aSpeed = GuardTime;// Allways clear first, than setCLRBIT(PORTB,LoBMask);CLRBIT(PORTB,LoAMask);CLRBIT(PORTD,HiBMask);SETBIT(PORTD,HiAMask);PWM1::SetChannelB(aSpeed);PWM1::SetChannelA(ControlTime);PWM1::DisableChAB();if (aSpeed > 0) PWM1::EnableChB();DataRecord.IsForward = true;}void Backward(uint8_t aSpeed) {if (aSpeed > DataRecord.DutyCycleThrottle) aSpeed = DataRecord.DutyCycleThrottle;if (aSpeed > GuardTime) aSpeed = GuardTime;// Allways clear first, than setCLRBIT(PORTB,LoBMask);CLRBIT(PORTB,LoAMask);CLRBIT(PORTD,HiAMask);SETBIT(PORTD,HiBMask);PWM1::SetChannelA(aSpeed);PWM1::SetChannelB(ControlTime);PWM1::DisableChAB();if (aSpeed > 0) PWM1::EnableChA();DataRecord.IsForward = false;}inline void FastFieldCollapseHighSide() {if (PWM1::IsChAEnabled()) {CLRBIT(PORTD,HiBMask);CLRBIT(PORTD,HiAMask);SETBIT(PORTD,HiAMask);} else if (PWM1::IsChBEnabled()) {CLRBIT(PORTD,HiAMask);CLRBIT(PORTD,HiBMask);SETBIT(PORTD,HiBMask);}}inline void FastFieldCollapseLowSide() {if (PWM1::IsChAEnabled()) {CLRBIT(PORTD,HiAMask);CLRBIT(PORTD,HiBMask);SHOOT_THROUGH_DELAY;CLRBIT(PORTB,LoAMask);PWM1::DisableChAB();SETBIT(PORTB,LoBMask);} else if (PWM1::IsChBEnabled()) {CLRBIT(PORTD,HiAMask);CLRBIT(PORTD,HiBMask);SHOOT_THROUGH_DELAY;CLRBIT(PORTB,LoBMask);PWM1::DisableChAB();SETBIT(PORTB,LoAMask);}}inline void SlowFieldCollapseHighSide() {}inline void SlowFieldCollapseLowSide() {if (PWM1::IsChAEnabled()) {CLRBIT(PORTD,HiAMask);CLRBIT(PORTD,HiBMask);SHOOT_THROUGH_DELAY;CLRBIT(PORTB,LoBMask);PWM1::DisableChAB();SETBIT(PORTB,LoAMask);} else if (PWM1::IsChBEnabled()) {CLRBIT(PORTD,HiAMask);CLRBIT(PORTD,HiBMask);SHOOT_THROUGH_DELAY;CLRBIT(PORTB,LoAMask);PWM1::DisableChAB();SETBIT(PORTB,LoBMask);}}inline void ResetAfterFastCollapse() {if (TESTBIT(PORTD,HiAMask) != 0) {CLRBIT(PORTD,HiAMask);SETBIT(PORTD,HiBMask);} else if (TESTBIT(PORTD,HiBMask) != 0) {CLRBIT(PORTD,HiBMask);SETBIT(PORTD,HiAMask);} else if (TESTBIT(PORTB,LoAMask) != 0) {CLRBIT(PORTB,LoAMask);SETBIT(PORTB,LoBMask);} else if (TESTBIT(PORTB,LoBMask) != 0) {CLRBIT(PORTB,LoBMask);SETBIT(PORTB,LoAMask);}}void CollapseField() {switch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseHighSide:FastFieldCollapseHighSide();break;case FastCollapseLowSide:FastFieldCollapseLowSide();break;default:case SlowCollapseHighSide:SlowFieldCollapseHighSide();break;case SlowCollapseLowSide:SlowFieldCollapseLowSide();break;}}inline void SwitchCollapseType() {switch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseHighSide:if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {DataRecord.CollapseState = CollapseStateAutoCycle | FastCollapseLowSide;}break;case FastCollapseLowSide:if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {DataRecord.CollapseState = CollapseStateAutoCycle | FastCollapseHighSide;}break;default:case SlowCollapseHighSide:if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {DataRecord.CollapseState = CollapseStateAutoCycle | SlowCollapseLowSide;} else {DataRecord.CollapseState = SlowCollapseHighSide;}break;case SlowCollapseLowSide:if (TESTBIT(DataRecord.CollapseState,CollapseStateAutoCycle) != 0) {DataRecord.CollapseState = CollapseStateAutoCycle | SlowCollapseHighSide;}break;}}inline void ResetHighSide() {if (TESTBIT(PORTD,HiBMask)) {CLRBIT(PORTD,HiBMask);SETBIT(PORTD,HiBMask);}if (TESTBIT(PORTD,HiAMask)) {CLRBIT(PORTD,HiAMask);SETBIT(PORTD,HiAMask);}}void FreeWheel() {// Disable everythingCLRBIT(PORTB,LoBMask);CLRBIT(PORTB,LoAMask);CLRBIT(PORTD,HiBMask);CLRBIT(PORTD,HiAMask);PWM1::DisableChAB();// Set up interrupts to some reasonable valuesif (!DataRecord.IsForward) {PWM1::SetChannelA(0x10);PWM1::SetChannelB(ControlTime);} else {PWM1::SetChannelB(0x10);PWM1::SetChannelA(ControlTime);}}void Brake() {// Allways clear first, than setCLRBIT(PORTB,LoBMask);CLRBIT(PORTB,LoAMask);PWM1::DisableChAB();PWM1::SetChannelB(0x10); // Set it to some reasonable valuePWM1::SetChannelA(ControlTime);DataRecord.IsForward = true;SETBIT(PORTD,HiAMask);SETBIT(PORTD,HiBMask);}void HandleOverload() {// Turn off both low-side FETs - this will remove the load for the rest of the cycleCLRBIT(PORTB,LoBMask);CLRBIT(PORTB,LoAMask);PWM1::DisableChAB();}int16_t ScaledMult(int16_t aA, int16_t aB) {return (((int32_t)aA * (int32_t)aB) >> 8);}static inline void DoControl() {// Control acceleration// Note: DoControl will not be called if RequestValue is Freewheel or Brakingint16_t SpeedDiff = DataRecord.RequestValue - DataRecord.CurrentRequest;if (SpeedDiff > DataRecord.MaxPositiveChange) {DataRecord.CurrentRequest += DataRecord.MaxPositiveChange;} else if (SpeedDiff < -DataRecord.MaxNegativeChange) {DataRecord.CurrentRequest -= DataRecord.MaxNegativeChange;} else {DataRecord.CurrentRequest = DataRecord.RequestValue;}// Limit motion to travel cutoff values. Note that we update RequestValue and not CurrentRequest// so stop will be smooth. Since we estimage the time required to stop this also implements// the go-to-distance functionality.int16_t Change = (DataRecord.OriginalRequestValue > 0)?DataRecord.MaxNegativeChange:-DataRecord.MaxPositiveChange;DataRecord.DistanceToStop = ((int32_t)DataRecord.VoltageSample * (int32_t)DataRecord.VoltageSample / (int32_t)Change) << 3;int32_t StopPosition = DataRecord.Distance + DataRecord.DistanceToStop;if (DataRecord.OriginalRequestValue > 0) {if (StopPosition > DataRecord.FwDistanceLimit) {DataRecord.RequestValue = DataRecord.CurrentRequest - min(DataRecord.MaxNegativeChange,DataRecord.CurrentRequest);}} else {if (StopPosition < DataRecord.BwDistanceLimit) {DataRecord.RequestValue = DataRecord.CurrentRequest + min(DataRecord.MaxPositiveChange,-DataRecord.CurrentRequest);}}// Control loopint16_t ScaledRequest = DataRecord.CurrentRequest >> 4;DataRecord.LastError = DataRecord.Error;DataRecord.Error = DataRecord.VoltageSample - ScaledRequest;int16_t DValue = DataRecord.Error - DataRecord.LastError;DataRecord.Command = ScaledMult(ScaledRequest,DataRecord.PFFactor) + ScaledMult(DataRecord.IValue,DataRecord.IFactor) + ScaledMult(DataRecord.Error,DataRecord.PFactor) + ScaledMult(DValue,DataRecord.DFactor);// Limit command to valid range and limit IValue growth as wellif (DataRecord.Command >= 0x100) {DataRecord.Command = 0xffL;// In an overflow case allow integrator value updates if it works against the overflow (sign bits are differenet)if (((DataRecord.IValue ^ DataRecord.Error) & 0x8000) != 0) DataRecord.IValue += DataRecord.Error;} else if (DataRecord.Command <= -0x100) {DataRecord.Command = -0xffL;// In an overflow case allow integrator value updates if it works against the overflow (sign bits are differenet)if (((DataRecord.IValue ^ DataRecord.Error) & 0x8000) != 0) DataRecord.IValue += DataRecord.Error;} else {// Use saturated arithmetics to avoid roll-over in the accumulatorint32_t TempIValue = (int32_t)DataRecord.IValue + (int32_t)DataRecord.Error;if (TempIValue > 0x7fff) {DataRecord.IValue = 0x7fffL;} else if (TempIValue < -0x7fffL) {DataRecord.IValue = -0x7fff;} else {DataRecord.IValue = TempIValue;}}if (DataRecord.Command > 0) Forward(DataRecord.Command); else Backward((-DataRecord.Command));DataRecord.NewData = true;}uint8_t SampleStateCnt = 0;inline bool BatSampleWhileOn() {uint8_t OnTime = (DataRecord.IsForward)?PWM1::GetChannelB():PWM1::GetChannelA();return (OnTime > 128);}// Samples the positive pole of the motor WRT back-EMFvoid SetADChannelMotorPositive() {if (DataRecord.IsForward) {ADConv::SetChannel(ADConv::Ch_MotorA,ADConv::Ref_MotorA,ADConv::RightAdjust);} else {ADConv::SetChannel(ADConv::Ch_MotorB,ADConv::Ref_MotorB,ADConv::RightAdjust);}}// Samples the negative pole of the motor WRT back-EMFvoid SetADChannelMotorNegative() {if (DataRecord.IsForward) {ADConv::SetChannel(ADConv::Ch_MotorB,ADConv::Ref_MotorB,ADConv::RightAdjust);} else {ADConv::SetChannel(ADConv::Ch_MotorA,ADConv::Ref_MotorA,ADConv::RightAdjust);}}static void StartOffPhase(bool aWasChAOrBEnabled) {// Start new measurementsswitch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseLowSide:SetADChannelMotorNegative();if (aWasChAOrBEnabled) {DataRecord.SampleState = SampleState_PreFastCollapse;} else {DataRecord.SampleState = SampleState_PostFastCollapse;}break;case FastCollapseHighSide:SetADChannelMotorPositive();if (aWasChAOrBEnabled) {DataRecord.SampleState = SampleState_PreFastCollapse;} else {DataRecord.SampleState = SampleState_PostFastCollapse;}break;case SlowCollapseLowSide:SetADChannelMotorNegative();DataRecord.SampleState = SampleState_PreSampleBase;break;case SlowCollapseHighSide:SetADChannelMotorPositive();DataRecord.SampleState = SampleState_PreSampleBase;break;}}static inline void StartOnPhase() {switch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseLowSide:ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);DataRecord.SampleState = SampleState_PreCurrentSample;break;case FastCollapseHighSide:if (BatSampleWhileOn()) {SetADChannelMotorPositive();DataRecord.SampleState = SampleState_PreSampleBat;SampleStateCnt = 0;} else {ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);DataRecord.SampleState = SampleState_PreCurrentSample;}break;case SlowCollapseLowSide:ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);DataRecord.SampleState = SampleState_PreCurrentSample;break;case SlowCollapseHighSide:ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);DataRecord.SampleState = SampleState_PreCurrentSample;break;}bool InvertSample = !DataRecord.IsForward;switch (DataRecord.OperatingMode) {default:case OperatingMode_Speed: {int16_t LocalBaseValue = DataRecord.BaseValue;switch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseHighSide:case SlowCollapseHighSide:break;case FastCollapseLowSide:case SlowCollapseLowSide:InvertSample = !InvertSample;LocalBaseValue = 0; // These are ground-based measurements...break;}if (!InvertSample) {DataRecord.VoltageSample = LocalBaseValue - DataRecord.MinValue;} else {DataRecord.VoltageSample = DataRecord.MinValue - LocalBaseValue;}}break;case OperatingMode_Servo:DataRecord.VoltageSample = DataRecord.MaxValue;break;}DataRecord.VoltageSample -= DataRecord.SampleOffset;DataRecord.SampleCnt_Snapshot = DataRecord.SampleCnt;DataRecord.MinValue_Snapshot = DataRecord.MinValue;}uint8_t BlinkCnt;const uint8_t BlinkInterval = 50;void HandleIRQ(bool IsIRQA) {if (DataRecord.IsForward == IsIRQA) {// Almost at the end: grab back-EMF data from sampling,// do the control, and start sampling for currentStartOnPhase();switch (DataRecord.RequestValue) {case RequestFreewheel:FreeWheel();DataRecord.CurrentRequest = 0;break;case RequestBrake:Brake();DataRecord.CurrentRequest = 0;break;default:// Update the travalled distance:DataRecord.Distance += DataRecord.VoltageSample;DoControl();break;}SwitchCollapseType();// Toggle user LED++BlinkCnt;if (BlinkCnt > BlinkInterval) {BlinkCnt = 0;OpLed::Toggle();}} else {// At the end of the on-part: reverse voltage across motor to fast-discharge it.bool WasChAOrBEnabled = PWM1::IsChAOrBEnabled();CollapseField();ADSampleIdx = 0;if (DataRecord.ADBufferEnable == 0) {DataRecord.ADBufferEnable = 1;}if (DataRecord.ADBufferEnableHost) {DataRecord.ADBufferEnableHost = false;DataRecord.ADBufferEnable = 0;}// Start sampling for voltages and back-EMFStartOffPhase(WasChAOrBEnabled);ResetHighSide();}}uint8_t CurrentSampleCnt;const uint8_t CollapseSearchBlank = 1;const uint16_t CollapseSearchLowLimit = 0x30;const uint16_t CollapseSearchHighLimit = 0x90;void Sample() {int16_t CurData = ADConv::FastGetSample();// Save off samples in the ADBuffer for host-side 'scope' display.if (DataRecord.ADBufferEnable == 0) {if (ADSampleIdx < sizeof(DataRecord.ADBuffer)/sizeof(DataRecord.ADBuffer[0])) {int16_t ADSampleData = (int16_t)ADConv::GetChannel() << 10;ADSampleData |= CurData;DataRecord.ADBuffer[ADSampleIdx] = ADSampleData;++ADSampleIdx;if (ADSampleIdx < sizeof(DataRecord.ADBuffer)/sizeof(DataRecord.ADBuffer[0])) {DataRecord.ADBuffer[ADSampleIdx] = 0;}TOGGLEBIT(PORTD,0x10);}}// State-machine for data-sampling.switch (DataRecord.SampleState) {case SampleState_PreFastCollapse:DataRecord.SampleState = SampleState_FastCollapse;// Update max current from the search fieldDataRecord.CurrentMax = DataRecord.CurrentMaxSearch;// TODO: If DataRecord.CurrentTemp isn't DataRecord.CurrentMaxSearch (more or less)// than the over-current protection must have been activated.SampleStateCnt = 0;break;case SampleState_FastCollapse:// Wait in this state until turn-off transient is overswitch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseLowSide:if (CurData < CollapseSearchLowLimit && SampleStateCnt > CollapseSearchBlank) {ResetAfterFastCollapse();DataRecord.SampleState = SampleState_PostFastCollapse;} else if (CurData > CollapseSearchHighLimit) {SampleStateCnt = CollapseSearchBlank;}++SampleStateCnt;break;case FastCollapseHighSide:if (CurData > CollapseSearchHighLimit && SampleStateCnt > CollapseSearchBlank) {ResetAfterFastCollapse();DataRecord.SampleState = SampleState_PostFastCollapse;} else if (CurData < CollapseSearchLowLimit) {SampleStateCnt = CollapseSearchBlank;}++SampleStateCnt;break;}break;case SampleState_PostFastCollapse:switch (DataRecord.OperatingMode) {default:case OperatingMode_Speed:switch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseLowSide:SetADChannelMotorPositive();DataRecord.BaseValue = CurData;DataRecord.SampleState = SampleState_PreSearchMax;break;case FastCollapseHighSide:if (BatSampleWhileOn()) {SetADChannelMotorNegative();DataRecord.SampleState = SampleState_PreSearchMax;} else {SetADChannelMotorPositive();DataRecord.SampleState = SampleState_PreSampleBat;SampleStateCnt = 0;}break;}break;case OperatingMode_Servo:ADConv::SetChannel(ADConv::Ch_ServoPot,ADConv::Ref_ServoPot,ADConv::RightAdjust);DataRecord.SampleState = SampleState_PreSamplePot;break;}break;case SampleState_PreSampleBase:// Throw away the data, but the next one is for real!DataRecord.SampleState = SampleState_PreSampleBase2;switch (DataRecord.CollapseState & CollapseStateMask) {case SlowCollapseLowSide:SetADChannelMotorNegative();break;case SlowCollapseHighSide:SetADChannelMotorPositive();break;}// Update max current from the search fieldDataRecord.CurrentMax = DataRecord.CurrentMaxSearch;// TODO: If DataRecord.CurrentTemp isn't DataRecord.CurrentMaxSearch (more or less)// than the over-current protection must have been activated.SampleStateCnt = 0;break;case SampleState_PreSampleBase2:// throw away this data, but the next one is for real!DataRecord.SampleState = SampleState_SampleBase;break;case SampleState_SampleBase:switch (DataRecord.OperatingMode) {default:case OperatingMode_Speed:switch (DataRecord.CollapseState & CollapseStateMask) {case SlowCollapseLowSide:SetADChannelMotorPositive();DataRecord.BaseValue = CurData;DataRecord.SampleState = SampleState_PreSearchMax;break;case SlowCollapseHighSide:SetADChannelMotorNegative();DataRecord.BaseValue = CurData;DataRecord.SampleState = SampleState_PreSearchMax;break;}break;case OperatingMode_Servo:ADConv::SetChannel(ADConv::Ch_ServoPot,ADConv::Ref_ServoPot,ADConv::RightAdjust);DataRecord.SampleState = SampleState_PreSamplePot;break;}break;case SampleState_PreSamplePot:DataRecord.SampleState = SampleState_SamplePot;break;case SampleState_SamplePot:DataRecord.MaxValue = CurData;DataRecord.SampleCnt = 1;break;case SampleState_PreSearchMax:DataRecord.SampleState = SampleState_SearchMax;DataRecord.MaxValue = 0x0000;DataRecord.SampleCnt = 0;break;case SampleState_SearchMax:switch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseHighSide:case SlowCollapseLowSide:if (CurData <= DataRecord.MaxValue) {DataRecord.MaxValue = CurData;} else {DataRecord.SampleState = SampleState_SearchMin;DataRecord.MinValue = DataRecord.MaxValue;}break;case FastCollapseLowSide:case SlowCollapseHighSide:if (CurData >= DataRecord.MaxValue) {DataRecord.MaxValue = CurData;} else {DataRecord.SampleState = SampleState_SearchMin;DataRecord.MinValue = DataRecord.MaxValue;}break;}break;case SampleState_SearchMin:switch (DataRecord.CollapseState & CollapseStateMask) {case FastCollapseHighSide:case SlowCollapseLowSide:if (CurData > DataRecord.MinValue) {DataRecord.MinValue = CurData;}DataRecord.SampleCnt++;break;case FastCollapseLowSide:case SlowCollapseHighSide:if (CurData < DataRecord.MinValue) {DataRecord.MinValue = CurData;}DataRecord.SampleCnt++;break;}break;case SampleState_PreSampleBat:if (SampleStateCnt > 4)DataRecord.SampleState = SampleState_SampleBat;++SampleStateCnt;break;case SampleState_SampleBat:DataRecord.BaseValue = CurData;if (BatSampleWhileOn()) {ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);DataRecord.SampleState = SampleState_PreCurrentSample;} else {SetADChannelMotorNegative();DataRecord.SampleState = SampleState_PreSearchMax;}break;case SampleState_PreCurrentSample:ADConv::SetChannel(ADConv::Ch_MotorCurrent,ADConv::Ref_MotorCurrent,ADConv::RightAdjust);CurrentSampleCnt = 0;DataRecord.SampleState = SampleState_CurrentSample1;break;case SampleState_CurrentSample1:if (CurData > DataRecord.CurrentLimit) HandleOverload();if (CurrentSampleCnt == 0) {DataRecord.CurrentTemp = CurData;DataRecord.CurrentMaxSearch = CurData;}++CurrentSampleCnt;if (CurData > DataRecord.CurrentMaxSearch) DataRecord.CurrentMaxSearch = CurData;if (CurrentSampleCnt == 3) DataRecord.SampleState = SampleState_CurrentSample2;break;case SampleState_CurrentSample2:if (CurData > DataRecord.CurrentLimit) HandleOverload();// Sample delta-current. It is related to the back-EMF voltage, though// this measurement is not precise enough to base control off of itDataRecord.CurrentDelta = CurData - DataRecord.CurrentTemp;DataRecord.CurrentTemp = CurData;if (CurData > DataRecord.CurrentMaxSearch) DataRecord.CurrentMaxSearch = CurData;DataRecord.SampleState = SampleState_CurrentSample3;break;case SampleState_CurrentSample3:// We'll stay in this state until the on-phase ends...if (CurData > DataRecord.CurrentLimit) HandleOverload();DataRecord.CurrentTemp = CurData;if (CurData > DataRecord.CurrentMaxSearch) DataRecord.CurrentMaxSearch = CurData;break;}}}SIGNAL(SIG_OUTPUT_COMPARE1A) {HBridge::HandleIRQ(true);}SIGNAL(SIG_OUTPUT_COMPARE1B) {HBridge::HandleIRQ(false);}SIGNAL(SIG_ADC) {HBridge::Sample();}namespace TWI {uint8_t Address;extern const sConfigRecord ConfigRecord PROGMEM __attribute__ ((weak)) = {UniqueIDUnassigned,DevClassHBridge,#if defined MEGA_BRIDGEDevUmHBridge#elif defined H_BRIDGEDevUmHBridge#elif defined SERVO_BRAINDevUmServoBrain#else#error No HW version is specified!#endif};enum UserStates {US_ReceiveAddr = TWI::US_Base,US_ReceiveData,US_SendData};void HandleUserReceive() {State = US_ReceiveAddr;}union {uint8_t UInt8[4];uint16_t UInt16[2];uint32_t UInt32[1];} TransmitBuffer;void SendData() __attribute__ ((noinline));void SendData() {switch (HBridge::GetDataElementSize(Address)) {default:case 1:TWIPrevData = *HBridge::GetDataRecord8(Address);break;case 2:if ((Address & 1) == 0) {TransmitBuffer.UInt16[0] = *HBridge::GetDataRecord16(Address);}TWIPrevData = TransmitBuffer.UInt8[Address & 1];break;case 4:if ((Address & 3) == 0) {TransmitBuffer.UInt32[0] = *HBridge::GetDataRecord32(Address);}TWIPrevData = TransmitBuffer.UInt8[Address & 3];break;}TWDR = TWIPrevData;++Address;if (Address >= HBridge::GetDataRecordSize()) Address = 0; // Wrap around}void GetData() {switch (HBridge::GetDataElementSize(Address)) {default:case 1:TransmitBuffer.UInt8[0] = TWIData;*HBridge::GetDataRecord8(Address) = TransmitBuffer.UInt8[0];break;case 2:TransmitBuffer.UInt8[Address & 1] = TWIData;if ((Address & 1) != 0) {// High-byte: store the whole word*HBridge::GetDataRecord16(Address & (~0x01)) = TransmitBuffer.UInt16[0];// Special-case request value, we have to save that in another spot as well...if (Address == 1) {HBridge::DataRecord.OriginalRequestValue = TransmitBuffer.UInt16[0];}}break;case 4:TransmitBuffer.UInt8[Address & 3] = TWIData;if ((Address & 3) != 0) {// Highest byte: store the whole dword*HBridge::GetDataRecord32(Address & (~0x03)) = TransmitBuffer.UInt32[0];}break;}++Address;if (Address >= HBridge::GetDataRecordSize()) Address = 0; // Wrap around}void HandleUserTransmit() {SendData();State = US_SendData;}void HandleUserState() {switch (State) {case US_ReceiveAddr:switch (TWIStatus) {case TW_SR_DATA_ACK:// TODO: handle different command codes here -> check the data written to this address!if (TWIData == 0xff) {HBridge::SaveSettings();ResetTWI();} else {//if ((TWIData & 0x7f) >= HBridge::GetDataRecordSize()) Address = HBridge::GetDataRecordSize() - 1; else Address = TWIData;if (TWIData >= HBridge::GetDataRecordSize()) Address = HBridge::GetDataRecordSize() - 1; else Address = TWIData;State = US_ReceiveData;}break;default:ResetTWI();break;}break;case US_ReceiveData:switch (TWIStatus) {case TW_SR_DATA_ACK:GetData();// We stay in this state for any optional additional databreak;default:ResetTWI();break;}break;case US_SendData:switch (TWIStatus) {case TW_ST_DATA_ACK:SendData();SETBIT(TWIControl,(1 << TWEA)); // require ACKbreak;default:ResetTWI();break;}break;default:// This really REALLY shouldn't happen...ResetTWI();break;}}}#ifdef USE_SERIAL_DEBUG// Debug interfacesnamespace UsartComm {inline void Init(uint16_t aBaudSetting) {USART0::Init(aBaudSetting);}void HandleInput() {uint8_t CurData = USART0::FastReceiveData();OpLed::Toggle();}}SIGNAL(SIG_USART_RECV) {UsartComm::HandleInput();}void DebugStat() {cli();int16_t VoltageSample = HBridge::DataRecord.VoltageSample;uint16_t SampleCnt = HBridge::DataRecord.SampleCnt_Snapshot;int16_t MinValue = HBridge::DataRecord.MinValue_Snapshot;int16_t CurrentRequest = HBridge::DataRecord.CurrentRequest;int16_t Error = HBridge::DataRecord.Error;int16_t IValue = HBridge::DataRecord.IValue;int16_t Command = HBridge::DataRecord.Command;sei();USART0::SendHexData(VoltageSample);USART0::SendData(' '); USART0::SendData('S'); USART0::SendData(':'); USART0::SendHexData(SampleCnt);USART0::SendData(' '); USART0::SendData('M'); USART0::SendData(':'); USART0::SendHexData(MinValue);USART0::SendData(' '); USART0::SendData('R'); USART0::SendData(':'); USART0::SendHexData(CurrentRequest);USART0::SendData(' '); USART0::SendData('E'); USART0::SendData(':'); USART0::SendHexData(Error);USART0::SendData(' '); USART0::SendData('I'); USART0::SendData(':'); USART0::SendHexData(IValue);USART0::SendData(' '); USART0::SendData('C'); USART0::SendData(':'); USART0::SendHexData(Command);USART0::SendData('\r'); USART0::SendData('\n');}#endif // USE_SERIAL_DEBUGint main() {cli();EEPROM::Init();OpLed::Init();HBridge::BlinkCnt = 0;#ifdef USE_SERIAL_DEBUGUsartComm::Init(USART0::baud57600_8MHz);#endif // USE_SERIAL_DEBUGTCCR0A = 0;TWI::Init();ADConv::Init();HBridge::Init();SETBIT(DDRD,0x08|0x10);sei();// Everything happens in the interrupt routines. We have nothing else to do// here but some optional debuggingwhile (true) {#ifdef USE_SERIAL_DEBUGcli();if (HBridge::DataRecord.NewData) {HBridge::DataRecord.NewData = false;DebugStat();} else {sei();}#endif // USE_SERIAL_DEBUG}return 0;}