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