Diff: main.cpp

Revision:

6:111a7f1b978f

Parent:

5:e04419b1e369

diff -r e04419b1e369 -r 111a7f1b978f main.cpp
--- a/main.cpp	Wed May 02 11:18:49 2018 +0000
+++ b/main.cpp	Fri Mar 01 10:07:05 2019 +0000
@@ -1,191 +1,117 @@
 #include "mbed.h"
-#include "TextLCD.h"
 #include "reScale.h"
-#include "eeprom.h"
-
-#define tole_min 10 //tolerancia minima em km/h
-#define tole_max 100 //tolerancia maxima em km/h
-#define ndentes 60    //numero de dentes do sensor
-
-
-
-I2C i2c_lcd(PTC9,PTC8); // SDA, SCL
-
-EEPROM e2prom(PTC11,PTC10,0x0,EEPROM::T24C04); 
 
-InterruptIn motora(D0);
-InterruptIn movida(D1);
-
-DigitalOut ign(LED2);
-PwmOut boost(PTC1);
-
-
-Timer t1;
-Timer t2;
-Ticker call;
-
-Serial pc(USBTX, USBRX); // tx, rx
-
-reScale _scale(tole_min,tole_max,0,1); 
-
-float t_motora=0, t_movida=0;
-float perimetro_motora=0, perimetro_movida=0;
+#define KP 0.8f
+#define KI 1.0f
+#define KD 0.01f
+#define offset 0.02f
 
 
-TextLCD_I2C lcd(&i2c_lcd, 0x7E, TextLCD::LCD20x4);                  // I2C exp: I2C bus, PCF8574 Slaveaddress, LCD Type
+AnalogIn SETPOINT(A0);
+AnalogIn TPS1(A2);
+AnalogIn TPS2(A3);
+Ticker flipper;
+DigitalOut  controlLED(LED2); 
+DigitalOut  IN1A(D10);
+DigitalOut  IN2A(D8);
+PwmOut TBI(D9);
+
+//Timer t1;
 
-void tracao() 
-{
-    t1.stop();
-    t_motora = t1.read_us();
-    t1.reset();
-    t1.start();   
-}
- 
-void livre() 
+//Serial device(PB_6, PB_7); // tx, rx
+Serial device(USBTX, USBRX); // tx, rx
+
+reScale _scale(0.0f,1.0f,0.09f,0.83f); 
+
+float pwm=0, erro=0, proporcional=0, integrador=0;
+float TPS1_VAL, SETPOINT_VAL=0, SETPOINT_scaled=0;
+bool flag=0; 
+
+void flip() 
 {
-    t2.stop();
-    t_movida = t2.read_us();
-    t2.reset();
-    t2.start();   
-}
- 
-void send() 
-{       
-    pc.printf("Tempo: %.3f Hz  \n\r");       
-}
- 
-void le_memoria()
-{ 
-    e2prom.read(12, perimetro);
-    e2prom.ready();  
-}
- 
-void calc_perimetro_motora()
-{   
-    float raio_dinamico=0;
-    float pneu_largura=205;
-    float pneu_perfil=50;
-    float pneu_aro=15;
-    /*
-    e2prom.read(0,pneu_largura);
-    e2prom.ready();
-    e2prom.read(4,pneu_perfil);
-    e2prom.ready();
-    e2prom.read(8,pneu_aro);
-    e2prom.ready();  
-    */ 
-    raio_dinamico = pneu_largura * (pneu_perfil /100);
-    raio_dinamico = raio_dinamico + (pneu_aro * 25.4);
-    perimetro_motora = raio_dinamico * 3.14; //resultado em milimetros
-    perimetro_motora = perimetro_motora / 1000; //converter em metros
-    
-    //e2prom.write(12, perimetro_motora);
-    //e2prom.ready();  
+    flag=!flag;
+    controlLED=!controlLED;
 }
 
-void calc_perimetro_movida()
-{   
-    float raio_dinamico=0;
-    float pneu_largura=205;
-    float pneu_perfil=50;
-    float pneu_aro=15;
-    /*
-    e2prom.read(0,pneu_largura);
-    e2prom.ready();
-    e2prom.read(4,pneu_perfil);
-    e2prom.ready();
-    e2prom.read(8,pneu_aro);
-    e2prom.ready();  
-    */ 
-    raio_dinamico = pneu_largura * (pneu_perfil /100);
-    raio_dinamico = raio_dinamico + (pneu_aro * 25.4);
-    perimetro_movida = raio_dinamico * 3.14; //resultado em milimetros
-    perimetro_movida = perimetro_movida / 1000; //converter em metros
-    
-    //e2prom.write(12, perimetro_movida);
-    //e2prom.ready();  
+void open()
+{
+    IN1A=1;
+    IN2A=0;
 }
- 
-int main() 
+
+void close()
 {
-    float delta=0;
-    float vel_movida=0;
-    float vel_motora=0;
-    float pwmturbo=0;
-
+    IN1A=0;
+    IN2A=1;
+}
     
-    motora.fall(&tracao);  // attach the address of the measure function to the rising edge
-    movida.fall(&livre);  // attach the address of the measure function to the rising edge
-    
-    
-    boost.period(0.01f);
-    
-    lcd.setBacklight(TextLCD::LightOn);    
-    lcd.setCursor(TextLCD::CurOff_BlkOff);       
-    lcd.setAddress(0,0);  
-    lcd.printf("Controle de");
-    lcd.setAddress(0,1);  
-    lcd.printf("TRACAO");
-    wait(1.5);
-    lcd.cls(); 
+int main() 
+{   
+    flipper.attach(&flip, 0.2);
+    TBI.period(0.01f);
     
-    /*
-    if(  USUARIO DESEJA CALCULAR O PERIMETRO  ) usar while com timeout!!!!!!!!!!!!!!!!!!
-    {
-    calc_perimetro_movida();   
-    calc_perimetro_motora();  
-    e2prom.read(12, perimetro);
-    e2prom.ready();    
-    }
-    
-    le_memoria();    
-    */
-    
-    calc_perimetro_motora();
-    calc_perimetro_movida();
-
+    IN1A=0;
+    IN2A=0;
     
     while(1) 
-    {              
-        vel_motora = perimetro / ((t_motora*ndentes) / 1000000);
-        vel_motora = vel_motora * 3.6;
-
-        vel_movida = perimetro / ((t_movida*ndentes) / 1000000);
-        vel_movida = vel_movida * 3.6;
+    {   
+        
+    //t1.reset();
+    //t1.start(); 
         
-        delta = vel_motora - vel_movida;
-                
-        if(delta<=0)
+        TPS1_VAL = TPS1.read();
+        SETPOINT_VAL = SETPOINT.read();
+        
+        SETPOINT_scaled = _scale.from(SETPOINT_VAL);
+        
+        if(SETPOINT_scaled < 0.185f)
         {
-            delta=0;
-        }
-        
-        pwmturbo =_scale.from(delta); 
-        boost.write(pwmturbo);
-
-        if(delta > tole_min)
-        {
-            ign=0;
-            wait(0.1);
-            lcd.setAddress(0,0);  
-            lcd.printf("Destracionando");
+            close();
+            erro =  TPS1_VAL - SETPOINT_scaled;        
+            proporcional = erro * KP;
         }
         
         else
         {
-            ign=1;
-            lcd.setAddress(0,0);  
-            lcd.printf("Normal         ");
+            open();
+            erro = SETPOINT_scaled - TPS1_VAL;        
+            proporcional = erro * KP;
+        }
+                
+        if(erro>1 || erro<-1)
+        {
+            //do nothing
         }
         
+        else
+        {
+            integrador =  integrador + ((erro * KI)*0.01f);
+        }
+        
+        if(integrador >=1)
+        integrador = 1;
+        
+        if(integrador <=-1)
+        integrador = -1;
+        
                 
-        lcd.setAddress(0,1);  
-        lcd.printf("Motora: %.1f Km/h   ",vel_motora);
-        lcd.setAddress(0,2);  
-        lcd.printf("Movida: %.1f Km/h   ",vel_movida);
-        lcd.setAddress(0,3);  
-        lcd.printf("D:%.1f Km/h B:%.1f  ",delta, pwmturbo);
-   
+        pwm = proporcional + integrador;
+        TBI = pwm + offset;
+        
+        
+        if(flag==1)
+        {
+            device.printf("\n\rPedal: %.4f",SETPOINT_scaled);
+            device.printf("\n\rTPS: %.4f",TPS1_VAL);
+            device.printf("\n\rErro: %.4f",erro);
+            device.printf("\n\rPWM: %.4f",pwm);
+            //device.printf("integrador: %f\n",integrador);
+            //device.printf("proporcional: %f\n",proporcional);
+            flag=0;
+        }
+        
+        //t1.stop();
+        //printf("O ciclo levou: %i uS \n\r", t1.read_us());
+        wait(0.01);
     }
 }
\ No newline at end of file