J Daniel Martinez C
/
Sumobot
Sumo mbed
Embed:
(wiki syntax)
Show/hide line numbers
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; }
Generated on Sun Jul 17 2022 01:47:52 by 1.7.2