Team 5 AUH
/
Team5_AUH_robot
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
Team5_AUH_robot
by 
Kasper Martensen
main_copy.cpp@17:d6e8234ffc91, 2020-11-09 (annotated)
- Committer:
- mathiasbergma
- Date:
- Mon Nov 09 15:13:17 2020 +0000
- Revision:
- 17:d6e8234ffc91
- Parent:
- 16:b3617369a00c
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| mathiasbergma | 16:b3617369a00c | 1 | #include "mbed.h" |
| mathiasbergma | 16:b3617369a00c | 2 | #include "m3pi.h" |
| mathiasbergma | 16:b3617369a00c | 3 | //Mathias tester |
| mathiasbergma | 16:b3617369a00c | 4 | |
| mathiasbergma | 16:b3617369a00c | 5 | m3pi m3pi; |
| mathiasbergma | 16:b3617369a00c | 6 | |
| mathiasbergma | 16:b3617369a00c | 7 | // Minimum and maximum motor speeds |
| mathiasbergma | 16:b3617369a00c | 8 | #define MAX 1.0 |
| mathiasbergma | 16:b3617369a00c | 9 | #define MIN 0 |
| mathiasbergma | 16:b3617369a00c | 10 | |
| mathiasbergma | 16:b3617369a00c | 11 | // PID terms |
| mathiasbergma | 16:b3617369a00c | 12 | #define P_TERM 2.5 |
| mathiasbergma | 16:b3617369a00c | 13 | #define I_TERM 0.007 |
| mathiasbergma | 16:b3617369a00c | 14 | #define D_TERM 16 //Ændret fra 4 |
| mathiasbergma | 16:b3617369a00c | 15 | |
| mathiasbergma | 16:b3617369a00c | 16 | //Watt hour measurements |
| mathiasbergma | 16:b3617369a00c | 17 | #define AVERAGE 30 //No. of measurements |
| mathiasbergma | 16:b3617369a00c | 18 | #define AMPREL 24.07 //amps |
| mathiasbergma | 16:b3617369a00c | 19 | #define VOLTREL 8.984830805 |
| mathiasbergma | 16:b3617369a00c | 20 | #define SECDEC 1/3600 |
| mathiasbergma | 16:b3617369a00c | 21 | #define AIOFFSET 0.45900 //Zeroing of amp (0 amp = 2.5V) |
| mathiasbergma | 16:b3617369a00c | 22 | |
| mathiasbergma | 16:b3617369a00c | 23 | AnalogIn ain_1(A0); //ADC for amps |
| mathiasbergma | 16:b3617369a00c | 24 | AnalogIn ain_2(A1); //ADC for volts |
| mathiasbergma | 16:b3617369a00c | 25 | Timer t; //Used for Watt hour measurement |
| mathiasbergma | 16:b3617369a00c | 26 | |
| mathiasbergma | 16:b3617369a00c | 27 | int main() |
| mathiasbergma | 16:b3617369a00c | 28 | { |
| mathiasbergma | 16:b3617369a00c | 29 | |
| mathiasbergma | 16:b3617369a00c | 30 | t.start(); //Starts timer function |
| mathiasbergma | 16:b3617369a00c | 31 | float analogAmp; //Analog amp reading |
| mathiasbergma | 16:b3617369a00c | 32 | float analogVolt; //Analog volt reading |
| mathiasbergma | 16:b3617369a00c | 33 | float voltreading[AVERAGE]; //Volt array |
| mathiasbergma | 16:b3617369a00c | 34 | float ampreading[AVERAGE]; //Amp array |
| mathiasbergma | 16:b3617369a00c | 35 | float voltmean; |
| mathiasbergma | 16:b3617369a00c | 36 | float ampsmean; |
| mathiasbergma | 16:b3617369a00c | 37 | float wh = 0; |
| mathiasbergma | 16:b3617369a00c | 38 | int measno = 0; |
| mathiasbergma | 16:b3617369a00c | 39 | |
| mathiasbergma | 16:b3617369a00c | 40 | for (int i = 1; i <= AVERAGE; i++) { |
| mathiasbergma | 16:b3617369a00c | 41 | ampreading[i] = 0; |
| mathiasbergma | 16:b3617369a00c | 42 | voltreading[i] = 0; |
| mathiasbergma | 16:b3617369a00c | 43 | } |
| mathiasbergma | 16:b3617369a00c | 44 | |
| mathiasbergma | 16:b3617369a00c | 45 | m3pi.locate(0,1); |
| mathiasbergma | 16:b3617369a00c | 46 | m3pi.printf("Line PID"); |
| mathiasbergma | 16:b3617369a00c | 47 | |
| mathiasbergma | 16:b3617369a00c | 48 | wait(2.0); |
| mathiasbergma | 16:b3617369a00c | 49 | |
| mathiasbergma | 16:b3617369a00c | 50 | m3pi.sensor_auto_calibrate(); |
| mathiasbergma | 16:b3617369a00c | 51 | float right; |
| mathiasbergma | 16:b3617369a00c | 52 | float left; |
| mathiasbergma | 16:b3617369a00c | 53 | float current_pos_of_line = 0.0; |
| mathiasbergma | 16:b3617369a00c | 54 | float previous_pos_of_line = 0.0; |
| mathiasbergma | 16:b3617369a00c | 55 | float derivative,proportional,integral = 0; |
| mathiasbergma | 16:b3617369a00c | 56 | float power; |
| mathiasbergma | 16:b3617369a00c | 57 | float speed = MAX; |
| mathiasbergma | 16:b3617369a00c | 58 | |
| mathiasbergma | 16:b3617369a00c | 59 | |
| mathiasbergma | 16:b3617369a00c | 60 | while (1) { |
| mathiasbergma | 16:b3617369a00c | 61 | |
| mathiasbergma | 16:b3617369a00c | 62 | ampsmean = 0; //Zeroing mean amps before next calculation |
| mathiasbergma | 16:b3617369a00c | 63 | |
| mathiasbergma | 16:b3617369a00c | 64 | analogAmp = ain_1; |
| mathiasbergma | 16:b3617369a00c | 65 | analogAmp -= AIOFFSET; |
| mathiasbergma | 16:b3617369a00c | 66 | ampreading[measno] = analogAmp*AMPREL; |
| mathiasbergma | 16:b3617369a00c | 67 | |
| mathiasbergma | 16:b3617369a00c | 68 | for (int i = 0; i < AVERAGE; i++) { |
| mathiasbergma | 16:b3617369a00c | 69 | ampsmean += ampreading[i]; |
| mathiasbergma | 16:b3617369a00c | 70 | voltmean += voltreading[i]; |
| mathiasbergma | 16:b3617369a00c | 71 | } |
| mathiasbergma | 16:b3617369a00c | 72 | ampsmean /= AVERAGE; |
| mathiasbergma | 16:b3617369a00c | 73 | voltmean /= AVERAGE; |
| mathiasbergma | 16:b3617369a00c | 74 | |
| mathiasbergma | 16:b3617369a00c | 75 | if (t > 1.0) { //Zeroing timer |
| mathiasbergma | 16:b3617369a00c | 76 | wh += ampsmean * voltmean * SECDEC; |
| mathiasbergma | 16:b3617369a00c | 77 | t.stop(); |
| mathiasbergma | 16:b3617369a00c | 78 | t.reset(); |
| mathiasbergma | 16:b3617369a00c | 79 | t.start(); |
| mathiasbergma | 16:b3617369a00c | 80 | } |
| mathiasbergma | 16:b3617369a00c | 81 | |
| mathiasbergma | 16:b3617369a00c | 82 | analogVolt = ain_2; //distance between two ADC for stability |
| mathiasbergma | 16:b3617369a00c | 83 | voltreading[measno] = analogVolt * VOLTREL; |
| mathiasbergma | 16:b3617369a00c | 84 | m3pi.locate(1,0); |
| mathiasbergma | 16:b3617369a00c | 85 | m3pi.printf("%.3f",wh); |
| mathiasbergma | 16:b3617369a00c | 86 | |
| mathiasbergma | 16:b3617369a00c | 87 | // Get the position of the line. |
| mathiasbergma | 16:b3617369a00c | 88 | current_pos_of_line = m3pi.line_position(); |
| mathiasbergma | 16:b3617369a00c | 89 | proportional = current_pos_of_line; |
| mathiasbergma | 16:b3617369a00c | 90 | |
| mathiasbergma | 16:b3617369a00c | 91 | // Compute the derivative |
| mathiasbergma | 16:b3617369a00c | 92 | derivative = current_pos_of_line - previous_pos_of_line; |
| mathiasbergma | 16:b3617369a00c | 93 | |
| mathiasbergma | 16:b3617369a00c | 94 | // Compute the integral |
| mathiasbergma | 16:b3617369a00c | 95 | integral += proportional; |
| mathiasbergma | 16:b3617369a00c | 96 | |
| mathiasbergma | 16:b3617369a00c | 97 | // Remember the last position. |
| mathiasbergma | 16:b3617369a00c | 98 | previous_pos_of_line = current_pos_of_line; |
| mathiasbergma | 16:b3617369a00c | 99 | |
| mathiasbergma | 16:b3617369a00c | 100 | // Compute the power |
| mathiasbergma | 16:b3617369a00c | 101 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
| mathiasbergma | 16:b3617369a00c | 102 | |
| mathiasbergma | 16:b3617369a00c | 103 | // Compute new speeds |
| mathiasbergma | 16:b3617369a00c | 104 | right = speed+power; |
| mathiasbergma | 16:b3617369a00c | 105 | left = speed-power; |
| mathiasbergma | 16:b3617369a00c | 106 | |
| mathiasbergma | 16:b3617369a00c | 107 | // limit checks |
| mathiasbergma | 16:b3617369a00c | 108 | if (right < MIN) |
| mathiasbergma | 16:b3617369a00c | 109 | right = MIN; |
| mathiasbergma | 16:b3617369a00c | 110 | else if (right > MAX) |
| mathiasbergma | 16:b3617369a00c | 111 | right = MAX; |
| mathiasbergma | 16:b3617369a00c | 112 | |
| mathiasbergma | 16:b3617369a00c | 113 | if (left < MIN) |
| mathiasbergma | 16:b3617369a00c | 114 | left = MIN; |
| mathiasbergma | 16:b3617369a00c | 115 | else if (left > MAX) |
| mathiasbergma | 16:b3617369a00c | 116 | left = MAX; |
| mathiasbergma | 16:b3617369a00c | 117 | |
| mathiasbergma | 16:b3617369a00c | 118 | // set speed |
| mathiasbergma | 16:b3617369a00c | 119 | m3pi.left_motor(left); |
| mathiasbergma | 16:b3617369a00c | 120 | m3pi.right_motor(right); |
| mathiasbergma | 16:b3617369a00c | 121 | |
| mathiasbergma | 16:b3617369a00c | 122 | measno++; //Used for average amps & volts |
| mathiasbergma | 16:b3617369a00c | 123 | |
| mathiasbergma | 16:b3617369a00c | 124 | if (measno >= AVERAGE) { |
| mathiasbergma | 16:b3617369a00c | 125 | measno = 0; |
| mathiasbergma | 16:b3617369a00c | 126 | } |
| mathiasbergma | 16:b3617369a00c | 127 | voltmean = 0; //Zeroing mean volts before next calculation |
| mathiasbergma | 16:b3617369a00c | 128 | |
| mathiasbergma | 16:b3617369a00c | 129 | } |
| mathiasbergma | 16:b3617369a00c | 130 | } |
