Mirte H
/
Lijn_code_af_robotzwenkwiel
leuk
Revision 1:a8c5e92c6cbf, committed 2019-05-29
- Comitter:
- mirteholm
- Date:
- Wed May 29 15:28:52 2019 +0000
- Parent:
- 0:7d87166a1170
- Commit message:
- leuk
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 7d87166a1170 -r a8c5e92c6cbf main.cpp --- a/main.cpp Tue Apr 09 07:25:01 2019 +0000 +++ b/main.cpp Wed May 29 15:28:52 2019 +0000 @@ -2,34 +2,218 @@ // the result of the printf on PC (USB Virtual Com) // I suggest to use TeraTerm on PC. // TeraTerm configuration must be: 9600-8-N-1 FlowControl None - + /* EXAMPLE */ #include "mbed.h" #include "hcsr04.h" -DigitalIn IR(PA_4); -DigitalOut led(PA_3); Serial pc(USBTX, USBRX); - -//D12 TRIGGER D11 ECHO -HCSR04 sensor(D12, D11); -int main() + + +DigitalOut linksachter(D9); +DigitalOut rechtsachter(A6); +DigitalOut rechtsvoor(D7); +DigitalOut linksvoor(D10); +PwmOut pwm(D6); // links +PwmOut pwm2(D11); + +HCSR04 sensor(D4, A0); + + + +DigitalIn lijnsensor4(D1); +DigitalIn lijnsensor3(D0); +DigitalIn lijnsensor1(D2); +DigitalIn lijnsensor2(D3); + + +HCSR04 sensor2(D4, A2); +//HCSR04 sensor2(D4, A0); +//HCSR04 sensor3(D4, A0); +//HCSR04 sensor4(D4, A0); + + + +Timer timer; +Timer timer2; +Timer timer3; +Timer timer4; +Timer timer5; +Timer timer6; + + +enum ROBOTSTATE {vooruit, lijn_detect_linksvoor, lijn_detect_rechtsvoor, lijn_detect_linksachter, lijn_detect_rechtsachter, lijn_detect_voor, lijn_detect_achter, robot_detect_voor, robot_detect_rechts, robot_detect_links, robot_detect_achter, start,}; +ROBOTSTATE myState; +int main() { - while(1) - { - int a = IR; - if(a==1) - { - led=0; + pwm.period_ms(10/6); + pwm.pulsewidth_ms(1); + pwm2.period_ms(10/6); + pwm2.pulsewidth_ms(1); + + while(1) { + + + + switch(myState) { + + + + + + case vooruit: + pwm.period_ms(10/6); + pwm.pulsewidth_ms(1); + pwm2.period_ms(10/6); + pwm2.pulsewidth_ms(1); + + pwm=0.4; + pwm2=0.4; + linksvoor=1; + rechtsvoor=1; + linksachter=0; + rechtsachter=0; + if(lijnsensor1==0) { + myState = lijn_detect_linksvoor; + } + if(lijnsensor2==0) { + myState=lijn_detect_rechtsvoor; + } + if (lijnsensor3 ==0) { + myState=lijn_detect_linksachter; + } + if (lijnsensor4==0) { + myState = lijn_detect_rechtsachter; + } + if (sensor.distance()<50) { myState = robot_detect_voor; } + if (sensor2.distance()<30) {myState = robot_detect_achter;} + break; + + + + case lijn_detect_linksvoor: + + linksvoor=1; + rechtsvoor=0; + linksachter=0; + rechtsachter=0; + pwm=0.9; + pwm2=0.9; + wait_ms(2000); + linksvoor=1; + rechtsvoor=1; + wait_ms(500); + myState = vooruit; + break; + + case lijn_detect_rechtsvoor: + + + linksvoor=0; + rechtsvoor=1; + linksachter=0; + rechtsachter=0; + pwm=0.9; + pwm2=0.9; + wait_ms(2000); + linksvoor=1; + rechtsvoor=1; + wait_ms(500); + myState= vooruit; + break; + + + case lijn_detect_linksachter: + + timer3.start(); + linksvoor=0; + rechtsvoor=1; + linksachter=0; + rechtsachter=0; + pwm=0.7; + pwm2=0.8; + if(timer3.read_ms() >= 500 ) { + timer3.stop(); + timer3.reset(); + myState = vooruit; + } + break; + + case lijn_detect_rechtsachter: + + timer4.start(); + linksvoor=1; + rechtsvoor=0; + linksachter=0; + rechtsachter=0; + pwm=0.7; + pwm2=0.8; + if(timer4.read_ms() >= 500 ) { // voor twee seconden draait hij + timer4.stop(); + timer4.reset(); + myState = vooruit; + } + break; + + + case robot_detect_voor: + + + timer5.start(); + linksvoor=1; + rechtsvoor=1; + linksachter=0; + rechtsachter=0; + pwm=1; + pwm2=1; + + if(lijnsensor1==0) { + timer5.stop(); timer5.reset(); myState = lijn_detect_linksvoor; + } + if(lijnsensor2==0) { + timer5.stop(); timer5.reset(); myState=lijn_detect_rechtsvoor; + } + if (lijnsensor3 ==0) { + timer5.stop(); timer5.reset(); myState=lijn_detect_linksachter; + } + if (lijnsensor4==0) { + timer5.stop(); timer5.reset(); myState = lijn_detect_rechtsachter; + } + if (timer5.read_ms() >= 2000) { timer5.stop(); timer5.reset(); myState = vooruit; } + break; + + case robot_detect_achter: + + + timer6.start(); + pwm=1; + pwm2=1; + linksvoor=1; + rechtsvoor=0; + linksachter=0; + rechtsachter=0; + + if(lijnsensor1==0) { + timer6.stop(); timer6.reset(); myState = lijn_detect_linksvoor; + } + if(lijnsensor2==0) { + timer6.stop(); timer6.reset(); myState=lijn_detect_rechtsvoor; + } + if (lijnsensor3 ==0) { + timer6.stop(); timer6.reset(); myState=lijn_detect_linksachter; + } + if (lijnsensor4==0) { + timer6.stop(); timer6.reset(); myState = lijn_detect_rechtsachter; + } + if (timer6.read_ms() >= 1000) { timer6.stop(); timer6.reset(); myState = vooruit; } + + + + + + + + } - else - { - led=1; - } - - long distance = sensor.distance(); - printf("distance %d\r\n",distance); - wait_ms(50); // 1 sec - } } \ No newline at end of file