J Daniel Martinez C
/
Sumobot
Sumo mbed
Revision 1:1c837db2123b, committed 2014-04-04
- Comitter:
- dan_cuspi
- Date:
- Fri Apr 04 16:32:13 2014 +0000
- Parent:
- 0:2c00079aafe8
- Commit message:
- Sumo;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Oct 06 08:56:58 2013 +0000 +++ b/main.cpp Fri Apr 04 16:32:13 2014 +0000 @@ -1,234 +1,86 @@ #include "mbed.h" // screen /dev/tty.usbmodemfa132 Serial pc(USBTX, USBRX); +// ||Motor Izquierdo: Direc=p25,Pwm=p26|| Derecho: Direc=p23,Pwm=p24|| +// ||Sensor linea Izquierdo=p21||Derecho=p20|| +// ||Sonar Izquierdo=p22||Derechp=p19||Frontal=p18|| + /////////////////////////////////////////////////////////////////////// + //PWM ||Fast|| ||Slow|| ||wait time|| ||90°|| ||45°|| // + float pwmF=1, pwmS=.5, t=1, spin_90=1, spin_180=1; // +/////////////////////////////////////////////////////////////////////// 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(); - - } +DigitalOut MotorDirLA(p27); +DigitalOut MotorDirLB(p27); //1 Derecho 0 Atras +PwmOut MotorRPwm(p23); +DigitalOut MotorDirRA(p22); +DigitalOut MotorDirRB(p21); +//1 Derecho 0 Atras +DigitalIn LineL(p20); +DigitalIn LineR(p19); +DigitalIn SonarL(p18); +DigitalIn SonarR(p17); +DigitalIn SonarF(p16); +DigitalOut led1(LED1),led4(LED4); +PwmOut led2(LED2),led3(LED3); -} -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 +unsigned int LineL_val=0,LineR_val=0,SonarL_val=0,SonarR_val=0,SonarF_val=0,Factor=0; - 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 prueba(); +void Cases(); +void Values(); +void ForwardS(); +void ForwardF(); +void Backward(); +void Right(); +void Left(); +void Stop(); +int main() + { +LineL.mode(PullUp); +LineR.mode(PullUp); +SonarL.mode(PullUp); +SonarR.mode(PullUp); +SonarF.mode(PullUp); + + 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 + case 0: Factor=0; ForwardS(); break;// ningun sensor + case 1: Factor=0; Backward(); wait(t); Right(); wait(spin_90); break; //sensor linea izquierda + case 10: Factor=0; Backward(); wait(t); Left(); wait(spin_90); break; //sensor linea derecha + case 11: Factor=0; Backward(); wait(t); Right(); wait(spin_180); break; //2 sensores linea + case 100: Factor=0; Left(); wait(spin_90); break; //sonar izquierda + case 110: Factor=0; Backward(); wait(t); Left(); wait(spin_90); break; //sonar izquierda y linea derecha + case 1000: Factor=0; Right(); wait(spin_90); break; //sonar derecha + case 1001: Factor=0; Backward(); wait(t); Right(); wait(spin_90); break; //sonar derecha y linea izquierda + case 10000: Factor=0; ForwardF(); break;// sonar frontal + case 10001: Factor=0; ForwardF(); break;// sonar frontal linea izquierda + case 10010: Factor=0; ForwardF(); break;// sonar frontal linea derecha + case 10011: Factor=0; ForwardF(); break;// sonar frontal 2 lineas + case 11100: Factor=0; ForwardF(); break;// sonar frontal sonar iaquierda sonar derecha + default: Factor=0; Stop(); break; + } } -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() +void Values() { - MotorLF=0; - MotorLB=1; - MotorRF=0; - MotorRB=1; - MotorLPwm=pwmF; - MotorRPwm=pwmF; - led4=.1; - led3=.1; - led2=.1; - led1=.1; + 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 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; -} - - +void Stop() { MotorDirLA=0; MotorDirLA=0; MotorLPwm=0; MotorDirRA=0; MotorDirRB=0; MotorRPwm=0; led4=1; led3=1; led2=1; led1=1; } +void ForwardS() { MotorDirLA=1; MotorDirLA=0; MotorLPwm=pwmS; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmS; led4=0; led3=pwmS;led2=pwmS; led1=0; } +void ForwardF() { MotorDirLA=1; MotorDirLA=0; MotorLPwm=pwmF; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmF; led4=0; led3=pwmF;led2=pwmF; led1=0; } +void Backward() { MotorDirLA=0; MotorDirLA=1; MotorLPwm=pwmF; MotorDirRA=0; MotorDirRB=1; MotorRPwm=pwmF; led4=1; led3=0; led2=0; led1=1;} +void Right() { MotorDirLA=1; MotorDirLA=0; MotorLPwm=pwmF; MotorDirRA=0; MotorDirRB=1; MotorRPwm=pwmF; led4=0; led3=0; led2=0; led1=1; } +void Left() { MotorDirLA=0; MotorDirLA=1; MotorLPwm=pwmF; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmF; led4=1; led3=0; led2=0; led1=0; } \ No newline at end of file