Diff: main.cpp

Revision:

1:e8cbe60decea

Parent:

0:7b3def60ac72

--- a/main.cpp	Tue Dec 17 09:43:47 2019 +0000
+++ b/main.cpp	Sun May 23 16:37:52 2021 +0000
@@ -1,12 +1,131 @@
 #include "mbed.h"
 
-DigitalOut myled(LED1);
+DigitalIn r1Ray(D2);//RaySensor
+DigitalIn r2Ray(D3);
+DigitalIn mRay(D4);
+DigitalIn l2Ray(D11);
+DigitalIn l1Ray(D6);
+DigitalOut RA(D7);//DCMotocontroler
+DigitalOut RB(D12);
+DigitalOut LA(D9);
+DigitalOut LB(D10);
+PwmOut Right(A5);
+PwmOut Left(A6);
+DigitalOut text(D13);
+void judgement();
+void turnRight();
+void turnLeft();
+void moveForward();
+void rush();
+void rightMoto(int,int);
+void leftMoto(int,int);
 
 int main() {
-    while(1) {
-        myled = 1; // LED is ON
-        wait(0.2); // 200 ms
-        myled = 0; // LED is OFF
-        wait(1.0); // 1 sec
+    while ((r1Ray == 0)||(r2Ray == 0))
+        judgement();
+    turnRight();
+    while (((r1Ray == 0)||(r2Ray == 0))||((l1Ray == 0)||(l2Ray == 0)))
+        judgement();
+    rush();
+    while (((r1Ray == 0)||(r2Ray == 0))||((l1Ray == 0)||(l2Ray == 0)))
+        judgement();
+    rush();
+    while ((r1Ray == 0)||(r2Ray == 0))
+        judgement();
+    turnRight();
+    while (((r1Ray == 0)||(r2Ray == 0))||((l1Ray == 0)||(l2Ray == 0)))
+        judgement();
+    rush();
+    while ((r1Ray == 0)||(r2Ray == 0))
+        judgement();
+    turnRight();
+    while ((r1Ray == 0)||(r2Ray == 0))
+        judgement();
+    turnRight();
+    while (((r1Ray == 0)||(r2Ray == 0))||((l1Ray == 0)||(l2Ray == 0)))
+        judgement();
+    rush();
+}
+void rightMoto(int a,int b){
+    switch (a)
+    {
+    case 1:
+        RB = 1;
+        RA = 0;
+        break;
+    case -1:
+        RB = 0;
+        RA = 1;
+        break;
+    case 0:
+        RB = 1;
+        RA = 1;
+        break;
+    }
+    Right.period(0.1);
+    Right = (float)b/100;
+}
+void leftMoto(int a,int b){
+    switch (a)
+    {
+    case 1:
+        LB = 1;
+        LA = 0;
+        break;
+    case -1:
+        LB = 0;
+        LA = 1;
+        break;
+    case 0:
+        LB = 1;
+        LA = 1;
+        break;
+    }
+    Left.period(0.1);
+    Left = (float)b/100;
+}
+void moveForward(){
+    rightMoto(1,70);
+    leftMoto(1,70);
+}
+void judgement(){
+    if (r1Ray == 1)
+    {
+        rightMoto(1,30);
+        leftMoto(1,80);
+    }
+    else if (l1Ray == 1)
+    {
+        rightMoto(1,80);
+        leftMoto(1,30);
+    }
+    else if (r2Ray == 1)
+    {
+        rightMoto(1,50);
+        leftMoto(1,70);
+    }
+    else if (l2Ray == 1)
+    {
+        rightMoto(1,70);
+        leftMoto(1,50);
+    }
+    else
+    {
+        moveForward();
     }
 }
+void turnLeft(){
+    rightMoto(1,70);
+    leftMoto(0,0);
+    wait(0.4);
+
+}
+void turnRight(){
+    rightMoto(0,0);
+    leftMoto(1,70);
+    wait(0.4);
+}
+void rush(){
+    moveForward();
+    wait(0.3);
+}