Diff: main.cpp

Revision:

6:93c9844af619

Parent:

5:5199647e821a

--- a/main.cpp	Fri Sep 25 19:14:30 2015 +0000
+++ b/main.cpp	Tue Dec 22 03:52:59 2015 +0000
@@ -7,6 +7,41 @@
 
 #define MMA8451_I2C_ADDRESS (0x1d<<1)
 
+#define espera  0.0001
+#define sp      0.5     // set point 
+#define sp0     0.5     // set point 
+#define sp1     0.4     // set point 
+#define idle    0
+#define full    0.7     
+#define k       100     // Divisor
+#define lol     0.0
+#define m       -1.333
+
+PwmOut motor0(A2);    // PITCH arriba 
+PwmOut motor1(A3);    // ROLL izq 
+PwmOut motor2(A4);    // ROLL der 
+PwmOut motor3(A5);    // PITCH abajo
+
+/*PwmOut motor1(A2);    // PITCH arriba 
+PwmOut motor0(A3);    // ROLL izq 
+PwmOut motor3(A4);    // ROLL der 
+PwmOut motor2(A5);    // PITCH abajo*/
+
+DigitalIn   enable(SW1);
+DigitalIn   btn2(SW3);
+DigitalOut  led(LED1);
+DigitalOut  led2(LED2);
+
+int i=0,flag=0;
+float xm=0, x=0;
+float ym=0, y=0;
+
+float pwmVal0=0, pwmVal1=0; // Valores auxiliares para imprimir
+int i0=0,i1=0,i2=0,i3=0;
+float scalerX=0.5;
+float scalerY=0.5;
+float difSample = 0, currSample = 0;
+float c = 0;
 
 eCompass compass;
 MAG3110  mag( PTE25, PTE24);
@@ -25,32 +60,23 @@
 MotionSensorDataCounts mag_raw;
 MotionSensorDataCounts acc_raw;
 
+// Declaracion de tareas
 Thread *(debugp);
 Thread *(calibrate);
 Thread *(lcdp);
+Thread *(bandera);
+Thread *(controller);
+Thread *(controllerY);
 
 //
 // Print data values for debug
 //
 void debug_print(void)
-{
-    int h, m, s;
-    h = seconds / 3600;
-    m = (seconds % 3600) / 60;
-    s = seconds % 60;
-    // Some useful printf statements for debug
-    printf("Runtime= %d:%02d:%02d\r\n", h, m, s);
-    printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
-    printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f    ", axis6.fGax, axis6.fGay, axis6.fGaz);
-    printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
-    printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3); 
+{    
+    pc.printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
 }
+
 // HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
-//
-// This routing move and negates data as needed the
-// properly align the sensor axis for our desired compass (NED)
-// For more information see Freescale appication note AN4696
-//
 void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data)
 {
     int16_t t;
@@ -75,17 +101,19 @@
         acc.getAxis(acc_raw);
         if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
         if(!(l % 10)) lcdp->signal_set(0x04);
-        if(l++ >= 50) { // take car of business once a second
+        if(l++ >= 10) { // take care of business once a second//50
             seconds++;
+            bandera->signal_set(0x03);  
+            controller->signal_set(0x05);
+            controllerY->signal_set(0x06);
             calibrate->signal_set(0x2);
-            debugp->signal_set(0x01);
+            //debugp->signal_set(0x01);
             l = 0;
             green = !green;
             }
         tcount++;
 }
 
-
 void calibrate_thread(void const *argument) {
     while(true) {
         Thread::signal_wait(0x2); 
@@ -94,27 +122,165 @@
         red = 1;
         }
 }
-
  
 void print_thread(void const *argument) {
     while (true) {
         // Signal flags that are reported as event are automatically cleared.
         Thread::signal_wait(0x1);    
-        debug_print();
+        debug_print();        
+    }
+}
+
+void bandera_thread(void const *argument) {
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x3);  
+        //pc.printf("xm=%f\r\n", xm);  
+        if(!enable) 
+        {
+            flag=!flag;
+            wait(0.5);
+        } 
+        else flag=flag;
+
+        if(flag==1) 
+        {
+            led2=1;
+            led=0;
+        } 
+        else 
+        {    // Apaga los motores
+            led2=0; // Green off
+            led=1;  // Red on
+            motor0 = lol;
+            motor1 = lol;
+            motor2 = lol;
+            motor3 = lol;
+        }
     }
 }
 
+// Funciones de Membresia para control por logica difusa
+void controller_thread(void const *argument) {
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x5);  
+        
+        while (flag==1) 
+        {     
+            x = axis6.roll;
+            xm=x/k;    
+            
+            int LE = 10;     // Low Error
+            int ME = 10;    // Medium Error
+            int HE = 30;    // High Error
+            
+            float m0 = -1.9;
+            float m1 = -0.8;
+            
+            if (x > -ME || x < ME)
+            {                    
+                motor3 = sp-xm*m1*c;                
+                wait(espera);                
+                motor0 = sp+xm*m1*c;                
+                wait(espera);  
+                scalerX = 0.6;         
+                pc.printf("vcxvcvcxvxcvxc");                               
+            }
+            if (x < -HE)
+            {                
+                motor3 = idle;
+                wait(espera);                
+                motor0 = full*scalerX*c;                
+                wait(espera); 
+                if (scalerX <=0.85)
+                scalerX = scalerX+0.1;
+                pc.printf("vcxvcvcxvxcvxc");
+            }            
+            if (x > HE)
+            {                
+                motor3 = full*scalerX*c;
+                wait(espera);                
+                motor0 = idle;
+                wait(espera); 
+                if (scalerX <=0.85)
+                scalerX = scalerX+0.1;
+                pc.printf("vcxvcvcxvxcvxc");
+            }
+            
+            i2++;
+            if (i2 >= 15)  currSample = xm;                
+            if(i2 >= 30)
+            {
+                difSample = abs(xm -currSample);
+                i2 = 0;
+            }
+            
+            if (difSample >= 0.4)   c = 0.1;
+            if (difSample < 0.4)   c = 1;                                        
+                        
+        }
+    }
+}
+
+// Funciones de Membresia para control por logica difusa
+void controllerY_thread(void const *argument) {
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x6);  
+        
+        while (flag==1) 
+        {     
+            y = axis6.pitch;
+            ym=y/k;    
+            
+            int LE = 10;     // Low Error
+            int ME = 17;    // Medium Error
+            int HE = 30;    // High Error
+            
+            float my = -0.8;
+            
+            if (y > -ME && y < ME)
+            {    
+                motor2 = 0.2;//sp-ym*my;
+                wait(espera);                                            
+                motor1 = 0.2;//sp+ym*my;
+                wait(espera);                
+                scalerY = 0.6;                                        
+            }
+            if (y < -ME)
+            {
+                motor2 = idle; 
+                wait(espera);            
+                motor1 = full*scalerY;                
+                wait(espera);                
+                if (scalerY <=0.8)
+                scalerY = scalerY+0.1;
+            }            
+            if (y > ME)
+            {
+                motor2 = full*scalerY; 
+                wait(espera);            
+                motor1 = idle;   
+                wait(espera); 
+                if (scalerY <=0.8)
+                scalerY = scalerY+0.1;
+            }
+        }
+    }
+}
 
 void lcd_thread(void const *argument) {
     while (true) {
         // Signal flags that are reported as event are automatically cleared.
         Thread::signal_wait(0x4);  
-        slcd.printf("%04d", axis6.roll); // print the heading (NED compass) to the LCD
+        slcd.printf("%04d", axis6.roll); // print the heading (NED compass) to the LCD             
     }
 }
                                  
-int main() {
-
+int main() 
+{
+    pc.baud (115200);
     slcd.clear();
     tcount = 0;
     red = 1;
@@ -122,18 +288,25 @@
     //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data.
     compass_type = NED_COMPASS;
     seconds = 0;
-    Thread t1(print_thread);  
+    //Thread t1(print_thread);  
     Thread t2(calibrate_thread);
-    Thread t3(lcd_thread);
-    debugp = &t1;
+    //Thread t3(lcd_thread);        
+    
+    Thread t4(bandera_thread);
+    Thread t5(controller_thread);
+    Thread t6(controllerY_thread);
+    
+    //debugp = &t1;
     calibrate = &t2;
-    lcdp = &t3;  
+    //lcdp = &t3;  
+    
+    bandera = &t4;  
+    controller = &t5;  
+    controllerY = &t6;  
+    
     mag.enable();
     acc.enable();
-    // Say hello to our sensors
-    // Native KL46-FRDM sensors
-    printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI());
-    printf("MAG3110 ID= %X\r\n", mag.whoAmI());
+
     mag.getAxis(mag_raw); // flush the magnetmeter
     RtosTimer compass_timer(compass_thread, osTimerPeriodic);
     
@@ -148,8 +321,12 @@
 
     debugp->set_priority(osPriorityBelowNormal);
     lcdp->set_priority(osPriorityLow);
-    calibrate->set_priority(osPriorityAboveNormal);
-    compass_timer.start(20); // Run the Compass every 20ms
+    calibrate->set_priority(osPriorityAboveNormal);    
+    bandera->set_priority(osPriorityHigh);
+    controller->set_priority(osPriorityAboveNormal);
+    controllerY->set_priority(osPriorityAboveNormal);
+    
+    compass_timer.start(10); // Run the Compass every 20ms
     
     Thread::wait(osWaitForever);   
 }