Codigo comentado da IronCup 01/03/2020
Dependencies: mbed
Diff: main.cpp
- Revision:
- 8:66d5045d9a5b
- Parent:
- 7:0dc01968a6e7
- Child:
- 9:fccecc817082
diff -r 0dc01968a6e7 -r 66d5045d9a5b main.cpp --- a/main.cpp Wed Feb 26 02:07:12 2020 +0000 +++ b/main.cpp Wed Feb 26 04:38:56 2020 +0000 @@ -4,7 +4,7 @@ //========================= DEFINICOES =========================// -#define threshold 250 // parametro que diferencia a leitura do branco pro preto +#define threshold 127 // parametro que diferencia a leitura do branco pro preto PwmOut pwm(p25); DigitalOut dir (p26); PwmOut pwm1(p24); @@ -44,10 +44,10 @@ //============== VARIAVEIS MAIS IMPORTANTES ================// //Separadas do resto para facilitar a alteração -float ParametroCorrecao = 4.63; //2 // 4.63 -float Kp_Sensor = 0.2; //0.14; //0.2 //0.16 // 0.2 -float Kd_Sensor = 2.7; //1.75; //1.15 //0.3 // 2.7 -float BaseSpeed = 0.15; //0.12 // 0.15 +float ParametroCorrecao = 4.968; //2 // 4.63 +float Kp_Sensor = 0.225; //0.14; //0.2 //0.16 // 0.2 +float Kd_Sensor = 4.1; //1.75; //1.15 //0.3 // 2.7 +float BaseSpeed = 0.18; //0.12 // 0.15 //==========================================================// //#define VMAX_PWM 100 @@ -65,6 +65,7 @@ int TotalError = 0; int LastSensor = 0; int SET_POINT = 45; +int lastRead = 3; //float Turbo = BaseSpeed; @@ -144,15 +145,15 @@ 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;} - else if (sensorCheck(4)==1) {Position = 50;} - else if (sensorCheck(2)==1) {Position = 30;} - else if (sensorCheck(5)==1) {Position = 60;} - else if (sensorCheck(1)==1) {Position = 20;} - else if (sensorCheck(6)==1) {Position = 70;} - else if (sensorCheck(0)==1) {Position = 10;} - else if (sensorCheck(7) == 1) {Position = 80;} + if (sensorCheck(3)== 1 && sensorCheck(4) == 1) {Position = 45;lastRead = 3;} //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 = 2;} + else if (sensorCheck(0)==1 && (lastRead == 2 || lastRead == 1)) {Position = 10;lastRead = 0;} + else if (sensorCheck(7) == 1 && (lastRead == 6 ||lastRead == 5)) {Position = 80;lastRead = 7;} //pc.printf("\n\rPosicao: %d", Position); //Print de teste } @@ -201,7 +202,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{