Sumo mbed

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"  // screen /dev/tty.usbmodemfa132
00002 Serial pc(USBTX, USBRX);
00003 // ||Motor        Izquierdo: Direc=p25,Pwm=p26|| Derecho: Direc=p23,Pwm=p24||
00004 // ||Sensor linea Izquierdo=p21||Derecho=p20||
00005 // ||Sonar        Izquierdo=p22||Derechp=p19||Frontal=p18||
00006    ///////////////////////////////////////////////////////////////////////
00007   //PWM ||Fast||   ||Slow||   ||wait time||   ||90°||     ||45°||      //
00008   float  pwmF=1,    pwmS=.5,       t=1,     spin_90=1,  spin_180=1; //
00009 ///////////////////////////////////////////////////////////////////////
00010 PwmOut MotorLPwm(p26);
00011 DigitalOut MotorDirLA(p27);
00012 DigitalOut MotorDirLB(p27); //1 Derecho 0 Atras
00013 PwmOut MotorRPwm(p23);
00014 DigitalOut MotorDirRA(p22);
00015 DigitalOut MotorDirRB(p21); 
00016 //1 Derecho 0 Atras
00017 DigitalIn LineL(p20);
00018 DigitalIn LineR(p19);
00019 DigitalIn SonarL(p18);
00020 DigitalIn SonarR(p17);
00021 DigitalIn SonarF(p16);
00022 DigitalOut led1(LED1),led4(LED4);
00023 PwmOut led2(LED2),led3(LED3);
00024 
00025 unsigned int LineL_val=0,LineR_val=0,SonarL_val=0,SonarR_val=0,SonarF_val=0,Factor=0;
00026 
00027 void prueba();
00028 void Cases();
00029 void Values();
00030 void ForwardS();
00031 void ForwardF();
00032 void Backward();
00033 void Right();
00034 void Left();
00035 void Stop();
00036 int main()
00037  {
00038 LineL.mode(PullUp);
00039 LineR.mode(PullUp);
00040 SonarL.mode(PullUp);
00041 SonarR.mode(PullUp);
00042 SonarF.mode(PullUp);
00043   
00044       while(1)
00045       {
00046      
00047         Values();
00048         Cases();  
00049       }
00050       
00051   }
00052   
00053 //void prueba() {led1=pwmF;wait(1);led1=pwmS;wait(1);led1=0;wait(1); }
00054 void Cases()
00055 {switch(Factor)
00056  {
00057 //    LineL=1 LineR=10 SonarL=100 SonarR=1000 SonarF=10000                  
00058       case 0:     Factor=0; ForwardS(); break;// ningun sensor          
00059       case 1:     Factor=0; Backward();  wait(t);  Right();  wait(spin_90); break; //sensor linea izquierda
00060       case 10:    Factor=0; Backward();  wait(t);  Left();   wait(spin_90); break; //sensor linea derecha
00061       case 11:    Factor=0; Backward();  wait(t);  Right();  wait(spin_180); break;   //2 sensores linea
00062       case 100:   Factor=0; Left();                          wait(spin_90); break; //sonar izquierda
00063       case 110:   Factor=0; Backward();  wait(t);  Left();   wait(spin_90); break; //sonar izquierda y linea derecha
00064       case 1000:  Factor=0; Right();                         wait(spin_90); break; //sonar derecha
00065       case 1001:  Factor=0; Backward();  wait(t);  Right();  wait(spin_90); break; //sonar derecha y linea izquierda
00066       case 10000: Factor=0; ForwardF(); break;// sonar frontal
00067       case 10001: Factor=0; ForwardF(); break;// sonar frontal linea izquierda
00068       case 10010: Factor=0; ForwardF(); break;// sonar frontal linea derecha
00069       case 10011: Factor=0; ForwardF(); break;// sonar frontal 2 lineas
00070       case 11100: Factor=0; ForwardF(); break;// sonar frontal sonar iaquierda sonar derecha
00071       default:    Factor=0; Stop(); break;
00072  }
00073 }
00074 void Values()
00075 {
00076     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;}
00077     Factor=LineL_val+LineR_val+SonarL_val+SonarR_val+SonarF_val;
00078     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);
00079     LineL_val=0; LineR_val=0; SonarL_val=0; SonarR_val=0; SonarF_val=0;
00080 }
00081 void Stop()     { MotorDirLA=0; MotorDirLA=0;  MotorLPwm=0;    MotorDirRA=0; MotorDirRB=0; MotorRPwm=0;     led4=1;   led3=1;   led2=1;    led1=1;  }  
00082 void ForwardS() { MotorDirLA=1; MotorDirLA=0;  MotorLPwm=pwmS; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmS;  led4=0;   led3=pwmS;led2=pwmS; led1=0;  }  
00083 void ForwardF() { MotorDirLA=1; MotorDirLA=0;  MotorLPwm=pwmF; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmF;  led4=0;   led3=pwmF;led2=pwmF; led1=0;  }  
00084 void Backward() { MotorDirLA=0; MotorDirLA=1;  MotorLPwm=pwmF; MotorDirRA=0; MotorDirRB=1; MotorRPwm=pwmF;  led4=1;   led3=0;   led2=0;    led1=1;}  
00085 void Right()    { MotorDirLA=1; MotorDirLA=0;  MotorLPwm=pwmF; MotorDirRA=0; MotorDirRB=1; MotorRPwm=pwmF;  led4=0;   led3=0;   led2=0;    led1=1;  } 
00086 void Left()     { MotorDirLA=0; MotorDirLA=1;  MotorLPwm=pwmF; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmF;  led4=1;   led3=0;   led2=0;    led1=0;  }