J Daniel Martinez C
Sumo mbed
Diff: main.cpp
- Revision:
- 1:1c837db2123b
- Parent:
- 0:2c00079aafe8
--- 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