Hoofdprogramma

->Dowloadlink<-

4:    
5:  #include <EEPROM.h>  
6:  #include "SerialCommand.h"  
7:  #include "EEPROMAnything.h"  
8:    
9:  #define SerialPort Serial1                     // Selecteren USB (Serial) of Bluetooth (Serial1)  
10:  SerialCommand sCmd(SerialPort);                   // The demo SerialCommand object  
11:    
12:  struct param_t                           // Tabel met parameters  
13:  {  
14:   bool start;  
15:   int speedl;  
16:   int speedr;  
17:   float kp;  
18:   float ki;  
19:   float kd;  
20:   unsigned long cycleTime;  
21:   int white[6];  
22:   int black[6];  
23:  } params;  
24:    
25:  // Global parameters  
26:  int channel[] = {A5, A4, A3, A2, A1, A0};              // Sensor inputs in array A0 -> A5  
27:  int value[6];  
28:  float normalized[6];  
29:  int positie;  
30:  int voltage;  
31:  float average;  
32:  unsigned long time, prevTime, delta, calculationTime;  
33:    
34:  double Output;  
35:  double errSum, lastInput;  
36:    
37:  // connect motor controller pins to Atmega32U4 digital pins  
38:  // motor one  
39:  int enA = 10;  
40:  int in1 = 13;  
41:  int in2 = 5;  
42:  // motor two  
43:  int enB = 9;  
44:  int in3 = 12;  
45:  int in4 = 4;  
46:    
47:  // start stop button  
48:  int button = 3;      // Number of input pin from button  
49:  int buttonstate = HIGH;  // Current state of the button  
50:  int buttonreading;    // Current reading of the button  
51:  int buttonprevious = LOW; // Previous reading of the button  
52:    
53:  void setup()  
54:  // Run once  
55:  {   
56:   attachInterrupt(0, onInterrupt, RISING);  
57:     
58:   //All commands  
59:   SerialPort.begin(115200);   
60:   sCmd.addCommand("start", onStart);  
61:   sCmd.addCommand("stop", onStop);  
62:   sCmd.addCommand("set", onSet);  
63:   sCmd.addCommand("debug", onDebug);  
64:   sCmd.addCommand("reset", onReset);  
65:   sCmd.addCommand("calibrate", onCalibrate);  
66:   sCmd.setDefaultHandler(onUnknownCommand);  
67:    
68:   //Read the parameters that are in the EEPROM  
69:   EEPROM_readAnything(0, params);  
70:   params.start = false;  
71:     
72:   time = micros();  
73:   prevTime = time;  
74:     
75:   SerialPort.println("Ready");  
76:    
77:   // set all the motor control pins to outputs  
78:   pinMode(enA, OUTPUT);  
79:   pinMode(enB, OUTPUT);  
80:   pinMode(in1, OUTPUT);  
81:   pinMode(in2, OUTPUT);  
82:   pinMode(in3, OUTPUT);  
83:   pinMode(in4, OUTPUT);  
84:    
85:   // start stop button  
86:   pinMode(button, INPUT);  
87:  }  
88:    
89:  void loop()  
90:  // Run in a loop  
91:  {   
92:   sCmd.readSerial(); // We don't do much, just process serial commands  
93:     
94:   time = micros();    // Tijd opvragen  
95:     
96:   if (time > prevTime) delta = time - prevTime;  
97:   else delta = 4294967295 - prevTime + time + 1;  
98:      
99:   if (delta > params.cycleTime)  
100:   {  
101:    prevTime = time;  
102:      
103:    for (int i = 0; i < 6; i++)                // Do this for all 6 sensors  
104:    {  
105:     value[i] = analogRead(channel[i]);  
106:       
107:     normalized[i] = value[i] - params.white[i];       // This is to convert the sensor values into a percentage  
108:     normalized[i] /= (params.black[i] - params.white[i]);  // This is to convert the sensor values into a percentage  
109:     normalized[i] *= 100;                  // This is to convert the sensor values into a percentage  
110:       
111:     // Parabool kuile  
112:    /*int pos = 0;  
113:     for (int i = 1; i < 6; i++)  
114:     {  
115:      if (normalized[i] > normalized[pos]) pos = i;  
116:     }  
117:       
118:     if ((pos > 0) && (pos < 5))  
119:     {  
120:      c = normalized[pos];  
121:      b = (normalized[pos + 1] - normalized[pos - 1]) / 2;  
122:      a = (normalized[pos + 1] + normalized[pos - 1] - 2 * normalized[pos]) / 2;  
123:        
124:      positie = -b / (2 * a);  
125:      positie *= 40;  
126:      positie += -100 + pos * 40;  
127:     }  
128:     else if (pos == 0) positie = -100;  
129:     else if (pos == 5) positie = 100;   
130:     */  
131:       
132:     // Gewogen gemiddelde  
133:     if ((normalized[0] > 90) && (normalized[1] < 10)) average = -100;  
134:     else if ((normalized[5] > 90) && (normalized[4] < 10)) average = 100;  
135:     else  
136:     {  
137:     average = ((100 * normalized[5]) + (60 * normalized[4]) + (20 * normalized [3]) + (-20 * normalized[2]) + (-60 * normalized[1]) + (-100 * normalized[0]));  
138:     average /= (normalized[5] + normalized[4] + normalized[3] + normalized[2] + normalized[1] + normalized[0]);  
139:     }  
140:       
141:     // PID  
142:       
143:     double error = - average;  
144:     errSum += error;  
145:     double dInput = (average - lastInput);  
146:       
147:     // PID value  
148:     Output = params.kp * error + params.ki * errSum - params.kd * dInput;  
149:       
150:     // Remember variable  
151:     lastInput = average;  
152:    
153:    
154:     // Motor controlling with PID output   
155:     int rightMotorSpeed = params.speedr + Output;  
156:     if (rightMotorSpeed < 0)  
157:       {rightMotorSpeed = 0;}  
158:       
159:     int leftMotorSpeed = params.speedl - Output;  
160:     if (leftMotorSpeed < 0)  
161:       {leftMotorSpeed = 0;}  
162:    
163:     // Start or stop with button  
164:     buttonreading = digitalRead(button);  
165:    
166:     if (buttonreading == HIGH && buttonprevious == LOW)  
167:     {  
168:     if (buttonstate == HIGH)  
169:     buttonstate = LOW;  
170:     else  
171:     buttonstate = HIGH;  
172:     }  
173:     buttonprevious = buttonreading;  
174:    
175:     if (buttonstate == 1)  
176:     {  
177:     digitalWrite(in1, HIGH); // vooruit bij A  
178:     digitalWrite(in2, LOW);  
179:     analogWrite(enA, rightMotorSpeed);  
180:     
181:     digitalWrite(in3, HIGH); // vooruit bij B  
182:     digitalWrite(in4, LOW);  
183:     analogWrite(enB, leftMotorSpeed);  
184:     }  
185:     else if (buttonstate == 0)  
186:     {  
187:     digitalWrite(in1, LOW);  
188:     digitalWrite(in2, LOW);  
189:     analogWrite(enA, 0);  
190:     
191:     digitalWrite(in3, LOW);  
192:     digitalWrite(in4, LOW);  
193:     analogWrite(enB, 0);  
194:     }  
195:    
196:     // Read battery tension  
197:     ADMUX &= B11100000; //MUX4..0  
198:     ADMUX |= B00000010; //MUX4..0  
199:     ADCSRB |= B00100000; //MUX5  
200:    
201:     ADCSRA |= B11000000; //set ADEN en ADSC  
202:     while(!(ADCSRA & (1<<ADIF))); //wacht op ADIF  
203:     ADCSRA|=(1<<ADIF); //reset ADIF  
204:    
205:     voltage = ADC;  
206:       
207:    }  
208:   }  
209:     
210:   unsigned long difference = micros() - time;  
211:   if (difference > calculationTime) calculationTime = difference;   
212:    
213:  }  
214:    
215:  void onUnknownCommand(char *command)  
216:  {  
217:   SerialPort.print("unknown command: \"");  
218:   SerialPort.print(command);  
219:   SerialPort.println("\"");  
220:  }  
221:    
222:  void onSet()                                   // In deze functie kan kan je de parameters verandere  
223:  {                                         // Typ in de serial monitor: set speed, set kp or set cycle  
224:   char* parameter = sCmd.next();  
225:   char* value = sCmd.next();  
226:     
227:   if (strcmp(parameter, "speedr") == 0) params.speedr = atoi(value);       //strcmp gaat 2strings met elkaar vergelijken  
228:   else if (strcmp(parameter, "speedl") == 0) params.speedl = atoi(value);      
229:   else if (strcmp(parameter, "kp") == 0) params.kp = atof(value);  
230:   else if (strcmp(parameter, "ki") == 0)  
231:   {  
232:    params.ki = atof(value);  
233:    params.ki *= params.cycleTime;  
234:    params.ki /= 1000000;                            //1000000 --> cycle time is in microseconden  
235:   }  
236:   else if (strcmp(parameter, "kd") == 0)  
237:   {  
238:    params.kd = atof(value);  
239:    params.kd /= params.cycleTime;  
240:    params.kd *= 1000000;  
241:   }  
242:   else if (strcmp(parameter, "cycle") == 0)  
243:   {  
244:    float ratio = params.cycleTime;  
245:    params.cycleTime = atol(value);  
246:    ratio /= params.cycleTime;  
247:    
248:    params.ki /= ratio;  
249:    params.kd /= ratio;  
250:   }  
251:     
252:   EEPROM_writeAnything(0, params);             // Schrijf de veranderde waarden naar de EEPROM  
253:  }  
254:    
255:  void onStart()  
256:  {  
257:   //params.start = true;  
258:   buttonstate = 1;  
259:  }  
260:    
261:  void onStop()  
262:  {  
263:   //params.start = false;  
264:   buttonstate = 0;  
265:  }  
266:    
267:  void onDebug()                       // Debug de data na het typen van het commando debug  
268:  {  
269:   //parameters  
270:   SerialPort.print("Right speed: ");  
271:   SerialPort.println(params.speedr);  
272:   SerialPort.print("Left speed: ");  
273:   SerialPort.println(params.speedl);  
274:   SerialPort.print("Kp: ");  
275:   SerialPort.println(params.kp);   
276:   SerialPort.print("Ki: ");  
277:   SerialPort.println(params.ki * 1000000 / params.cycleTime);  
278:   SerialPort.print("Kd: ");  
279:   SerialPort.println(params.kd * params.cycleTime / 1000000);  
280:   //SerialPort.println();  
281:     
282:   //cycle times  
283:   SerialPort.print("Cycle time: ");  
284:   SerialPort.println(params.cycleTime);  
285:   SerialPort.print("Calculation time: ");  
286:   SerialPort.println(calculationTime);  
287:   //SerialPort.println();  
288:   calculationTime = 0;         // reset calculation time  
289:     
290:   //running  
291:   SerialPort.print("Running: ");  
292:   SerialPort.println(buttonstate);   
293:     
294:   //sensorwaarden  
295:   SerialPort.println();  
296:   SerialPort.println("--DEBUG SENSOR--");  
297:   SerialPort.print("Sensor values: ");  
298:   for (int i = 0; i < 6; i++)  
299:   {  
300:     SerialPort.print(analogRead(channel[i]));  
301:     SerialPort.print(" ");  
302:   }  
303:   SerialPort.println();  
304:   SerialPort.println();  
305:   SerialPort.println("Calibrated white values:");  
306:   for (int i = 0; i < 6; i++)  
307:   {  
308:    SerialPort.print(params.white[i]);  
309:    SerialPort.print(" ");  
310:   }  
311:   SerialPort.println();  
312:   SerialPort.println();  
313:     
314:   SerialPort.println("Calibrated black values:");  
315:   for (int i = 0; i < 6; i++)  
316:   {  
317:    SerialPort.print(params.black[i]);  
318:    SerialPort.print(" ");  
319:   }  
320:   SerialPort.println();  
321:   SerialPort.println();  
322:     
323:   SerialPort.println("Normalized values:");  
324:   for (int i = 0; i < 6; i++)  
325:   {  
326:    SerialPort.print(normalized[i]);  
327:    SerialPort.print(" ");  
328:   }  
329:   SerialPort.println();  
330:   SerialPort.println();  
331:     
332:   SerialPort.print("Position:");  
333:   SerialPort.print(" ");  
334:   SerialPort.print(average);  
335:   SerialPort.println();  
336:   SerialPort.println();  
337:    
338:   // De batterij spanning weergeven  
339:   float voltagedisplay = (voltage * (5.0 / 1025.0)) * 2;  
340:   SerialPort.print("Battery tension: ");  
341:   SerialPort.print(voltagedisplay);  
342:   SerialPort.print(" V");  
343:   SerialPort.println();  
344:   SerialPort.println();  
345:  }  
346:    
347:  void onReset()   // Reset alle parameters  
348:  {  
349:   SerialPort.print("Resetting parameters... ");  
350:   EEPROM_resetAnything(0, params);  
351:   EEPROM_readAnything(0, params);   
352:   SerialPort.println("done");  
353:  }  
354:    
355:  void onCalibrate()  // Calibreer wit en zwart, tijdens kalibreren het karretje van een wit vlak naar een zwart vlak duwen  
356:  {  
357:   char *arg = sCmd.next();  
358:   if (strcmp (arg, "white") <= 0)  
359:   {  
360:    params.white[0] = 1023;  
361:    params.white[1] = 1023;  
362:    params.white[2] = 1023;  
363:    params.white[3] = 1023;  
364:    params.white[4] = 1023;  
365:    params.white[5] = 1023;  
366:      
367:     SerialPort.print("--CALIBRATING--");  
368:     SerialPort.println("");  
369:     SerialPort.print("Calibrating white... ");  
370:     for (int i = 0; i < 1000; i++) // We gaan 1000 keer "scannen"  
371:     {  
372:      for (int i = 0; i < 6; i++) // Voor de 6 sensotren  
373:      {  
374:       int value_white = analogRead(channel[i]);  
375:       if (value_white < params.white[i])  
376:        {  
377:         params.white[i] = value_white; // De laagst gemeten waarde van wit in het geheugen laden  
378:        }  
379:      }  
380:     }  
381:    EEPROM_writeAnything(0, params);  
382:    SerialPort.print("done");  
383:    SerialPort.println();  
384:    SerialPort.println();  
385:   }  
386:     
387:   if (strcmp (arg, "black") >= 0)  
388:   {   
389:    params.black[0] = 0;  
390:    params.black[1] = 0;  
391:    params.black[2] = 0;  
392:    params.black[3] = 0;  
393:    params.black[4] = 0;  
394:    params.black[5] = 0;  
395:    
396:    SerialPort.print("--CALIBRATING--");  
397:    SerialPort.println("");  
398:    SerialPort.print("Calibrating black... ");  
399:    for (int i = 0; i < 1000; i++) // We gaan 1000 keer "scannen"  
400:    {  
401:     for (int i = 0; i < 6; i++) // Voor de 6 sensoren  
402:     {  
403:      int value_black = analogRead(channel[i]);  
404:      if (value_black > params.black[i])  
405:      {  
406:       params.black[i] = value_black; // De hoogst geleten waarden van zwart in het geheugen plaatsen  
407:      }  
408:     }  
409:    }  
410:    EEPROM_writeAnything(0, params);  
411:    SerialPort.print("done");  
412:    SerialPort.println();  
413:    SerialPort.println();  
414:   }  
415:  }  
416:    
417:  void onInterrupt()    
418:  {  
419:   params.start = not params.start;  //Typ start in de seriele monitor om het autootje te laten rijden  
420:  }