Team 5 AUH / Mbed 2 deprecated 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@2:4f8930f623ca, 2020-11-09 (annotated)

Committer:
mathiasbergma
Date:
Mon Nov 09 14:12:30 2020 +0000
Revision:
2:4f8930f623ca
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mathiasbergma 2:4f8930f623ca 1 #include "mbed.h"
mathiasbergma 2:4f8930f623ca 2 #include "m3pi.h"
mathiasbergma 2:4f8930f623ca 3
mathiasbergma 2:4f8930f623ca 4
mathiasbergma 2:4f8930f623ca 5 m3pi m3pi;
mathiasbergma 2:4f8930f623ca 6
mathiasbergma 2:4f8930f623ca 7 // Minimum and maximum motor speeds
mathiasbergma 2:4f8930f623ca 8 #define MAX 1.0
mathiasbergma 2:4f8930f623ca 9 #define MIN 0
mathiasbergma 2:4f8930f623ca 10
mathiasbergma 2:4f8930f623ca 11 // PID terms
mathiasbergma 2:4f8930f623ca 12 #define P_TERM 0.9
mathiasbergma 2:4f8930f623ca 13 #define I_TERM 0.000
mathiasbergma 2:4f8930f623ca 14 #define D_TERM 0 //Ændret fra 4
mathiasbergma 2:4f8930f623ca 15
mathiasbergma 2:4f8930f623ca 16 //Watt hour measurements
mathiasbergma 2:4f8930f623ca 17 #define AVERAGE 30 //No. of measurements
mathiasbergma 2:4f8930f623ca 18 #define AMPREL 24.07 //amps
mathiasbergma 2:4f8930f623ca 19 #define VOLTREL 8.984830805
mathiasbergma 2:4f8930f623ca 20 #define SECDEC 1/3600
mathiasbergma 2:4f8930f623ca 21 #define AIOFFSET 0.45900 //Zeroing of amp (0 amp = 2.5V)
mathiasbergma 2:4f8930f623ca 22
mathiasbergma 2:4f8930f623ca 23 AnalogIn ain_1(A0); //ADC for amps
mathiasbergma 2:4f8930f623ca 24 AnalogIn ain_2(A1); //ADC for volts
mathiasbergma 2:4f8930f623ca 25 Timer t; //Used for Watt hour measurement
mathiasbergma 2:4f8930f623ca 26
mathiasbergma 2:4f8930f623ca 27 int main() {
mathiasbergma 2:4f8930f623ca 28
mathiasbergma 2:4f8930f623ca 29 t.start(); //Starts timer function
mathiasbergma 2:4f8930f623ca 30 float analogAmp; //Analog amp reading
mathiasbergma 2:4f8930f623ca 31 float analogVolt; //Analog volt reading
mathiasbergma 2:4f8930f623ca 32 float voltreading[AVERAGE]; //Volt array
mathiasbergma 2:4f8930f623ca 33 float ampreading[AVERAGE]; //Amp array
mathiasbergma 2:4f8930f623ca 34 float voltmean;
mathiasbergma 2:4f8930f623ca 35 float ampsmean;
mathiasbergma 2:4f8930f623ca 36 float wh = 0;
mathiasbergma 2:4f8930f623ca 37 int measno = 0;
mathiasbergma 2:4f8930f623ca 38
mathiasbergma 2:4f8930f623ca 39 for (int i = 1; i <= AVERAGE; i++)
mathiasbergma 2:4f8930f623ca 40 {
mathiasbergma 2:4f8930f623ca 41 ampreading[i] = 0;
mathiasbergma 2:4f8930f623ca 42 voltreading[i] = 0;
mathiasbergma 2:4f8930f623ca 43 }
mathiasbergma 2:4f8930f623ca 44
mathiasbergma 2:4f8930f623ca 45 m3pi.locate(0,1);
mathiasbergma 2:4f8930f623ca 46 m3pi.printf("Line PID");
mathiasbergma 2:4f8930f623ca 47
mathiasbergma 2:4f8930f623ca 48 wait(2.0);
mathiasbergma 2:4f8930f623ca 49
mathiasbergma 2:4f8930f623ca 50 m3pi.sensor_auto_calibrate();
mathiasbergma 2:4f8930f623ca 51 float right;
mathiasbergma 2:4f8930f623ca 52 float left;
mathiasbergma 2:4f8930f623ca 53 float current_pos_of_line = 0.0;
mathiasbergma 2:4f8930f623ca 54 float previous_pos_of_line = 0.0;
mathiasbergma 2:4f8930f623ca 55 float derivative,proportional,integral = 0;
mathiasbergma 2:4f8930f623ca 56 float power;
mathiasbergma 2:4f8930f623ca 57 float speed = MAX;
mathiasbergma 2:4f8930f623ca 58
mathiasbergma 2:4f8930f623ca 59
mathiasbergma 2:4f8930f623ca 60 while (1) {
mathiasbergma 2:4f8930f623ca 61
mathiasbergma 2:4f8930f623ca 62 measno++; //Used for average amps & volts
mathiasbergma 2:4f8930f623ca 63
mathiasbergma 2:4f8930f623ca 64 ampsmean = 0; //Zeroing mean amps before next calculation
mathiasbergma 2:4f8930f623ca 65
mathiasbergma 2:4f8930f623ca 66 analogAmp = ain_1;
mathiasbergma 2:4f8930f623ca 67 analogAmp -= AIOFFSET;
mathiasbergma 2:4f8930f623ca 68 ampreading[measno] = analogAmp*AMPREL;
mathiasbergma 2:4f8930f623ca 69
mathiasbergma 2:4f8930f623ca 70 for (int i = 1; i <= AVERAGE; i++)
mathiasbergma 2:4f8930f623ca 71 {
mathiasbergma 2:4f8930f623ca 72 ampsmean += ampreading[i];
mathiasbergma 2:4f8930f623ca 73 voltmean += voltreading[i];
mathiasbergma 2:4f8930f623ca 74 }
mathiasbergma 2:4f8930f623ca 75 ampsmean /= AVERAGE;
mathiasbergma 2:4f8930f623ca 76 voltmean /= AVERAGE;
mathiasbergma 2:4f8930f623ca 77
mathiasbergma 2:4f8930f623ca 78 if (t > 1.0) //Zeroing timer
mathiasbergma 2:4f8930f623ca 79 {
mathiasbergma 2:4f8930f623ca 80 wh += ampsmean * voltmean * SECDEC;
mathiasbergma 2:4f8930f623ca 81 t.stop();
mathiasbergma 2:4f8930f623ca 82 t.reset();
mathiasbergma 2:4f8930f623ca 83 t.start();
mathiasbergma 2:4f8930f623ca 84 }
mathiasbergma 2:4f8930f623ca 85
mathiasbergma 2:4f8930f623ca 86 analogVolt = ain_2; //distance between two ADC for stability
mathiasbergma 2:4f8930f623ca 87 voltreading[measno] = analogVolt * VOLTREL;
mathiasbergma 2:4f8930f623ca 88 m3pi.locate(1,0);
mathiasbergma 2:4f8930f623ca 89 m3pi.printf("%.3f",integral);
mathiasbergma 2:4f8930f623ca 90
mathiasbergma 2:4f8930f623ca 91 // Get the position of the line.
mathiasbergma 2:4f8930f623ca 92 current_pos_of_line = m3pi.line_position();
mathiasbergma 2:4f8930f623ca 93 proportional = current_pos_of_line;
mathiasbergma 2:4f8930f623ca 94
mathiasbergma 2:4f8930f623ca 95 // Compute the derivative
mathiasbergma 2:4f8930f623ca 96 derivative = current_pos_of_line - previous_pos_of_line;
mathiasbergma 2:4f8930f623ca 97
mathiasbergma 2:4f8930f623ca 98 // Compute the integral
mathiasbergma 2:4f8930f623ca 99 integral += proportional;
mathiasbergma 2:4f8930f623ca 100
mathiasbergma 2:4f8930f623ca 101 // Remember the last position.
mathiasbergma 2:4f8930f623ca 102 previous_pos_of_line = current_pos_of_line;
mathiasbergma 2:4f8930f623ca 103
mathiasbergma 2:4f8930f623ca 104 // Compute the power
mathiasbergma 2:4f8930f623ca 105 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
mathiasbergma 2:4f8930f623ca 106
mathiasbergma 2:4f8930f623ca 107 // Compute new speeds
mathiasbergma 2:4f8930f623ca 108 right = speed+power;
mathiasbergma 2:4f8930f623ca 109 left = speed-power;
mathiasbergma 2:4f8930f623ca 110
mathiasbergma 2:4f8930f623ca 111 // limit checks
mathiasbergma 2:4f8930f623ca 112 if (right < MIN)
mathiasbergma 2:4f8930f623ca 113 right = MIN;
mathiasbergma 2:4f8930f623ca 114 else if (right > MAX)
mathiasbergma 2:4f8930f623ca 115 right = MAX;
mathiasbergma 2:4f8930f623ca 116
mathiasbergma 2:4f8930f623ca 117 if (left < MIN)
mathiasbergma 2:4f8930f623ca 118 left = MIN;
mathiasbergma 2:4f8930f623ca 119 else if (left > MAX)
mathiasbergma 2:4f8930f623ca 120 left = MAX;
mathiasbergma 2:4f8930f623ca 121
mathiasbergma 2:4f8930f623ca 122 // set speed
mathiasbergma 2:4f8930f623ca 123 m3pi.left_motor(left);
mathiasbergma 2:4f8930f623ca 124 m3pi.right_motor(right);
mathiasbergma 2:4f8930f623ca 125
mathiasbergma 2:4f8930f623ca 126 if (measno >= AVERAGE)
mathiasbergma 2:4f8930f623ca 127 {
mathiasbergma 2:4f8930f623ca 128 measno = 0;
mathiasbergma 2:4f8930f623ca 129 }
mathiasbergma 2:4f8930f623ca 130 voltmean = 0; //Zeroing mean volts before next calculation
mathiasbergma 2:4f8930f623ca 131
mathiasbergma 2:4f8930f623ca 132 }
mathiasbergma 2:4f8930f623ca 133 }