No changes between revisions
/Modules/H_BRIDGE/HB1FET01A/SW/h-bridge.cpp
0,0 → 1,1339
// 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/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 PWM
+namespace 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 disables
+ CLRBIT(PORTB,0x02|0x04); // Set port bits to 0 so that disable will work correctly
+ CLRBIT(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_BRIDGE
+ const 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_BRIDGE
+ const 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_BRAIN
+ const 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
+#endif
+
+ const uint8_t ADCSRA_BaseValue = (1 << ADEN) | (1 << ADATE) | (1 << ADPS2) | (1 << ADPS1);
+ static inline void Init() {
+ ADMUX = (1<< REFS0) | 15; // AVCC is the default reference
+ ADCSRA = ADCSRA_BaseValue;
+ ADCSRB = 0; // Free-running mode
+ DIDR0 = (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 finishes
+ uint16_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 mode
+ SampleState_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 cycle
+ const uint8_t ControlTime = 255 - 10;
+#if defined MEGA_BRIDGE
+ const 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_BRIDGE
+ const 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_BRAIN
+ const 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!
+#endif
+
+ enum 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 braking
+ int16_t RequestValue;
+
+ // PID control loop parameters R/W
+ int16_t IFactor;
+ int16_t PFactor;
+ int16_t DFactor;
+ int16_t PFFactor;
+ int16_t SampleOffset;
+
+ // Request change limits (acceleration limits) R/W
+ int16_t MaxPositiveChange;
+ int16_t MaxNegativeChange;
+
+ // Travel (distance counter) R/W
+ int32_t Distance;
+
+ // Travel cutoff R/W
+ int32_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/O
+ int16_t Command;
+
+ // PID loop working set R/O
+ int16_t IValue;
+ int16_t LastError;
+ int16_t Error;
+
+ // Last Back-EMF sample R/O
+ int16_t VoltageSample;
+
+ // Back-EMF sampling code working set R/O
+ int16_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 debugging
+ int16_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); else
+ OFFSET_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 state
+ PWM1::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 clock
+ CLRBIT(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 OFF
+ for(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 code
+ void Forward(uint8_t aSpeed) {
+ if (aSpeed > DataRecord.DutyCycleThrottle) aSpeed = DataRecord.DutyCycleThrottle;
+ if (aSpeed > GuardTime) aSpeed = GuardTime;
+ // Allways clear first, than set
+ CLRBIT(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 set
+ CLRBIT(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 everything
+ CLRBIT(PORTB,LoBMask);
+ CLRBIT(PORTB,LoAMask);
+ CLRBIT(PORTD,HiBMask);
+ CLRBIT(PORTD,HiAMask);
+ PWM1::DisableChAB();
+ // Set up interrupts to some reasonable values
+ if (!DataRecord.IsForward) {
+ PWM1::SetChannelA(0x10);
+ PWM1::SetChannelB(ControlTime);
+ } else {
+ PWM1::SetChannelB(0x10);
+ PWM1::SetChannelA(ControlTime);
+ }
+ }
+ void Brake() {
+ // Allways clear first, than set
+ CLRBIT(PORTB,LoBMask);
+ CLRBIT(PORTB,LoAMask);
+ PWM1::DisableChAB();
+ PWM1::SetChannelB(0x10); // Set it to some reasonable value
+ PWM1::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 cycle
+ CLRBIT(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 Braking
+ int16_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 loop
+ int16_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 well
+ 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 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 accumulator
+ int32_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-EMF
+ void 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-EMF
+ void 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 measurements
+ switch (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 current
+ StartOnPhase();
+
+ 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-EMF
+ StartOffPhase(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 field
+ DataRecord.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 over
+ switch (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 field
+ DataRecord.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 it
+ DataRecord.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_BRIDGE
+ DevUmHBridge
+#elif defined H_BRIDGE
+ DevUmHBridge
+#elif defined SERVO_BRAIN
+ DevUmServoBrain
+#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 data
+ break;
+ default:
+ ResetTWI();
+ break;
+ }
+ break;
+ case US_SendData:
+ switch (TWIStatus) {
+ case TW_ST_DATA_ACK:
+ SendData();
+ SETBIT(TWIControl,(1 << TWEA)); // require ACK
+ break;
+ default:
+ ResetTWI();
+ break;
+ }
+ break;
+ default:
+ // This really REALLY shouldn't happen...
+ ResetTWI();
+ break;
+ }
+ }
+}
+
+#ifdef USE_SERIAL_DEBUG
+// Debug interfaces
+namespace 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_DEBUG
+
+int main() {
+ cli();
+ EEPROM::Init();
+ OpLed::Init();
+ HBridge::BlinkCnt = 0;
+ #ifdef USE_SERIAL_DEBUG
+ UsartComm::Init(USART0::baud57600_8MHz);
+ #endif // USE_SERIAL_DEBUG
+ TCCR0A = 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 debugging
+ while (true) {
+ #ifdef USE_SERIAL_DEBUG
+ cli();
+ if (HBridge::DataRecord.NewData) {
+ HBridge::DataRecord.NewData = false;
+ DebugStat();
+ } else {
+ sei();
+ }
+ #endif // USE_SERIAL_DEBUG
+ }
+ return 0;
+}