finish the low level control

Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino

@@ -6,6 +6,7 @@
 
 #include "Camera.h"
 #include "utilities.h"
+#include <Adafruit_MotorShield.h>
 
 // REPLACE WITH THE MAC Address of your receiver (MASTER)
 // Slave: 40:F5:20:44:B6:4C
@@ -28,6 +29,7 @@ int count = 0;
 int count_var = 0;
 int print_count = 0;
 int change_count =0;
+int Lidar_flag = 0;
 
 // Define variables to be sent;
 int Sent_tx = 0;
@@ -113,21 +115,34 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
 openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
 openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
 Camera cam(&interface);
-
+// ========================== 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 
 // ==================================== Set up =================================================
 void setup() {
   // Init Serial Monitor
   Serial.begin(115200);
   Wire.begin();
   interface.begin(); //communication between ESP and OpenMV
+  AFMS.begin();  // create with the default frequency 1.6KHz
   // -------------- LED part --------------------
   pinMode(LED, OUTPUT);
   digitalWrite(LED, HIGH);
   digitalWrite(LED, LOW);
   
   // -------------- lidar part --------------------
-  if (distanceSensor.begin() == 0)
-     Serial.println("Sensor online!");
+  if (distanceSensor.begin() == 0){
+    Serial.println("Sensor online!");
+    Lidar_flag = 1;
+  }else {
+    Lidar_flag = 0;
+  }
+     
   distanceSensor.startRanging();
   distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
   
@@ -164,6 +179,7 @@ void setup() {
 // ================================== Main loop ===================================================
 void loop()
 { 
+  //-------------------------------------------------------------------------------------
   // ========== goal finder =========
   int id = -1;
   int tx = 0; int ty = 0; int tz = 0;
@@ -188,14 +204,18 @@ void loop()
       Sent_ry = 0;
       Sent_rz = 0; 
   }
-
-  Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
-
-  if (Sent_dist < 300){
+  if (Lidar_flag == 1){
+    Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
+  }else {
+    Sent_dist = 0;
+  }
+  // ========== lidar state info =========
+  if (Sent_dist < 300 && Sent_dist < 0){
     receivedData.DebugM = "found b";
   }else{
     receivedData.DebugM = "no b";
   }
+  // ========== send info =========
   receivedData.Rtx = Sent_tx;
   receivedData.Rty = Sent_ty;
   receivedData.Rtz = Sent_tz;  
@@ -203,14 +223,49 @@ void loop()
   receivedData.Rry = Sent_ry; 
   receivedData.Rrz = Sent_rz;
   receivedData.Rdist = Sent_dist;
-  
   send_var_once();
-  
   print_received_Data();
   
   //-------------------------------------------------------------------------------------
+  control_motion();
+  //-------------------------------------------------------------------------------------
 }
 // ================================== ^ Main loop ^ ===================================================
+void control_motion(){
+  // vertical motor
+  motorVertical_L->setSpeed(abs(Rec_pwm1));
+  motorVertical_R->setSpeed(abs(Rec_pwm2));  
+  
+  if (Rec_dir1 == "+"){
+    motorVertical_L->run(BACKWARD); // up 
+  }else if (Rec_dir1 == "-"){
+    motorVertical_L->run(FORWARD); // down 
+  }
+
+  if (Rec_dir2 == "+"){ 
+    motorVertical_R->run(FORWARD);
+  }else if (Rec_dir2 == "-"){
+    motorVertical_R->run(BACKWARD);    
+  }
+
+  // horizontal motor
+  motorLeft->setSpeed(abs(Rec_pwm3));
+  motorRight->setSpeed(abs(Rec_pwm4));
+
+  if (Rec_dir3 == "+"){ 
+    motorLeft->run(BACKWARD); // make it move forward 
+  }else if (Rec_dir3 == "-"){
+    motorLeft->run(FORWARD);   // make it move backward  
+  }
+
+  if (Rec_dir4 == "+"){ 
+    motorRight-> run(FORWARD);  // make it move forward 
+  }else if (Rec_dir4 == "-"){
+    motorRight-> run(BACKWARD); // make it move backward 
+  }
+  
+}
+
 void print_received_Data(){
   if (print_count == 0){
   Serial.print("Rec_pwm1:");