I don't know what I have done but the board's rgb led now cycles through all colours. If i press it multiple times it starts flashing green. Uploading any code doesn't work. I tried hard resetting it and somehow I made the rgb led stop for a while. I then wanted to test the code, uploaded it and it started again. Also when I put my hand in front of some sensors the rgb led shuts of and one motor rotates (not as intended).
#include <Arduino.h>
#include <ESP32Servo.h>
#include <QTRSensors.h>
#define START D0 //start module
// Define motor A
#define AIN1 D5
#define AIN2 D4
#define PWMA D3
// Define motor B
#define BIN1 D7
#define BIN2 D8
#define PWMB D9
// Define STBY pin
#define STBY D6
// Define senzor pins
#define FRONTLEFT A1
#define FRONT A0
#define FRONTRIGHT A2
#define LEFT A4
#define RIGHT A3
// Define line sensor pins
#define LEFTLINE A6
#define RIGHTLINE A7
// Variable for storage of sensor values
uint16_t SensorValues[5];
// Define sensor pins
#define LEFTSERVO D12
#define RIGHTSERVO D11
// Initialize servos
Servo leftServo;
Servo rightServo;
// Define dip switch pins
#define DIP1 A5
#define DIP2 D13
// Variable for dip switch pins
uint8_t DipPins[] = {A5, D13};
size_t DipSwitchSize = 2;
// Create int value for strategy index
uint8_t strategy_index = 0;
void setup() {
// Start Module
pinMode(START, INPUT);
// Motor A
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
// Motor B
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
// Motor Driver
pinMode(STBY, OUTPUT);
digitalWrite(STBY, HIGH);
// Distance Sensors
pinMode(FRONTLEFT, INPUT);
pinMode(FRONT, INPUT);
pinMode(FRONTRIGHT, INPUT);
pinMode(LEFT, INPUT);
pinMode(RIGHT, INPUT);
// Line Sensors
pinMode(LEFTLINE, INPUT);
pinMode(RIGHTLINE, INPUT);
// Dip Switch
pinMode(DIP1, INPUT_PULLUP);
pinMode(DIP2, INPUT_PULLUP);
// Attach servo pins
leftServo.attach(D12);
rightServo.attach(D11);
Serial.begin(9600);
}
void loop() {
uint16_t fl = analogRead(FRONTLEFT);
uint16_t f = analogRead(FRONT);
uint16_t fr = analogRead(FRONTRIGHT);
uint16_t l = analogRead(LEFT);
uint16_t r = analogRead(RIGHT);
if (true){
strategy_index = 0;
switch (strategy_index){
case 0: // Random search
/*
if (!leftLine.See()){
MoveMotors(255, -255);
delay(200);
}
else if (!rightLine.See()){
MoveMotors(-255, 255);
delay(200);
}
*/
if (fl > 1000 && f > 1000){
MoveMotors(255, 175);
}
else if (f > 1000 && fr > 1000){
MoveMotors(175, 255);
}
else if (fr > 1000){
MoveMotors(255, 255);
}
else if (l > 1200){
MoveMotors(255, -125);
}
else if (r > 1200){
MoveMotors(-125, 255);
}
else {
MoveMotors(0, 0);
}
break;
case 1:
// Add strategy code here...
break;
// Add more strategies...
default:
//Serial.println("Error 001: No strategy for provided strategy index.");
break;
}
}
else {
MoveMotors(0, 0);
}
}
uint8_t GetDipValue(uint8_t pins[], size_t BitCount){
bool values[BitCount];
for (int i = 0; i < BitCount; i++){
values[i] = digitalRead(pins[i]);
}
return GetBinary(values, BitCount);
}
uint8_t GetBinary(bool values[], size_t BitCount){
uint8_t total = 0;
for (int i = 0; i < BitCount; i++){
total += pow(2, i)*values[i];
}
return total;
}
void MoveMotors(int8_t value1, int8_t value2){
digitalWrite(AIN1, (value1+abs(value1))/2/abs(value1));
digitalWrite(AIN2, (value1-abs(value1))/2/abs(value1));
analogWrite(PWMA, abs(value1));
digitalWrite(BIN1, (value2+abs(value2))/2/abs(value2));
digitalWrite(BIN2, (value2-abs(value2))/2/abs(value2));
analogWrite(PWMB, abs(value2));
}