Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <AFMotor.h>
- #define ft 1
- #define ts 2
- int sense1 = 52;
- int sense2 = 53;
- int valor1 = 0;
- int valor2 = 0;
- int ult = 0;
- int s0_1 = 22;
- int s1_1 = 24;
- int s2_1 = 26;
- int s3_1 = 28;
- int out_1 = 30;
- int red_1 = 0;
- int green_1 = 0;
- int blue_1 = 0;
- int s0_2 = 31;
- int s1_2 = 33;
- int s2_2 = 35;
- int s3_2 = 37;
- int out_2 = 39;
- int red_2 = 0;
- int green_2 = 0;
- int blue_2 = 0;
- AF_DCMotor m1(1);
- AF_DCMotor m2(2);
- AF_DCMotor m3(3);
- AF_DCMotor m4(4);
- void setup() {
- m1.setSpeed(255);
- m2.setSpeed(255);
- m3.setSpeed(255);
- m4.setSpeed(255);
- pinMode(sense1, INPUT);
- pinMode(sense2, INPUT);
- pinMode(s0_1, OUTPUT);
- pinMode(s1_1, OUTPUT);
- pinMode(s2_1, OUTPUT);
- pinMode(s3_1, OUTPUT);
- pinMode(out_1, INPUT);
- pinMode(s0_2, OUTPUT);
- pinMode(s1_2, OUTPUT);
- pinMode(s2_2, OUTPUT);
- pinMode(s3_2, OUTPUT);
- pinMode(out_2, INPUT);
- digitalWrite(s0_1, HIGH);
- digitalWrite(s1_1, LOW);
- digitalWrite(s0_2, HIGH);
- digitalWrite(s1_2, LOW);
- delay(1000);
- Serial.begin(9600);
- }
- void loop() {
- valor1 = digitalRead(sense1);
- valor2 = digitalRead(sense2);
- if((valor1==HIGH) && (valor2==HIGH)){
- cor1();
- cor2();
- if ((green_1 < red_1) && (blue_1 < green_1) && (blue_1>80)){
- direitaCor();
- delay(800);
- } else {
- if ((red_2 < green_2) && (blue_2 < red_2) && (blue_2>55)){
- esquerdaCor();
- delay(800);
- } else {
- ult = 0;
- frente();
- }
- }
- }
- if((valor1==HIGH) && (valor2==LOW)){
- ult = 1;
- direita();
- }
- if((valor1==LOW) && (valor2==HIGH)){
- ult = 2;
- esquerda();
- }
- if((valor1==LOW) && (valor2==LOW)){
- if (ult != 0){
- if (ult == 1)
- direita();
- if (ult == 2)
- esquerda();
- }
- else{
- giro180();
- }
- }
- }
- void frente() {
- m1.setSpeed(200);
- m2.setSpeed(200);
- m3.setSpeed(200);
- m4.setSpeed(200);
- m1.run(ft);
- m2.run(ft);
- m3.run(ft);
- m4.run(ft);
- }
- void giro180() {
- m1.setSpeed(160);
- m2.setSpeed(160);
- m3.setSpeed(160);
- m4.setSpeed(160);
- m1.run(ft);
- m2.run(ft);
- m3.run(ts);
- m4.run(ts);
- }
- void esquerda() {
- m1.setSpeed(200);
- m2.setSpeed(200);
- m3.setSpeed(200);
- m4.setSpeed(200);
- m1.run(ft);
- m2.run(ft);
- m3.run(ts);
- m4.run(ts);
- }
- void direita() {
- m1.setSpeed(200);
- m2.setSpeed(200);
- m3.setSpeed(200);
- m4.setSpeed(200);
- m1.run(ts);
- m2.run(ts);
- m3.run(ft);
- m4.run(ft);
- }
- void esquerdaCor() {
- m1.setSpeed(200);
- m2.setSpeed(200);
- m3.setSpeed(85);
- m4.setSpeed(85);
- m1.run(ft);
- m2.run(ft);
- m3.run(ts);
- m4.run(ts);
- }
- void direitaCor() {
- m1.setSpeed(85);
- m2.setSpeed(85);
- m3.setSpeed(200);
- m4.setSpeed(200);
- m1.run(ts);
- m2.run(ts);
- m3.run(ft);
- m4.run(ft);
- }
- void cor1() {
- digitalWrite(s2_1, LOW);
- digitalWrite(s3_1, LOW);
- red_1 = pulseIn(out_1, digitalRead(out_1) == HIGH ? LOW : HIGH);
- digitalWrite(s3_1, HIGH);
- blue_1 = pulseIn(out_1, digitalRead(out_1) == HIGH ? LOW : HIGH);
- digitalWrite(s2_1, HIGH);
- green_1 = pulseIn(out_1, digitalRead(out_1) == HIGH ? LOW : HIGH);
- }
- void cor2() {
- digitalWrite(s2_2, LOW);
- digitalWrite(s3_2, LOW);
- red_2 = pulseIn(out_2, digitalRead(out_2) == HIGH ? LOW : HIGH);
- digitalWrite(s3_2, HIGH);
- blue_2 = pulseIn(out_2, digitalRead(out_2) == HIGH ? LOW : HIGH);
- digitalWrite(s2_2, HIGH);
- green_2 = pulseIn(out_1, digitalRead(out_2) == HIGH ? LOW : HIGH);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement