#include <esp_now.h> 
#include <WiFi.h> 
#include <Wire.h> 
#include <SparkFun_VL53L1X.h> 
#include <Arduino.h> 
#include <Adafruit_MotorShield.h> 

int Rec_pwm1; 
int Rec_pwm2; 
int Rec_pwm3; 
int Rec_pwm4; 
String Rec_dir1; 
String Rec_dir2; 
String Rec_dir3; 
String Rec_dir4; 

// ========================== Motor part ==================================== 
// Create the motor shield object with the default I2C address 
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
// changed 
Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1);  // pwm1 
Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);  // pwm2 
Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);        // pwm3 
Adafruit_DCMotor *motorRight = AFMS.getMotor(4);       // pwm4  

void setup() {
  // put your setup code here, to run once:
 // Init Serial Monitor 
  Serial.begin(115200); 
  //  Wire.begin(); 
AFMS.begin();  // create with the default frequency 1.6KHz 

}
void motor_control(int pwm1,int pwm2,int pwm3,int pwm4){
  motorVertical_L->setSpeed(abs(pwm1)); 
  motorVertical_R->setSpeed(abs(pwm2)); 
  motorLeft->setSpeed(abs(pwm3)); 
  motorRight->setSpeed(abs(pwm4));  
}

void loop() {
  // put your main code here, to run repeatedly:
  int t_speed = 50;
 

    motorVertical_L->setSpeed(t_speed); 
  motorVertical_R->setSpeed(t_speed); 
  motorLeft->setSpeed(t_speed); 
  motorRight->setSpeed(t_speed); 
    motorVertical_L->run(BACKWARD); // 
  motorVertical_R->run(BACKWARD); // 
    motorLeft->run(BACKWARD); // 
  motorRight->run(BACKWARD); //   
  delay(1000);
  /* 
  delay(2000);
  motor_control(t_speed,0,0,0);
  motorVertical_L->run(BACKWARD); //  
  Serial.println("V_L: backward"); 
  delay(2000);
  motorVertical_L->run(FORWARD); 
  Serial.println("V_L: forward");
  
    delay(2000);
  motor_control(0,t_speed,0,0);
  motorVertical_R->run(BACKWARD); // 
  Serial.println("V_R: backward"); 
  delay(2000);
  motorVertical_R->run(FORWARD); // 
  Serial.println("V_R: forward");
  
    delay(2000);
  motor_control(0,0,t_speed,0);
  motorLeft->run(BACKWARD); // 
  Serial.println("H_L: backward"); 
  delay(2000);
  motorLeft->run(FORWARD); // 
  Serial.println("H_L: forward");  

    delay(2000);
  motor_control(0,0,0,t_speed);
  motorRight->run(BACKWARD); // 
  Serial.println("H_R: backward"); 
  delay(2000);
  motorRight->run(FORWARD); // 
  Serial.println("H_R: forward"); 
  */
  
 
}
