Mirte H
/
Lijn_code_af_robotzwenkwiel
leuk
main.cpp
- Committer:
- mirteholm
- Date:
- 2019-05-29
- Revision:
- 1:a8c5e92c6cbf
- Parent:
- 0:7d87166a1170
File content as of revision 1:a8c5e92c6cbf:
// Initialize a pins to perform Serial Communication for receive // 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" Serial pc(USBTX, USBRX); 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() { 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; } } } }