Testing for publishing

Dependencies:   mbed

main.cpp

Committer:
atalkingdog
Date:
2021-05-23
Revision:
1:e8cbe60decea
Parent:
0:7b3def60ac72

File content as of revision 1:e8cbe60decea:

#include "mbed.h"

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 ((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);
}