Diff: main.cpp

Revision:

0:2c00079aafe8

Child:

1:1c837db2123b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 06 08:56:58 2013 +0000
@@ -0,0 +1,234 @@
+#include "mbed.h"  // screen /dev/tty.usbmodemfa132
+Serial pc(USBTX, USBRX);
+PwmOut MotorLPwm(p26);
+DigitalOut MotorLF(p30);
+DigitalOut MotorLB(p29);
+
+PwmOut MotorRPwm(p25);
+DigitalOut MotorRF(p11);
+DigitalOut MotorRB(p12);
+
+DigitalIn LineL(p21);
+DigitalIn LineR(p20);
+
+DigitalIn SonarL(p22);
+DigitalIn SonarR(p19);
+DigitalIn SonarF(p18);
+
+PwmOut led1(LED1),led2(LED2),led3(LED3),led4(LED4);
+
+unsigned int LineL_val=0,LineR_val=0,SonarL_val=0,SonarR_val=0,SonarF_val=0,factor=0;
+
+////////////////////////////////////
+//PWM ||Fast||  ||Slow||         //
+float  pwmF=1,   pwmS=.5;       //
+//Giro  ||90º||    ||180º||    //
+float  spin90=.1,  spin180=1; //
+///////////////////////////////
+
+void prueba(),cases(),values(),forward_S(),forward_F(),backward(),right(),left(),stop();
+
+int main()
+{
+    while(1) {
+        values();
+        cases();
+
+    }
+
+}
+void prueba()
+{
+    led1=pwmF;
+    wait(1);
+    led1=pwmS;
+    wait(1);
+    led1=0;
+    wait(1);
+}
+void cases()
+{
+    switch(factor) {
+//       LineL=1 LineR=10 SonarL=100 SonarR=1000 SonarF=10000
+//                               ||tiempo de paro para reversa||   ||90º||
+        case 1:
+            factor=0;
+            backward();
+            wait(.1);
+            right();
+            wait(.4);
+            break; //sensor linea izquierda
+        case 10:
+            factor=0;
+            backward();
+            wait(.1);
+            left();
+            wait(.4);
+            break; //sensor linea derecha
+        case 110:
+            factor=0;
+            backward();
+            wait(.1);
+            left();
+            wait(.4);
+            break; //sonar izquierda y linea derecha
+        case 1001:
+            factor=0;
+            backward();
+            wait(.1);
+            right();
+            wait(.4);
+            break; //sonar derecha y linea izquierda
+        case 100:
+            factor=0;
+            left();
+            wait(.4);
+            break; //sonar izquierda
+        case 1000:
+            factor=0;
+            right();
+            wait(.4);
+            break; //sonar derecha
+//                                                                 ||180º||
+        case 11:
+            factor=0;
+            backward();
+            wait(.01);
+            right();
+            wait(.8);
+            break;   //2 sensores linea
+
+        case 10000:
+            factor=0;
+            forward_F();
+            break;// sonar frontal
+        case 10001:
+            factor=0;
+            forward_F();
+            break;// sonar frontal linea izquierda
+        case 10010:
+            factor=0;
+            forward_F();
+            break;// sonar frontal linea derecha
+        case 10011:
+            factor=0;
+            forward_F();
+            break;// sonar frontal 2 lineas
+        case 11100:
+            factor=0;
+            forward_F();
+            break;// sonar frontal sonar iaquierda sonar derecha
+        case 0:
+            factor=0;
+            forward_S();
+            break;// ningun sensor
+        default:
+            factor=0;
+            stop();
+            break;
+
+    }
+}
+void values()
+{
+    if (LineL==1) {
+        LineL_val=1;
+    }
+    if (LineR==1) {
+        LineR_val=10;
+    }
+    if (SonarL==1) {
+        SonarL_val=100;
+    }
+    if (SonarR==1) {
+        SonarR_val=1000;
+    }
+    if (SonarF==1) {
+        SonarF_val=10000;
+    }
+    factor=LineL_val+LineR_val+SonarL_val+SonarR_val+SonarF_val;
+    pc.printf ("LL=%i LR=%i SL=%i SR=%i SF=%i F=%i \r\n",LineL_val,LineR_val,SonarL_val,SonarR_val,SonarF_val,factor);
+    LineL_val=0;
+    LineR_val=0;
+    SonarL_val=0;
+    SonarR_val=0;
+    SonarF_val=0;
+}
+void stop()
+{
+    MotorLF=0;
+    MotorLB=0;
+    MotorRF=0;
+    MotorRB=0;
+    led4=0;
+    led3=0;
+    led2=0;
+    led1=0;
+}
+void forward_S()
+{
+    MotorLF=1;
+    MotorLB=0;
+    MotorRF=1;
+    MotorRB=0;
+    MotorLPwm=pwmS;
+    MotorRPwm=pwmS;
+    led4=0;
+    led3=.1;
+    led2=.1;
+    led1=0;
+}
+void forward_F()
+{
+    MotorLF=1;
+    MotorLB=0;
+    MotorRF=1;
+    MotorRB=0;
+    MotorLPwm=pwmF;
+    MotorRPwm=pwmF;
+    led4=0;
+    led3=1;
+    led2=1;
+    led1=0;
+}
+void backward()
+{
+    MotorLF=0;
+    MotorLB=1;
+    MotorRF=0;
+    MotorRB=1;
+    MotorLPwm=pwmF;
+    MotorRPwm=pwmF;
+    led4=.1;
+    led3=.1;
+    led2=.1;
+    led1=.1;
+}
+void right()
+{
+    MotorLF=0;
+    MotorLB=1;
+    MotorRF=1;
+    MotorRB=0;
+    MotorLPwm=pwmF;
+    MotorRPwm=pwmF;
+    led4=0;
+    led3=0;
+    led2=0;
+    led1=1;
+}
+void left()
+{
+    MotorLF=1;
+    MotorLB=0;
+    MotorRF=0;
+    MotorRB=1;
+    MotorLPwm=pwmF;
+    MotorRPwm=pwmF;
+    led4=1;
+    led3=0;
+    led2=0;
+    led1=0;
+}
+
+