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