J Daniel Martinez C / Mbed 2 deprecated Sumobot

Sumo mbed

Dependencies:   mbed

main.cpp@1:1c837db2123b, 2014-04-04 (annotated)

Committer:
dan_cuspi
Date:
Fri Apr 04 16:32:13 2014 +0000
Revision:
1:1c837db2123b
Parent:
0:2c00079aafe8 
Sumo;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dan_cuspi 0:2c00079aafe8 1 #include "mbed.h" // screen /dev/tty.usbmodemfa132
dan_cuspi 0:2c00079aafe8 2 Serial pc(USBTX, USBRX);
dan_cuspi 1:1c837db2123b 3 // ||Motor Izquierdo: Direc=p25,Pwm=p26|| Derecho: Direc=p23,Pwm=p24||
dan_cuspi 1:1c837db2123b 4 // ||Sensor linea Izquierdo=p21||Derecho=p20||
dan_cuspi 1:1c837db2123b 5 // ||Sonar Izquierdo=p22||Derechp=p19||Frontal=p18||
dan_cuspi 1:1c837db2123b 6 ///////////////////////////////////////////////////////////////////////
dan_cuspi 1:1c837db2123b 7 //PWM ||Fast|| ||Slow|| ||wait time|| ||90°|| ||45°|| //
dan_cuspi 1:1c837db2123b 8 float pwmF=1, pwmS=.5, t=1, spin_90=1, spin_180=1; //
dan_cuspi 1:1c837db2123b 9 ///////////////////////////////////////////////////////////////////////
dan_cuspi 0:2c00079aafe8 10 PwmOut MotorLPwm(p26);
dan_cuspi 1:1c837db2123b 11 DigitalOut MotorDirLA(p27);
dan_cuspi 1:1c837db2123b 12 DigitalOut MotorDirLB(p27); //1 Derecho 0 Atras
dan_cuspi 1:1c837db2123b 13 PwmOut MotorRPwm(p23);
dan_cuspi 1:1c837db2123b 14 DigitalOut MotorDirRA(p22);
dan_cuspi 1:1c837db2123b 15 DigitalOut MotorDirRB(p21);
dan_cuspi 1:1c837db2123b 16 //1 Derecho 0 Atras
dan_cuspi 1:1c837db2123b 17 DigitalIn LineL(p20);
dan_cuspi 1:1c837db2123b 18 DigitalIn LineR(p19);
dan_cuspi 1:1c837db2123b 19 DigitalIn SonarL(p18);
dan_cuspi 1:1c837db2123b 20 DigitalIn SonarR(p17);
dan_cuspi 1:1c837db2123b 21 DigitalIn SonarF(p16);
dan_cuspi 1:1c837db2123b 22 DigitalOut led1(LED1),led4(LED4);
dan_cuspi 1:1c837db2123b 23 PwmOut led2(LED2),led3(LED3);
dan_cuspi 0:2c00079aafe8 24
dan_cuspi 1:1c837db2123b 25 unsigned int LineL_val=0,LineR_val=0,SonarL_val=0,SonarR_val=0,SonarF_val=0,Factor=0;
dan_cuspi 0:2c00079aafe8 26
dan_cuspi 1:1c837db2123b 27 void prueba();
dan_cuspi 1:1c837db2123b 28 void Cases();
dan_cuspi 1:1c837db2123b 29 void Values();
dan_cuspi 1:1c837db2123b 30 void ForwardS();
dan_cuspi 1:1c837db2123b 31 void ForwardF();
dan_cuspi 1:1c837db2123b 32 void Backward();
dan_cuspi 1:1c837db2123b 33 void Right();
dan_cuspi 1:1c837db2123b 34 void Left();
dan_cuspi 1:1c837db2123b 35 void Stop();
dan_cuspi 1:1c837db2123b 36 int main()
dan_cuspi 1:1c837db2123b 37 {
dan_cuspi 1:1c837db2123b 38 LineL.mode(PullUp);
dan_cuspi 1:1c837db2123b 39 LineR.mode(PullUp);
dan_cuspi 1:1c837db2123b 40 SonarL.mode(PullUp);
dan_cuspi 1:1c837db2123b 41 SonarR.mode(PullUp);
dan_cuspi 1:1c837db2123b 42 SonarF.mode(PullUp);
dan_cuspi 1:1c837db2123b 43
dan_cuspi 1:1c837db2123b 44 while(1)
dan_cuspi 1:1c837db2123b 45 {
dan_cuspi 1:1c837db2123b 46
dan_cuspi 1:1c837db2123b 47 Values();
dan_cuspi 1:1c837db2123b 48 Cases();
dan_cuspi 1:1c837db2123b 49 }
dan_cuspi 1:1c837db2123b 50
dan_cuspi 1:1c837db2123b 51 }
dan_cuspi 1:1c837db2123b 52
dan_cuspi 1:1c837db2123b 53 //void prueba() {led1=pwmF;wait(1);led1=pwmS;wait(1);led1=0;wait(1); }
dan_cuspi 1:1c837db2123b 54 void Cases()
dan_cuspi 1:1c837db2123b 55 {switch(Factor)
dan_cuspi 1:1c837db2123b 56 {
dan_cuspi 1:1c837db2123b 57 // LineL=1 LineR=10 SonarL=100 SonarR=1000 SonarF=10000
dan_cuspi 1:1c837db2123b 58 case 0: Factor=0; ForwardS(); break;// ningun sensor
dan_cuspi 1:1c837db2123b 59 case 1: Factor=0; Backward(); wait(t); Right(); wait(spin_90); break; //sensor linea izquierda
dan_cuspi 1:1c837db2123b 60 case 10: Factor=0; Backward(); wait(t); Left(); wait(spin_90); break; //sensor linea derecha
dan_cuspi 1:1c837db2123b 61 case 11: Factor=0; Backward(); wait(t); Right(); wait(spin_180); break; //2 sensores linea
dan_cuspi 1:1c837db2123b 62 case 100: Factor=0; Left(); wait(spin_90); break; //sonar izquierda
dan_cuspi 1:1c837db2123b 63 case 110: Factor=0; Backward(); wait(t); Left(); wait(spin_90); break; //sonar izquierda y linea derecha
dan_cuspi 1:1c837db2123b 64 case 1000: Factor=0; Right(); wait(spin_90); break; //sonar derecha
dan_cuspi 1:1c837db2123b 65 case 1001: Factor=0; Backward(); wait(t); Right(); wait(spin_90); break; //sonar derecha y linea izquierda
dan_cuspi 1:1c837db2123b 66 case 10000: Factor=0; ForwardF(); break;// sonar frontal
dan_cuspi 1:1c837db2123b 67 case 10001: Factor=0; ForwardF(); break;// sonar frontal linea izquierda
dan_cuspi 1:1c837db2123b 68 case 10010: Factor=0; ForwardF(); break;// sonar frontal linea derecha
dan_cuspi 1:1c837db2123b 69 case 10011: Factor=0; ForwardF(); break;// sonar frontal 2 lineas
dan_cuspi 1:1c837db2123b 70 case 11100: Factor=0; ForwardF(); break;// sonar frontal sonar iaquierda sonar derecha
dan_cuspi 1:1c837db2123b 71 default: Factor=0; Stop(); break;
dan_cuspi 1:1c837db2123b 72 }
dan_cuspi 0:2c00079aafe8 73 }
dan_cuspi 1:1c837db2123b 74 void Values()
dan_cuspi 0:2c00079aafe8 75 {
dan_cuspi 1:1c837db2123b 76 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;}
dan_cuspi 1:1c837db2123b 77 Factor=LineL_val+LineR_val+SonarL_val+SonarR_val+SonarF_val;
dan_cuspi 1:1c837db2123b 78 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);
dan_cuspi 1:1c837db2123b 79 LineL_val=0; LineR_val=0; SonarL_val=0; SonarR_val=0; SonarF_val=0;
dan_cuspi 0:2c00079aafe8 80 }
dan_cuspi 1:1c837db2123b 81 void Stop() { MotorDirLA=0; MotorDirLA=0; MotorLPwm=0; MotorDirRA=0; MotorDirRB=0; MotorRPwm=0; led4=1; led3=1; led2=1; led1=1; }
dan_cuspi 1:1c837db2123b 82 void ForwardS() { MotorDirLA=1; MotorDirLA=0; MotorLPwm=pwmS; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmS; led4=0; led3=pwmS;led2=pwmS; led1=0; }
dan_cuspi 1:1c837db2123b 83 void ForwardF() { MotorDirLA=1; MotorDirLA=0; MotorLPwm=pwmF; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmF; led4=0; led3=pwmF;led2=pwmF; led1=0; }
dan_cuspi 1:1c837db2123b 84 void Backward() { MotorDirLA=0; MotorDirLA=1; MotorLPwm=pwmF; MotorDirRA=0; MotorDirRB=1; MotorRPwm=pwmF; led4=1; led3=0; led2=0; led1=1;}
dan_cuspi 1:1c837db2123b 85 void Right() { MotorDirLA=1; MotorDirLA=0; MotorLPwm=pwmF; MotorDirRA=0; MotorDirRB=1; MotorRPwm=pwmF; led4=0; led3=0; led2=0; led1=1; }
dan_cuspi 1:1c837db2123b 86 void Left() { MotorDirLA=0; MotorDirLA=1; MotorLPwm=pwmF; MotorDirRA=1; MotorDirRB=0; MotorRPwm=pwmF; led4=1; led3=0; led2=0; led1=0; }