Hi, i have problem with detecting an intersection, counting(intersections) and do a spesific thing base on the counter at line follower robot.
I had tried solutions like StateChangeDetection and its works but i dont know how to merge to my program, i'm using 8 TCRT5000 sensor and the robot following line without move the robot body(using 4 mecanum wheels).
Here is my code :
// motor
#define R_ENa 30
#define L_ENa 31
#define R_ENb 32
#define L_ENb 33
#define R_ENc 34
#define L_ENc 35
#define R_ENd 36
#define L_ENd 37
// motor pwm
#define RPWMa 2
#define LPWMa 3
#define RPWMb 4
#define LPWMb 5
#define RPWMc 6
#define LPWMc 7
#define RPWMd 8
#define LPWMd 9
// relay
#define RELAY_ON 1
#define RELAY_OFF 0
#define Relay 22
int spd = 100;
int spdcal = 100;
const int irPins[8] = {38, 39, 42, 43, 40, 41, 44, 45};
int irSensorDigital[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int irSensors = B00000000;
int i, j = 0;
void setup() {
Serial.begin(9600);
for (int j = 30; j < 38; j++) {
pinMode(j, OUTPUT);
}
for (int j = 2; j < 10; j++) {
pinMode(j, OUTPUT);
}
for (int j = 30; j < 38; j++) {
digitalWrite(j, HIGH);
}
for (int i = 0; i <= 9; i++)
{
pinMode(irPins[i], INPUT);
}
digitalWrite(Relay, RELAY_OFF);
pinMode(Relay, OUTPUT);
}
void loop() {
digRead();
check();
}
void check() {
switch (irSensors) {
// jalan maju
case B11000011:
maju();
break;
case B11000111:
maju();
break;
// kalibrasi maju
case B11000010:
CW();
break;
case B11000001:
CCW();
break;
case B10000010:
kanan();
break;
case B01000001:
kiri();
break;
case B10000001:
CCW();
break;
case B01000010:
CW();
break;
case B10000011:
CCW();
break;
case B01000011:
CW();
break;
// jalan kanan
case B11101111:
kanan();
break;
case B11011111:
kanan();
break;
case B11001111:
kanan();
break;
case B00001111:
kanan();
break;
case B00011111:
kanan();
break;
case B00101111:
kanan();
break;
case B01001111:
kanan();
break;
// kalibrasi kanan
case B00000011:
CCW();
break;
case B00011001:
kanan();
break;
case B10001111:
kanan;
break;
case B01001101:
CCW();
break;
case B00000010:
CCW();
break;
case B10001110:
CW();
break;
case B11001110:
CCW();
break;
case B11001101:
CCW();
break;
case B11001011:
CW();
break;
case B00001011:
CW();
break;
case B00000111:
CCW();
break;
case B11001001:
mundur();
break;
case B00001001:
mundur();
break;
case B00000110:
maju();
break;
case B00000101:
CCW();
break;
case B00001010:
CW();
break;
case B00001101:
CCW();
break;
case B00001110:
CW();
break;
// perempatan
default:
berhenti();
}
delay(1);
}
void maju() {
analogWrite(RPWMa, spd);
analogWrite(LPWMb, spd);
analogWrite(RPWMc, spd);
analogWrite(LPWMd, spd);
analogWrite(LPWMa, 0);
analogWrite(RPWMb, 0);
analogWrite(LPWMc, 0);
analogWrite(RPWMd, 0);
}
void mundur() {
analogWrite(LPWMa, spd);
analogWrite(RPWMb, spd);
analogWrite(LPWMc, spd);
analogWrite(RPWMd, spd);
analogWrite(RPWMa, 0);
analogWrite(LPWMb, 0);
analogWrite(RPWMc, 0);
analogWrite(LPWMd, 0);
}
void kiri() {
analogWrite(RPWMa, spd);
analogWrite(RPWMb, spd);
analogWrite(LPWMc, spd);
analogWrite(LPWMd, spd);
analogWrite(LPWMa, 0);
analogWrite(LPWMb, 0);
analogWrite(RPWMc, 0);
analogWrite(RPWMd, 0);
}
void kanan() {
analogWrite(LPWMa, spd);
analogWrite(LPWMb, spd);
analogWrite(RPWMc, spd);
analogWrite(RPWMd, spd);
analogWrite(RPWMa, 0);
analogWrite(RPWMb, 0);
analogWrite(LPWMc, 0);
analogWrite(LPWMd, 0);
}
void CCW() {
analogWrite(RPWMa, spd);
analogWrite(RPWMb, spd);
analogWrite(RPWMc, spd);
analogWrite(RPWMd, spd);
analogWrite(LPWMa, 0);
analogWrite(LPWMb, 0);
analogWrite(LPWMc, 0);
analogWrite(LPWMd, 0);
}
void CW() {
analogWrite(LPWMa, spd);
analogWrite(LPWMb, spd);
analogWrite(LPWMc, spd);
analogWrite(LPWMd, spd);
analogWrite(RPWMa, 0);
analogWrite(RPWMb, 0);
analogWrite(RPWMc, 0);
analogWrite(RPWMd, 0);
}
void berhenti() {
analogWrite(LPWMa, 0);
analogWrite(LPWMb, 0);
analogWrite(LPWMc, 0);
analogWrite(LPWMd, 0);
analogWrite(RPWMa, 0);
analogWrite(RPWMb, 0);
analogWrite(RPWMc, 0);
analogWrite(RPWMd, 0);
}
void periksa() {
Serial.print(digitalRead(irPins[0]));
Serial.print(" || ");
Serial.print(digitalRead(irPins[1]));
Serial.print(" || ");
Serial.print(digitalRead(irPins[2]));
Serial.print(" || ");
Serial.print(digitalRead(irPins[3]));
Serial.print(" || ");
Serial.print(digitalRead(irPins[4]));
Serial.print(" || ");
Serial.print(digitalRead(irPins[5]));
Serial.print(" || ");
Serial.print(digitalRead(irPins[6]));
Serial.print(" || ");
Serial.println(digitalRead(irPins[7]));
delay(100);
}
void digRead() {
for ( byte count = 0; count < 8; count++ )
{
bitWrite(irSensors, count, digitalRead( irPins[count] ));
}
}
I have problem like B11001111 must execute function "void maju()" at an intersection and execute "void kanan()" at another intersection.
Thank You.