Subversion Repositories – MLAB

/Modules/H_Bridge/HB1FET01A/SW @ 816   →   /Modules/H_Bridge/HB1FET01A/SW @ 817

Compare Path:	Rev
With Path:	Rev

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;
+}

/Modules/H_Bridge/HB1FET01A/SW/avr4.x
0,0 → 1,168
/* Default linker script, for normal executables */
OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr")
OUTPUT_ARCH(avr:4)
MEMORY
{
text (rx) : ORIGIN = 0, LENGTH = 8K
data (rw!x) : ORIGIN = 0x800060, LENGTH = 0xffa0
eeprom (rw!x) : ORIGIN = 0x810000, LENGTH = 64K
}
SECTIONS
{
/* Read-only sections, merged into text segment: */
.hash : { *(.hash) }
.dynsym : { *(.dynsym) }
.dynstr : { *(.dynstr) }
.gnu.version : { *(.gnu.version) }
.gnu.version_d : { *(.gnu.version_d) }
.gnu.version_r : { *(.gnu.version_r) }
.rel.init : { *(.rel.init) }
.rela.init : { *(.rela.init) }
.rel.text :
{
*(.rel.text)
*(.rel.text.*)
*(.rel.gnu.linkonce.t*)
}
.rela.text :
{
*(.rela.text)
*(.rela.text.*)
*(.rela.gnu.linkonce.t*)
}
.rel.fini : { *(.rel.fini) }
.rela.fini : { *(.rela.fini) }
.rel.rodata :
{
*(.rel.rodata)
*(.rel.rodata.*)
*(.rel.gnu.linkonce.r*)
}
.rela.rodata :
{
*(.rela.rodata)
*(.rela.rodata.*)
*(.rela.gnu.linkonce.r*)
}
.rel.data :
{
*(.rel.data)
*(.rel.data.*)
*(.rel.gnu.linkonce.d*)
}
.rela.data :
{
*(.rela.data)
*(.rela.data.*)
*(.rela.gnu.linkonce.d*)
}
.rel.ctors : { *(.rel.ctors) }
.rela.ctors : { *(.rela.ctors) }
.rel.dtors : { *(.rel.dtors) }
.rela.dtors : { *(.rela.dtors) }
.rel.got : { *(.rel.got) }
.rela.got : { *(.rela.got) }
.rel.bss : { *(.rel.bss) }
.rela.bss : { *(.rela.bss) }
.rel.plt : { *(.rel.plt) }
.rela.plt : { *(.rela.plt) }
/* Internal text space or external memory */
.text :
{
KEEP (*(.vectors))
__ctors_start = . ;
KEEP (*(.ctors))
__ctors_end = . ;
__dtors_start = . ;
KEEP (*(.dtors))
__dtors_end = . ;
*(.progmem.gcc*)
*(.progmem*)
. = ALIGN(2);
KEEP (*(.init0)) /* Start here after reset. */
KEEP (*(.init1))
KEEP (*(.init2)) /* Clear __zero_reg__, set up stack pointer. */
KEEP (*(.init3))
KEEP (*(.init4)) /* Initialize data and BSS. */
KEEP (*(.init5))
KEEP (*(.init6)) /* C++ constructors. */
KEEP (*(.init7))
KEEP (*(.init8))
KEEP (*(.init9)) /* Call main(). */
*(.text)
. = ALIGN(2);
*(.text.*)
. = ALIGN(2);
*(.fini9) /* _exit() starts here. */
*(.fini8)
*(.fini7)
*(.fini6) /* C++ destructors. */
*(.fini5)
*(.fini4)
*(.fini3)
*(.fini2)
*(.fini1)
*(.fini0) /* Infinite loop after program termination. */
_etext = . ;
} > text
.data : AT (ADDR (.text) + SIZEOF (.text))
{
PROVIDE (__data_start = .) ;
*(.data)
*(.gnu.linkonce.d*)
. = ALIGN(2);
_edata = . ;
PROVIDE (__data_end = .) ;
} > data
.bss SIZEOF(.data) + ADDR(.data) :
{
PROVIDE (__bss_start = .) ;
*(.bss)
*(COMMON)
PROVIDE (__bss_end = .) ;
} > data
__data_load_start = LOADADDR(.data);
__data_load_end = __data_load_start + SIZEOF(.data);
/* Global data not cleared after reset. */
.noinit SIZEOF(.bss) + ADDR(.bss) :
{
PROVIDE (__noinit_start = .) ;
*(.noinit*)
PROVIDE (__noinit_end = .) ;
_end = . ;
PROVIDE (__heap_start = .) ;
} > data
.eeprom :
{
*(.eeprom*)
__eeprom_end = . ;
} > eeprom
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
/* DWARF debug sections.
Symbols in the DWARF debugging sections are relative to the beginning
of the section so we begin them at 0. */
/* DWARF 1 */
.debug 0 : { *(.debug) }
.line 0 : { *(.line) }
/* GNU DWARF 1 extensions */
.debug_srcinfo 0 : { *(.debug_srcinfo) }
.debug_sfnames 0 : { *(.debug_sfnames) }
/* DWARF 1.1 and DWARF 2 */
.debug_aranges 0 : { *(.debug_aranges) }
.debug_pubnames 0 : { *(.debug_pubnames) }
/* DWARF 2 */
.debug_info 0 : { *(.debug_info) *(.gnu.linkonce.wi.*) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_line 0 : { *(.debug_line) }
.debug_frame 0 : { *(.debug_frame) }
.debug_str 0 : { *(.debug_str) }
.debug_loc 0 : { *(.debug_loc) }
.debug_macinfo 0 : { *(.debug_macinfo) }
}

/Modules/H_Bridge/HB1FET01A/SW/avr5.x
0,0 → 1,168
/* Default linker script, for normal executables */
OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr")
OUTPUT_ARCH(avr:5)
MEMORY
{
text (rx) : ORIGIN = 0, LENGTH = 128K
data (rw!x) : ORIGIN = 0x800060, LENGTH = 0xffa0
eeprom (rw!x) : ORIGIN = 0x810000, LENGTH = 64K
}
SECTIONS
{
/* Read-only sections, merged into text segment: */
.hash : { *(.hash) }
.dynsym : { *(.dynsym) }
.dynstr : { *(.dynstr) }
.gnu.version : { *(.gnu.version) }
.gnu.version_d : { *(.gnu.version_d) }
.gnu.version_r : { *(.gnu.version_r) }
.rel.init : { *(.rel.init) }
.rela.init : { *(.rela.init) }
.rel.text :
{
*(.rel.text)
*(.rel.text.*)
*(.rel.gnu.linkonce.t*)
}
.rela.text :
{
*(.rela.text)
*(.rela.text.*)
*(.rela.gnu.linkonce.t*)
}
.rel.fini : { *(.rel.fini) }
.rela.fini : { *(.rela.fini) }
.rel.rodata :
{
*(.rel.rodata)
*(.rel.rodata.*)
*(.rel.gnu.linkonce.r*)
}
.rela.rodata :
{
*(.rela.rodata)
*(.rela.rodata.*)
*(.rela.gnu.linkonce.r*)
}
.rel.data :
{
*(.rel.data)
*(.rel.data.*)
*(.rel.gnu.linkonce.d*)
}
.rela.data :
{
*(.rela.data)
*(.rela.data.*)
*(.rela.gnu.linkonce.d*)
}
.rel.ctors : { *(.rel.ctors) }
.rela.ctors : { *(.rela.ctors) }
.rel.dtors : { *(.rel.dtors) }
.rela.dtors : { *(.rela.dtors) }
.rel.got : { *(.rel.got) }
.rela.got : { *(.rela.got) }
.rel.bss : { *(.rel.bss) }
.rela.bss : { *(.rela.bss) }
.rel.plt : { *(.rel.plt) }
.rela.plt : { *(.rela.plt) }
/* Internal text space or external memory */
.text :
{
KEEP (*(.vectors))
__ctors_start = . ;
KEEP (*(.ctors))
__ctors_end = . ;
__dtors_start = . ;
KEEP (*(.dtors))
__dtors_end = . ;
*(.progmem.gcc*)
*(.progmem*)
. = ALIGN(2);
KEEP (*(.init0)) /* Start here after reset. */
KEEP (*(.init1))
KEEP (*(.init2)) /* Clear __zero_reg__, set up stack pointer. */
KEEP (*(.init3))
KEEP (*(.init4)) /* Initialize data and BSS. */
KEEP (*(.init5))
KEEP (*(.init6)) /* C++ constructors. */
KEEP (*(.init7))
KEEP (*(.init8))
KEEP (*(.init9)) /* Call main(). */
*(.text)
. = ALIGN(2);
*(.text.*)
. = ALIGN(2);
*(.fini9) /* _exit() starts here. */
*(.fini8)
*(.fini7)
*(.fini6) /* C++ destructors. */
*(.fini5)
*(.fini4)
*(.fini3)
*(.fini2)
*(.fini1)
*(.fini0) /* Infinite loop after program termination. */
_etext = . ;
} > text
.data : AT (ADDR (.text) + SIZEOF (.text))
{
PROVIDE (__data_start = .) ;
*(.data)
*(.gnu.linkonce.d*)
. = ALIGN(2);
_edata = . ;
PROVIDE (__data_end = .) ;
} > data
.bss SIZEOF(.data) + ADDR(.data) :
{
PROVIDE (__bss_start = .) ;
*(.bss)
*(COMMON)
PROVIDE (__bss_end = .) ;
} > data
__data_load_start = LOADADDR(.data);
__data_load_end = __data_load_start + SIZEOF(.data);
/* Global data not cleared after reset. */
.noinit SIZEOF(.bss) + ADDR(.bss) :
{
PROVIDE (__noinit_start = .) ;
*(.noinit*)
PROVIDE (__noinit_end = .) ;
_end = . ;
PROVIDE (__heap_start = .) ;
} > data
.eeprom :
{
*(.eeprom*)
__eeprom_end = . ;
} > eeprom
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
/* DWARF debug sections.
Symbols in the DWARF debugging sections are relative to the beginning
of the section so we begin them at 0. */
/* DWARF 1 */
.debug 0 : { *(.debug) }
.line 0 : { *(.line) }
/* GNU DWARF 1 extensions */
.debug_srcinfo 0 : { *(.debug_srcinfo) }
.debug_sfnames 0 : { *(.debug_sfnames) }
/* DWARF 1.1 and DWARF 2 */
.debug_aranges 0 : { *(.debug_aranges) }
.debug_pubnames 0 : { *(.debug_pubnames) }
/* DWARF 2 */
.debug_info 0 : { *(.debug_info) *(.gnu.linkonce.wi.*) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_line 0 : { *(.debug_line) }
.debug_frame 0 : { *(.debug_frame) }
.debug_str 0 : { *(.debug_str) }
.debug_loc 0 : { *(.debug_loc) }
.debug_macinfo 0 : { *(.debug_macinfo) }
}

/Modules/H_Bridge/HB1FET01A/SW/common.h
0,0 → 1,33
#ifndef __common_h__
#define __common_h__
#define CLRBIT(aReg,aBit) (aReg) &= ~(aBit)
#define SETBIT(aReg,aBit) (aReg) |= (aBit)
#define TOGGLEBIT(aReg,aBit) (aReg) ^= (aBit)
//#define TESTBIT(aValue,aConst) ((aValue & aConst) != 0)
#define TESTBIT(aValue,aConst) ({ \
uint8_t Temp = (uint8_t)(aValue); \
__asm__ ( \
"andi %0, %2" "\n\t" \
"breq L_%=" "\n\t" \
"ldi %0,1" "\n\t" \
"L_%=: " "\n\t" \
: "=r" (Temp) : "0" (Temp), "M" (aConst)\
); \
Temp; \
})
#if defined BOARD_rs232if
#define BOARD
#elif defined BOARD_umServo32
#define BOARD
#elif defined BOARD_umHBridge
#define BOARD
#elif defined BOARD_NA
#define BOARD
#else
#error BOARD_xxx must be defined
#endif
#endif // __common_h__

/Modules/H_Bridge/HB1FET01A/SW/eeprom.cpp
0,0 → 1,43
#include <avr/pgmspace.h>
#include "eeprom.h"
#include "common.h"
void EEPROM::Wait() {
while(TESTBIT(EECR,(1<<EEPE)));
}
uint8_t EEPROM::GetByte(uint16_t aOfs) {
// Wait for any pending writes to finish
while(TESTBIT(EECR,(1<<EEPE)));
EEARL = aOfs & 0x00ff;
#ifdef EEARH
EEARH = 0;
#endif
SETBIT(EECR,(1 << EERE));
uint8_t RetVal = EEDR;
return RetVal;
}
void EEPROM::SetByte(uint16_t aOfs,uint8_t aData) {
// Wait for any pending writes to finish
while(TESTBIT(EECR,(1<<EEPE)));
EEARL = aOfs & 0x00ff;
#ifdef EEARH
EEARH = 0;
#endif
EEDR = aData;
SETBIT(EECR,(1<<EEMPE));
SETBIT(EECR,(1<<EEPE));
}
uint16_t EEPROM::GetWord(uint16_t aOfs) {
uint16_t RetVal;
((uint8_t *)(&RetVal))[0] = GetByte(aOfs+0);
((uint8_t *)(&RetVal))[1] = GetByte(aOfs+1);
return RetVal;
}
void EEPROM::SetWord(uint16_t aOfs,uint16_t aData) {
SetByte(aOfs+0,((uint8_t *)(&aData))[0]);
SetByte(aOfs+1,((uint8_t *)(&aData))[1]);
}

/Modules/H_Bridge/HB1FET01A/SW/eeprom.h
0,0 → 1,18
#ifndef __EEPROM_H__
#define __EEPROM_H__
namespace EEPROM {
inline void Init() {
EECR = 0; // Make sure we're erasing and writing at the same time
}
uint8_t GetByte(uint16_t aOfs);
void SetByte(uint16_t aOfs,uint8_t aData);
uint16_t GetWord(uint16_t aOfs);
void SetWord(uint16_t aOfs,uint16_t aData);
void Wait();
}
namespace EEPROM_layout {
}
#endif // __EEPROM_H__

/Modules/H_Bridge/HB1FET01A/SW/gentwiuid.exe
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream
Property changes:
Added: svn:mime-type
+application/octet-stream
\ No newline at end of property

/Modules/H_Bridge/HB1FET01A/SW/guids.h
0,0 → 1,63
#ifndef __guids_h__
#define __guids_h__
#ifdef BOARD
// We're included for the target board. Define various structures
struct GUID {
uint32_t Part1;
uint16_t Part2;
uint16_t Part3;
uint8_t Part4[8];
};
#endif
// These are the only valid values for UniqueID items
const uint8_t num0 = 0x00;
const uint8_t num1 = 0x01;
const uint8_t num2 = 0x03;
const uint8_t num3 = 0x07;
const uint8_t num4 = 0x0f;
const uint8_t num5 = 0x1f;
const uint8_t num6 = 0x3f;
const uint8_t num7 = 0x7f;
const uint8_t num8 = 0xff; // this value is never used in any valid uniqueID, however it IS used in the unassigned ID
// Unique IDs
#define UniqueIDUnassigned { \
num0,num1,num2,num3, \
num4,num5,num6,num7, \
num8,num7,num6,num5, \
num4,num3,num2,num1, \
num0,num1,num2,num3, \
num4,num5,num6,num7, \
num8,num7,num6,num5, \
num4,num3,num2,num1, \
num0,num1,num2,num3, \
num4,num5,num6,num7, \
num8,num7,num6,num5, \
num4,num3,num2,num1 \
}
// Device Class GUIDs
// {00000000-0000-0000-0000-000000000000}
#define DevClassReserved { 0x00000000, 0x0000, 0x0000, { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } }
// {D8745C7D-71A5-4977-A989-2E1EF853CCB8}
#define DevClassUnassigned { 0xd8745c7d, 0x71a5, 0x4977, { 0xa9, 0x89, 0x2e, 0x1e, 0xf8, 0x53, 0xcc, 0xb8 } }
// {15D37F46-A4F2-416d-8157-FAB7D1A66BE8}
#define DevClassServoController { 0x15d37f46, 0xa4f2, 0x416d, { 0x81, 0x57, 0xfa, 0xb7, 0xd1, 0xa6, 0x6b, 0xe8 } }
// {D3C8787E-DCE8-445b-B1C7-86C2BF85D144}
#define DevClassHBridge { 0xd3c8787e, 0xdce8, 0x445b, { 0xb1, 0xc7, 0x86, 0xc2, 0xbf, 0x85, 0xd1, 0x44 } }
// Device GUIDs
// {00000000-0000-0000-0000-000000000000}
#define DevReserved { 0x00000000, 0x0000, 0x0000, { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } }
// {E59F78CE-6328-4316-B1DE-F0AA5E45F34F}
#define DevUnassigned { 0xe59f78ce, 0x6328, 0x4316, { 0xb1, 0xde, 0xf0, 0xaa, 0x5e, 0x45, 0xf3, 0x4f } }
// {E3470D73-9110-4d61-915E-916FF8A89BA6}
#define DevUmServo32 { 0xe3470d73, 0x9110, 0x4d61, { 0x91, 0x5e, 0x91, 0x6f, 0xf8, 0xa8, 0x9b, 0xa6 } }
// {CF232A58-497F-4bba-9E13-D9AB7938B418}
#define DevUmHBridge { 0xcf232a58, 0x497f, 0x4bba, { 0x9e, 0x13, 0xd9, 0xab, 0x79, 0x38, 0xb4, 0x18 } }
// {518FC22C-B5A1-40b3-8D7C-C75EC729F25C}
#define DevUmServoBrain { 0x518fc22c, 0xb5a1, 0x40b3, { 0x8d, 0x7c, 0xc7, 0x5e, 0xc7, 0x29, 0xf2, 0x5c } }
#endif // __guids_h__

/Modules/H_Bridge/HB1FET01A/SW/opled.h
0,0 → 1,34
#ifndef __opled_h__
#define __opled_h__
#include "common.h"
namespace OpLed {
#if defined BOARD_rs232if
const int OpLedMask=(0x01 << 4);
#elif defined BOARD_umServo32
const int OpLedMask=(0x01 << 4);
#elif defined BOARD_umHBridge
const int OpLedMask=(0x01 << 2);
#else
#error Unknown board specification
#endif
void inline Init() {
SETBIT(DDRD,OpLedMask);
}
void inline On() {
SETBIT(PORTD,OpLedMask);
}
void inline Off() {
CLRBIT(PORTD,OpLedMask);
}
void inline Toggle() {
TOGGLEBIT(PORTD,OpLedMask);
}
}
#endif // __opled_h__

/Modules/H_Bridge/HB1FET01A/SW/twi_aap.cpp
0,0 → 1,375
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <avr/sleep.h>
#include "common.h"
#include "usart.h"
#include "twi_aap.h"
#include "eeprom.h"
/*********
switch (TWOStatus) {
case TW_SR_SLA_ACK: //0x60
break;
case TW_SR_ARB_LOST_SLA_ACK: //0x68
break;
case TW_SR_GCALL_ACK: //0x70
break;
case TW_SR_ARB_LOST_GCALL_ACK: //0x78
break;
case TW_SR_DATA_ACK: //0x80
break;
case TW_SR_DATA_NACK: //0x88
break;
case TW_SR_GCALL_DATA_ACK: //0x90
break;
case TW_SR_GCALL_DATA_NACK: //0x98
break;
case TW_ST_LAST_DATA: //0xC8
case TW_SR_STOP: //0xA0
State = MS_Idle;
SETBIT(TWIControl,(1 << TWEA)); // turn address recognition back on
break;
default: // All non-handled states will basically terminate any processing
State = MS_Idle;
CLRBIT(TWIControl,(1 << TWEA)); // will return NACK, i.e. will pretend to be unaddressed
break;
}
switch (TWOStatus) {
case TW_ST_SLA_ACK: //0xA8
break;
case TW_ST_ARB_LOST_SLA_ACK: //0xB0
break;
case TW_ST_DATA_ACK: //0xB8
break;
case TW_ST_DATA_NACK: //0xC0
break;
case TW_ST_ARB_LOST: //0x38 - let's hope it works
break;
case TW_ST_LAST_DATA: //0xC8
State = MS_Idle;
SETBIT(TWIControl,(1 << TWEA)); // turn address recognition back on
break;
default: // All non-handled states will basically terminate any processing
State = MS_Idle;
CLRBIT(TWIControl,(1 << TWEA)); // will return NACK, i.e. will pretend to be unaddressed
break;
}
**********/
namespace TWI {
extern const sConfigRecord ConfigRecord PROGMEM __attribute__ ((weak)) = {UniqueIDUnassigned,DevClassUnassigned,DevUnassigned};
// void TWI::UserInit() __attribute__ ((weak));
void TWI::HandleUserReceive() __attribute__ ((weak));
void TWI::HandleUserTransmit() __attribute__ ((weak));
void TWI::HandleUserState() __attribute__ ((weak));
static inline void TWI::HandleState_MS_Idle();
static inline void TWI::HandleState_MS_Addressed();
static inline void TWI::StateMachine();
uint8_t ConfigCnt;
}
void TWI::ResetTWITransmit() {
// Same as above, but also returns the statemachine to the idle state
State = MS_Idle;
LastCmd = TWI_AAPCmd_Reserved;
// In slave-transmitter mode it's not possible to reset the TWI.
// But at least we can transmit all '1'-s so we won't interfere with the bus.
#ifdef SERIAL_DBG
USART0::SendData('*');
#endif
TWDR = 0xff;
SETBIT(TWIControl,(1 << TWEA) | (1 << TWSTO));
}
void TWI::ResetTWIReceive() {
// Same as above, but also returns the statemachine to the idle state
State = MS_Idle;
LastCmd = TWI_AAPCmd_Reserved;
// In slave-transmitter mode it's not possible to reset the TWI.
// But at least we can transmit all '1'-s so we won't interfere with the bus.
#ifdef SERIAL_DBG
USART0::SendData('#');
#endif
TWDR = 0xff;
SETBIT(TWIControl,(1 << TWSTO));
CLRBIT(TWIControl,(1 << TWEA));
}
void TWI::ResetTWI() {
switch (TWIStatus) {
case TW_ST_LAST_DATA: //0xC8
case TW_SR_STOP:
case TW_ST_DATA_NACK:
case TW_SR_DATA_NACK:
case TW_SR_GCALL_DATA_NACK:
State = MS_Idle;
SETBIT(TWIControl,(1 << TWEA)); // turn address recognition back on
CLRBIT(TWIControl,(1 << TWSTO)); // even if we did so, stop aborting
break;
case TW_BUS_ERROR:
// This is the point where we re-enable the address recognition
ResetTWITransmit();
break;
default:
ResetTWIReceive();
break;
}
}
/*void TWI::UserInit() {
}*/
void TWI::HandleUserReceive() {
#ifdef SERIAL_DBG
USART0::SendData('r');
#endif
ResetTWI();
}
void TWI::HandleUserTransmit() {
#ifdef SERIAL_DBG
USART0::SendData('t');
#endif
ResetTWI();
}
void TWI::HandleUserState() {
}
static inline void TWI::HandleState_MS_Idle() {
switch (TWIStatus) {
case TW_SR_SLA_ACK:
LastCmd = TWI_AAPCmd_Reserved;
HandleUserReceive();
break;
case TW_SR_GCALL_ACK:
State = MS_Addressed;
break;
case TW_ST_SLA_ACK:
if ((TWIData & 0xfe) == GeneralCallAddr) {
// TWI_AAP read command: things depend on what the last (write) command was:
if (LastCmd == TWI_AAPCmd_GetConfig) {
// This is the read part of the GetUDID command: Send the byte count and change state
TWIPrevData = sizeof(ConfigRecord);
TWDR = TWIPrevData;
State = AAP_CmdGetConfig_SendConfig;
ConfigCnt = 0;
//SETBIT(TWIControl,(1 << TWEA)); // require ACK
} else {
//Unknown read command: ignore
ResetTWITransmit();
}
} else {
HandleUserTransmit();
}
break;
case TW_ST_LAST_DATA:
case TW_SR_STOP:
#ifdef SERIAL_DBG
USART0::SendData('.');
#endif
State = MS_Idle;
SETBIT(TWIControl,(1 << TWEA)); // turn address recognition back on
CLRBIT(TWIControl,(1 << TWSTO)); // even if we did so, stop aborting
break;
case TW_ST_DATA_ACK:
case TW_SR_DATA_ACK:
case TW_SR_GCALL_DATA_ACK:
ResetTWIReceive();
break;
case TW_BUS_ERROR:
case TW_ST_DATA_NACK:
case TW_SR_DATA_NACK:
case TW_SR_GCALL_DATA_NACK:
// This is the point where we re-enable the address recognition
ResetTWITransmit();
break;
default:
State = MS_Idle;
SETBIT(TWIControl,(1 << TWEA) | (1 << TWSTO)); // reset the TWI
break;
}
}
static inline void TWI::HandleState_MS_Addressed() {
switch (TWIStatus) {
case TW_SR_GCALL_DATA_ACK:
switch(TWIData) {
case TWI_AAPCmd_ResetDevices:
LastCmd = TWIData;
State = MS_Idle;
SoftAddress = 0;
TWAR = SoftAddress | 1; // General call recognition and slave-receiver mode through address being 0
break;
case TWI_AAPCmd_ResetToPermAddress:
LastCmd = TWIData;
State = MS_Idle;
SoftAddress = EEPROM::GetByte(EEPROM_layout::TWI_SoftAddr_Ofs);
TWAR = SoftAddress | 1; // General call recognition and slave-receiver mode
if (SoftAddress != 0) {
// We have a valid address, do not respond to this command
LastCmd = TWI_AAPCmd_Reserved;
}
break;
case TWI_AAPCmd_GetConfig:
LastCmd = TWIData;
State = AAP_CmdGetConfig_GetAddress;
break;
case TWI_AAPCmd_AssignAddress:
LastCmd = TWIData;
ConfigCnt = 0;
State = AAP_CmdAssignAddress_GetUniqueID;
break;
default:
// unknown command - ignore the rest
ResetTWIReceive();
break;
}
break;
default:
ResetTWI();
break;
}
}
static inline void TWI::StateMachine() {
TWIStatus = TWSR & TW_STATUS_MASK;
TWIControl = TWCR;
if (TWIStatus != TW_SR_STOP) {
TWIData = TWDR;
} else {
// Fast short-circuit for stop-bit detection
// This condition cannot use clock-stretching so we might miss the next start if we don't hurry
State = MS_Idle;
SETBIT(TWIControl,(1 << TWEA)); // turn address recognition back on
CLRBIT(TWIControl,(1 << TWSTO)); // even if we did so, stop aborting
SETBIT(TWIControl,(1 << TWINT));
TWCR = TWIControl;
return;
}
#ifdef SERIAL_DBG
// DUMP state to serial port
USART0::SendData('s'); USART0::SendData(' '); USART0::SendHexData(TWIStatus); USART0::SendData(' ');
USART0::SendData('d'); USART0::SendData(' '); USART0::SendHexData(TWIData); USART0::SendData(' ');
USART0::SendData('S'); USART0::SendData(' '); USART0::SendHexData(State); USART0::SendData(' ');
USART0::SendData('L'); USART0::SendData(' '); USART0::SendHexData(LastCmd); USART0::SendData(' ');
USART0::SendData('A'); USART0::SendData(' '); USART0::SendHexData(SoftAddress); USART0::SendData('-');
#endif
switch (State) {
case MS_Idle:
HandleState_MS_Idle();
break;
case MS_Addressed:
HandleState_MS_Addressed();
break;
case AAP_CmdGetConfig_GetAddress:
switch (TWIStatus) {
case TW_SR_GCALL_DATA_ACK:
State = MS_Idle;
SETBIT(TWIControl,(1 << TWEA) | (1 << TWSTO)); // reset the TWI
if (SoftAddress != TWIData) {
// We have a valid address, or was not addressed -> do not respond to this command
LastCmd = TWI_AAPCmd_Reserved;
}
break;
default:
ResetTWI();
break;
}
break;
case AAP_CmdAssignAddress_GetUniqueID:
switch (TWIStatus) {
case TW_SR_GCALL_DATA_ACK:
if (TWIData == pgm_read_byte(ConfigRecord.UniqueID+ConfigCnt)) {
++ConfigCnt;
if (ConfigCnt == UniqueID_Size) State = AAP_CmdAssignAddress_GetAddress;
} else {
// UDID is not ours, abort command
ResetTWIReceive();
}
break;
default:
ResetTWI();
break;
}
break;
case AAP_CmdAssignAddress_GetAddress:
switch (TWIStatus) {
case TW_SR_GCALL_DATA_ACK:
SoftAddress = TWIData & 0xfe;
if (TWIData & 0x01 == 1) {
EEPROM::SetByte(EEPROM_layout::TWI_SoftAddr_Ofs,SoftAddress);
}
State = MS_Idle;
TWAR = SoftAddress | 1; // General call recognition and normal operation
SETBIT(TWIControl,(1 << TWEA) | (1 << TWSTO)); // reset the TWI
LastCmd = TWI_AAPCmd_Reserved;
break;
default:
ResetTWI();
break;
}
break;
// Read-oriented commands
case AAP_CmdGetConfig_SendConfig:
switch (TWIStatus) {
case TW_ST_DATA_ACK:
if (TWIData != TWIPrevData) {
// We've lost the arbitration
ResetTWITransmit();
break;
}
TWIPrevData = pgm_read_byte(((uint8_t*)(&ConfigRecord))+ConfigCnt);
TWDR = TWIPrevData;
ConfigCnt++;
//At the last byte, we require NACK
if (ConfigCnt == sizeof(ConfigRecord)) {
State = MS_Idle;
CLRBIT(TWIControl,(1 << TWEA)); // require NACK
} else {
State = AAP_CmdGetConfig_SendConfig;
SETBIT(TWIControl,(1 << TWEA)); // require ACK
}
break;
default:
ResetTWI();
break;
}
break;
// User commands
default:
HandleUserState();
break;
}
// re-enable the TWI interrupts
SETBIT(TWIControl,1 << TWINT);
#ifdef SERIAL_DBG
USART0::SendData('S'); USART0::SendData(' '); USART0::SendHexData(State); USART0::SendData(' ');
USART0::SendData('L'); USART0::SendData(' '); USART0::SendHexData(LastCmd); USART0::SendData(' ');
USART0::SendData('A'); USART0::SendData(' '); USART0::SendHexData(SoftAddress); USART0::SendData(' ');
USART0::SendData('C'); USART0::SendData(' '); USART0::SendHexData(TWIControl); USART0::SendData(0x0d); USART0::SendData(0x0a);
#endif
TWCR = TWIControl;
}
SIGNAL(SIG_TWI) {TWI::StateMachine();}
uint8_t TWI::State;
uint8_t TWI::LastCmd;
uint8_t TWI::SoftAddress;
uint8_t TWI::TWIStatus;
uint8_t TWI::TWIData;
uint8_t TWI::TWIPrevData;
uint8_t TWI::TWIControl;

/Modules/H_Bridge/HB1FET01A/SW/twi_aap.h
0,0 → 1,77
#ifndef __twi_aap_h__
#define __two_aap_h__
#include <compat/twi.h>
#include "common.h"
#include "usart.h"
#include "guids.h"
#include "eeprom.h"
//#define SERIAL_DBG
namespace EEPROM_layout {
const uint16_t TWI_SoftAddr_Ofs = 0xff;
}
namespace TWI {
const uint8_t GeneralCallAddr = 0;
const size_t UniqueID_Size = 16*3;
const size_t GUID_Size = sizeof(GUID);
struct sConfigRecord {
uint8_t UniqueID[UniqueID_Size];
GUID ClassID;
GUID DevID;
};
enum TWI_AAPCommands {
TWI_AAPCmd_Reserved = 0,
TWI_AAPCmd_Start = 0x20,
TWI_AAPCmd_End = 0x21,
TWI_AAPCmd_ResetDevices = 0x22,
TWI_AAPCmd_ResetToPermAddress = 0x23,
TWI_AAPCmd_GetConfig = 0x24,
TWI_AAPCmd_AssignAddress = 0x25
};
enum MainStates {
MS_Idle = 0,
MS_Addressed,
AAP_CmdGetConfig_GetAddress,
AAP_CmdGetConfig_SendConfig,
AAP_CmdAssignAddress_GetUniqueID,
AAP_CmdAssignAddress_GetAddress,
US_Base
};
extern uint8_t State;
extern uint8_t LastCmd;
extern uint8_t SoftAddress;
inline void Init() {
TWBR = 32; // 100kHz with an 8MHz crystal
// TWBR = 16; // 200kHz with an 8MHz crystal
TWSR = 0; // pre-scaler: 1
TWCR = (1 << TWEA) | (1 << TWEN) | (1 << TWIE) | (1 << TWINT);
if (TWSR == TW_BUS_ERROR) SETBIT(TWCR,(1 << TWSTO)); // reset TWI
TWAMR = 0; // all address bits matter
State = MS_Idle;
SoftAddress = EEPROM::GetByte(EEPROM_layout::TWI_SoftAddr_Ofs);
if (SoftAddress == 0xff) SoftAddress = 0;
SoftAddress &= 0xfe;
// TWAR = GeneralCallAddr | 1; // General call recognition and slave-receiver mode through address being 0
TWAR = SoftAddress | 1;
LastCmd = TWI_AAPCmd_Reserved;
}
// These used in all states, including user states, so make them available.
extern uint8_t TWIStatus;
extern uint8_t TWIData;
extern uint8_t TWIPrevData;
extern uint8_t TWIControl;
void ResetTWITransmit();
void ResetTWIReceive();
void ResetTWI();
}
#endif // __twi_aap_h__

/Modules/H_Bridge/HB1FET01A/SW/usart.cpp
0,0 → 1,65
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include "common.h"
#include "usart.h"
void USART0::SendData(uint8_t aData) {
while(!TESTBIT(UCSR0A,(1 << UDRE0)));
UDR0 = aData;
}
uint8_t USART0::ReceiveData() {
while(!TESTBIT(UCSR0A,(1 << RXC0)));
return UDR0;
}
// a non-exported helper function
static char MakeHexDigit(uint8_t aDigit) {
if (aDigit > 9) return 'a'+aDigit-10; else return '0'+aDigit;
}
void USART0::SendHexData(uint8_t aData) {
SendData(MakeHexDigit(aData >> 4));
SendData(MakeHexDigit(aData & 0x0f));
}
void USART0::SendHexData(uint16_t aData) {
SendHexData((uint8_t)(aData >> 8));
SendHexData((uint8_t)(aData & 0xff));
}
void USART0::SendHexData(uint32_t aData) {
SendHexData((uint16_t)(aData >> 16));
SendHexData((uint16_t)(aData & 0xffff));
}
void USART0::SendHexData(int16_t aData) {
if (aData < 0) {
SendData('-');
aData = -aData;
} else {
SendData(' ');
}
SendHexData((uint16_t)aData);
}
void USART0::SendHexData(int32_t aData) {
if (aData < 0) {
SendData('-');
aData = -aData;
} else {
SendData(' ');
}
SendHexData((uint32_t)aData);
}
void USART0::SendString(const char *aString) {
while(aString != 0) SendData(*(aString++));
}
void USART0::SendPGString(const char *aString) {
while(aString != 0) SendData(pgm_read_byte(aString++));
}

/Modules/H_Bridge/HB1FET01A/SW/usart.h
0,0 → 1,41
#ifndef __usart_h__
#define __usart_h__
#include "common.h"
namespace USART0 {
// setting = f_osc/16/BAUD - 1
const uint16_t baud9600_8MHz = 51;
const uint16_t baud19200_8MHz = 25;
const uint16_t baud57600_8MHz = 8;
const uint16_t baud19200_6_14MHz = 19;
const uint16_t baud38400_6_14MHz = 9;
const uint16_t baud19200_6_5MHz = 20;
const uint16_t baud38400_6_5MHz = 10;
const uint16_t baud57600_6_5MHz = 6;
inline uint8_t FastReceiveData();
inline void Init(uint16_t aBaudSetting) {
UBRR0 = aBaudSetting;
UCSR0A = 0;
FastReceiveData(); // Clear any possible pending input data (to avoid spurious IRQ
UCSR0B = (1 << RXCIE0) | (1 << RXEN0) | (1 << TXEN0);
UCSR0C = (1 << USBS0) | (3 << UCSZ00);
}
void SendData(uint8_t aData);
uint8_t ReceiveData();
inline void FastSendData(uint8_t aData) {
UDR0 = aData;
}
inline uint8_t FastReceiveData() {
return UDR0;
}
void SendHexData(uint8_t aData);
void SendHexData(uint16_t aData);
void SendHexData(uint32_t aData);
void SendHexData(int16_t aData);
void SendHexData(int32_t aData);
void SendString(const char *aString);
void SendPGString(const char *aString);
}
#endif // __usart_h__