Codigo comentado da IronCup 01/03/2020
Dependencies: mbed
Diff: main.cpp
- Revision:
- 10:d546eec37792
- Parent:
- 9:fccecc817082
- Child:
- 11:4f3dcdd4d8ce
diff -r fccecc817082 -r d546eec37792 main.cpp --- a/main.cpp Wed Feb 26 20:57:52 2020 +0000 +++ b/main.cpp Thu Feb 27 00:01:42 2020 +0000 @@ -44,9 +44,9 @@ //============== VARIAVEIS MAIS IMPORTANTES ================// //Separadas do resto para facilitar a alteração -float ParametroCorrecao = 1.2; //2 // 4.63 // 2 ----> 1.2 -float Kp_Sensor = 0.55; //0.14; //0.2 //0.16 // 0.2 // 0.35--->0.55 -float Kd_Sensor = 7.2; //1.75; //1.15 //0.3 // 2.7 // 4.3 --->7.2 +float ParametroCorrecao = 1.2; //1.1 // 4.63 // 2 ----> 1.2 +float Kp_Sensor = 0.55; //0.14; //0.2 //0.4 // 0.2 // 0.35--->0.55 +float Kd_Sensor = 7.2; //1.75; //1.15 //0.6 // 2.7 // 4.3 --->7.2 float BaseSpeed = 0.18; //0.12 // 0.15 // 0.18--->0.18 //==========================================================// @@ -65,7 +65,7 @@ int TotalError = 0; int LastSensor = 0; int SET_POINT = 45; -int lastRead = 3; + //float Turbo = BaseSpeed; @@ -90,6 +90,8 @@ //int Esquerda_Count = 0, Esquerda_LastCount = 0; //int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed; +void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso + /*================================================= -------------- CODIGO DOS SENSORES ---------------- =================================================*/ @@ -100,7 +102,7 @@ multiplexadorInOut.mode(PullNone); } -bool sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15 +int sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15 // função para ler os motores,retorna true se for branco e false se for preto multiplexador1 = tabelaVerdade1[sensor]; // selecionando o sensor @@ -130,7 +132,7 @@ tempoDoSensor.reset(); multiplexadorInOut.output(); multiplexadorInOut = 0; - return false; // retorna quando for preto + return 0; // retorna quando for preto } } @@ -140,21 +142,25 @@ tempoDoSensor.reset(); multiplexadorInOut.output(); multiplexadorInOut = 0; - return true;// retorna quando for branco + return 1;// retorna quando for branco } void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição - - if (sensorCheck(3)== 1 && sensorCheck(4) == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint - else if (sensorCheck(3)==1) {Position = 40;lastRead = 3;} - else if (sensorCheck(4)==1) {Position = 50;lastRead = 4;} - else if (sensorCheck(2)==1) {Position = 30;lastRead = 2;} - else if (sensorCheck(5)==1) {Position = 60;lastRead = 5;} - else if (sensorCheck(1)==1) {Position = 20;lastRead = 1;} - else if (sensorCheck(6)==1) {Position = 70;lastRead = 6;} - else if (sensorCheck(0)==1 && (lastRead == 1||lastRead == 2)) {Position = 10;lastRead = 0;} - else if (sensorCheck(7) == 1 && (lastRead == 6||lastRead == 5)) {Position = 80;lastRead = 7;} - + int leitura[8]={0,0,0,0,1,0,0,0}; // leitura representa os sensores lidos,onde 1 é branco e 0 é preto + for (int i=0;i<8;i++){leitura[i] = sensorCheck(i);} // leitura dos sensores + //analizando a leitura e setando a posição + if (leitura[3]== 1 && leitura[4] == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint + else if (leitura[2]== 1 && leitura[5] == 1) {Position = 45;} + else if (leitura[1]== 1 && leitura[6] == 1) {Position = 45;} + else if (leitura[3]==1) {Position = 40;} + else if (leitura[2]==1) {Position = 30;} + else if (leitura[5]==1) {Position = 60;} + else if (leitura[1]==1) {Position = 20;} + else if (leitura[6]==1) {Position = 70;} + else if (leitura[0]==1) {Position = 10;} + else if (leitura[7]==1) {Position = 80;} + else if (leitura[4]==1) {Position = 50;} + //else {Final_Stop();} //pc.printf("\n\rPosicao: %d", Position); //Print de teste } @@ -193,7 +199,7 @@ else { LastSensor = Error; } } */ -void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso + /*================================================= --------------- PID DOS SENSORES ----------------- =================================================*/ @@ -202,7 +208,7 @@ Error = Position - SET_POINT; //bt.printf("\n\rErro: %d", Error); - if (Error < 20 && Error > -20){ + if (Error < 25 && Error > -25){ Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError); } else{