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