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