Some adjustments
[UnoTest.git] / UnoTest.cpp
1 #include "uart.h"
2 #include "arduino.h"
3 #include "PWM.h"
4 #include "PWMDrive.h"
5
6 static const int CONTROLINPUT = A0;
7 static const int TEMPADC = A5;
8 static const int TEMPINPUT[] = { 0, 1, 2, 6, 7 };   // External mux index
9 static const int VOLTSINPUT[] = { A4, A3 };
10 static const int CURRENTINPUT[] = { A2 };
11
12 static const int MOTOROUTPUT = 3;
13 static const int MOTOROFF = 0;
14 static const unsigned char CONSTANTMARK = 9;
15
16 #define CONTACTORSET DDRD
17 #define CONTACTORPORT PORTD
18 static const int CONTACTORPIN = 4;
19
20 static const int TEMP45C = (45 - 21) * 76 / 10;
21 static const int TEMP80C = (80 - 21) * 76 / 10;
22 static const int TEMP70C = (70 - 21) * 76 / 10;
23 static const int TEMP112C = (112 - 21) * 76 / 10;
24
25 static const int DRIVERVOLTSLIMIT = 230;    // approx 25v
26
27 static const int CURRENTLIMIT = 30; // approx 300A (with factor of 2 applied)
28 // static const int CURRENTLIMIT = 60; // approx 300A
29 // static const int CURRENTLIMIT = 40; // approx 200A
30 // static const int CURRENTLIMIT = 20; // approx 100A
31
32 //static const int HALLADCOFFSET = 510;   // Hall offset is observed as 509.6 by graphing
33 static const int HALLADCOFFSET = 501;   // Hall offset is observed as 509.6 by graphing
34
35 static const int TEMPHIGH = 200;
36 static const int TEMPSAFE = 100;
37
38 static const int HITEMP = 999;
39 static const int PERIOD1 = 100;
40 static const int PERIOD2 = 1000;
41
42 static const int TEMPCOUNT = sizeof (TEMPINPUT) / sizeof (int);
43 static const int VOLTSCOUNT = sizeof (VOLTSINPUT) / sizeof (int);
44 static const int CURRENTCOUNT = sizeof (CURRENTINPUT) / sizeof (int);
45
46 static int FixedMarkSpace = 0;
47 static unsigned short ControlInput = 0;
48 static int HVOn = 1;
49 static int WithinLimits = 1;
50 static int BackOff = 0;
51 static unsigned short LastTempInput[TEMPCOUNT];
52 static unsigned short TempInput[TEMPCOUNT];
53 static unsigned short VoltsInput[VOLTSCOUNT];
54 static unsigned short CurrentInput[CURRENTCOUNT];
55 static int MotorOutput = 0;
56 static int period = 0;
57 static bool LimitCurrent = true;
58
59 static int SensorISRCount = 0;
60
61 //  16 000 000 Hz / 8 = 2 000 000 Hz
62 static const unsigned char TIMER0PRESCALE = CSx_0008;
63 // 2 000 000 Hz / 78 (77 + 1) = 25 641 Hz (39uS period)
64 static const unsigned char TIMER0RESET = 77;
65 // PWM = 25 641 Hz / 256 = 100.16 Hz
66
67 static unsigned int Ticker;
68
69 static char LastCharacter;
70
71 const char BANNER[] = "Graham's Arduino Uno Terminal Server\n";
72
73 void Receive (char Character)
74 {
75     LastCharacter = Character;
76
77     static unsigned char timerprescale = 0;
78
79     if (Character == '/')
80         FixedMarkSpace = 0;
81
82     if (Character >= '0' && Character <= '9')
83         FixedMarkSpace = Character - '0' + 1;
84
85     if (Character == '+' && FixedMarkSpace < 10)
86         FixedMarkSpace++;
87
88     if (Character == '-' && FixedMarkSpace > 1)
89         FixedMarkSpace--;
90
91     if (Character == 's')
92         timerprescale = CS2_1024;
93
94     if (Character == 'm')
95         timerprescale = CS2_0008;
96
97     if (Character == 'f')
98         timerprescale = CS2_NOSCALE;
99
100     if (Character == 'I')
101         LimitCurrent = false;
102
103     if (Character == 'i')
104         LimitCurrent = true;
105
106     if (Character == 'h')
107     {
108         timerprescale = TCCR2B & 0b00000111;
109         TCCR2A |= _BV (WGM21);
110     }
111
112     if (Character == 'l')
113     {
114         timerprescale = TCCR2B & 0b00000111;
115         TCCR2A &= ~_BV (WGM21);
116     }
117
118     if (timerprescale)
119     {
120 // Set prescaler and start PWM
121         TCCR2B &= ~0b00000111;
122         TCCR2B |= timerprescale;
123     }
124
125     if (Character == 'c')
126         HVOn = 0;
127
128     if (Character == 'C')
129         HVOn = 1;
130
131     if (Character == 'd')
132     {
133         switch (period)
134         {
135             case 0:
136                 period = PERIOD1;
137                 break;
138             case PERIOD1:
139                 period = PERIOD2;
140                 break;
141             case PERIOD2:
142                 period = 0;
143                 break;
144             default:
145                 period = 0;
146                 break;
147         }
148     }
149 }
150
151 void UARTReceive (char Character)
152 {
153     Receive (Character);
154 }
155
156 static const unsigned char PORTDSENSORMASK = 0b10000000;
157
158 void setup ()
159 {
160     puts ("Grahams Motor Test, " __DATE__ " (" __TIME__ ")");
161
162 // Counter reset level
163     OCR0A = TIMER0RESET;
164
165 // Enable interrupt on timer 0 output compare match
166     TIMSK0 = TIMSKxOCIEA;
167 // Enable interrupt on timer 2 overflow
168     TIMSK2 = TIMSKxTOIE;
169
170 // CTC mode
171     TCCR0A = TCCRxA_MODE_2_CTC;
172     TCCR0B = TCCRxB_MODE_2_CTC;
173
174 // Set prescaler and start PWM
175     TCCR0B |= TIMER0PRESCALE;
176
177 // Set prescaler and start PWM
178     TCCR2B &= ~0b00000111;
179     TCCR2B |= CS2_1024;
180
181     CONTACTORSET |= (1 << CONTACTORPIN);
182
183 // PORTD pin 7
184     PCICR |= (1 << PCIE2);
185     PCMSK2 |= PORTDSENSORMASK;
186
187     DDRB = 0b00000111;
188 }
189
190 static inline int TempInC (int Input)
191 {
192 // For 1k lowside: 45C < Temp ~ +-1C < 112C
193     if (Input < TEMP45C)
194         return 0;
195     else if (Input > TEMP112C)
196         return HITEMP;
197     else
198         return (Input * 10 / 76) + 21;
199 }
200
201 static inline unsigned short HallOffset (unsigned short Input)
202 {
203 #if 0
204     if (Input > HALLADCOFFSET)
205         return Input - HALLADCOFFSET;   // Return forward EMF
206     else
207         return 0;               // Not returning the back EMF
208 #else
209     if (Input < HALLADCOFFSET)
210         return HALLADCOFFSET - Input;   // Return forward EMF
211     else
212         return 0;               // Not returning the back EMF
213 #endif
214 }
215
216 // This calculated as: field(gauss) = (adc offset) / 512 * 670
217 //  then current(amps) = field(gauss) * 4
218 static inline unsigned short CurrentSensed (unsigned short Input)
219 {
220     return (HallOffset (Input) * 670) >> (9 - 2 + 1);   // >>9 is divide by 512, and <<2 is * 4 for current (+1 to allow for observed values)
221 }
222
223 static void PWMWrite (const unsigned char channel, const unsigned char pwm)
224 {
225     if (channel < 8)
226         PWMMask = (1 << channel);
227
228 #if 0
229     Mark = (pwm + 1) >> 1;
230 #elif 1
231     Mark = pwm;
232 #else
233     if (pwm > 0)
234         Mark = CONSTANTMARK;
235     else
236         Mark = 0;
237 #endif
238     Space = 255 - pwm;
239 }
240
241 static volatile bool Reading = false;
242
243 void Read ()
244 {
245     if (Reading)
246         return;
247
248     Reading = true;
249
250     CurrentInput[0] = analogRead (CURRENTINPUT[0]);
251
252     if (FixedMarkSpace)
253         ControlInput = ((FixedMarkSpace - 1) * 1023) / 9;
254     else
255         ControlInput = analogRead (CONTROLINPUT);
256
257     for (unsigned char count = 0; count < TEMPCOUNT; count++)
258     {
259         LastTempInput[count] = TempInput[count];
260         PORTB = TEMPINPUT[count];
261         TempInput[count] = analogRead (TEMPADC);
262
263         if (LastTempInput[count] > 0
264             && abs ((signed short) LastTempInput[count] - (signed short) TempInput[count]) > 10)
265             break;
266     }
267
268     for (unsigned char count = 0; count < VOLTSCOUNT; count++)
269         VoltsInput[count] = analogRead (VOLTSINPUT[count]);
270
271     Reading = false;
272 }
273
274 void loop ()
275 {
276 #if 0
277     Read ();
278 #endif
279     int TempLow = 1;
280     int TempHigh = 0;
281
282     for (unsigned char count = 0; count < TEMPCOUNT; count++)
283     {
284         if (TempInput[count] >= TEMPSAFE)
285             TempLow = 0;
286
287         if (TempInput[count] > TEMPHIGH)
288             TempHigh = 1;
289     }
290
291     if (TempLow)
292         WithinLimits = 1;
293
294     if (TempHigh)
295         WithinLimits = 0;
296
297     if (VoltsInput[0] > DRIVERVOLTSLIMIT)
298         WithinLimits = 0;
299
300     if (LimitCurrent && (HallOffset (CurrentInput[0]) > CURRENTLIMIT))
301         BackOff += 10;
302
303     if (ControlInput == 0 && BackOff > 0)   // Reset when control is zeroed
304         BackOff = 0;
305
306     if (HVOn && WithinLimits)
307         CONTACTORPORT |= (1 << CONTACTORPIN);
308     else
309         CONTACTORPORT &= ~(1 << CONTACTORPIN);
310
311     MotorOutput = (ControlInput >> 2) - BackOff;
312
313     if (MotorOutput < 0)
314         MotorOutput = 0;
315
316 #if 1
317     analogWrite (MOTOROUTPUT, MotorOutput);
318 #else
319     PWMWrite (MOTOROUTPUT, MotorOutput);
320 #endif
321
322     printf ("Control=%4d Temp ", ControlInput);
323
324     for (unsigned char count = 0; count < TEMPCOUNT; count++)
325         printf ("%u=%4u(%3uC) ", count, TempInput[count], TempInC (TempInput[count]));
326
327     printf ("Volts ");
328
329     for (unsigned char count = 0; count < VOLTSCOUNT; count++)
330         printf ("%u=%4u(%3uv) ", count, VoltsInput[count], (((unsigned short) VoltsInput[count] * 115U) >> 10));
331
332     printf ("Amps ");
333
334     for (unsigned char count = 0; count < CURRENTCOUNT; count++)
335         printf ("%u=%4u(%3uA) ", count, CurrentInput[count], CurrentSensed (CurrentInput[count]));
336
337     printf ("Motor=%3d Sensor=%3u Ticker=%4u %c %c\n", MotorOutput, SensorISRCount, Ticker,
338             (HVOn && WithinLimits) ? '!' : '_', LastCharacter ? LastCharacter : ' ');
339
340     LastCharacter = '\0';
341
342     SensorISRCount = 0;
343     Ticker = 0;
344
345     if (period == PERIOD1)
346         delay (PERIOD1);
347     else if (period == PERIOD2)
348         delay (PERIOD2);
349 }
350
351 // Timer 0 interrupt
352 ISR (TIMER0_COMPA_vect)
353 {
354     sei ();
355
356     Ticker++;
357
358     ShaftMoveTicks++;
359     PWMDriveMove ();
360 }
361
362 // Timer 2 interrupt
363 ISR (TIMER2_OVF_vect)
364 {
365     sei ();
366 #if 1
367     Read ();
368 #endif
369 }
370
371 // Port D
372 ISR (PCINT2_vect)
373 {
374     sei ();
375     if (PIND & PORTDSENSORMASK) // Only rising edge (as the magnet leaves the sensor)
376         SensorISRCount++;
377 }