Mirte H
/
Lijn_code_af_robotzwenkwiel
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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