Merge branch 'kamil' into main

Desktop/main_base/main_base/main_base.ino

@@ -21,6 +21,7 @@ typedef struct struct_message {
   int left_output;
   int right_output;
   int vertical_output;
+  int catcher_output;
   bool manual;
 } struct_message;
 
@@ -84,6 +85,14 @@ void loop() {
       manual_mode = false;
     }
 
+    if (PS4.R2()){
+      message.catcher_output = 1700;
+    } else if (PS4.L2()){
+      message.catcher_output = 1100;
+    } else {
+      message.catcher_output = 1350; 
+    }
+
     if (debug_mode_controller){
       Serial.print("LEFT MOTOR: ");
       Serial.println(LeftMotorOutput);
@@ -92,6 +101,8 @@ void loop() {
       Serial.printf("Left Stick x at %d\n", PS4.LStickX());
       Serial.printf("Left Stick y at %d\n", PS4.LStickY()); 
       Serial.printf("Right Stick y at %d\n", PS4.RStickY());
+      Serial.printf("R2 at %d\n", PS4.R2());
+      Serial.printf("L2 at %d\n", PS4.L2());
       Serial.print("MANUAL MODE: ");
       if (manual_mode){
         Serial.println("ON"); 
@@ -100,7 +111,7 @@ void loop() {
       }
     }
     
-    if (PS4.L1() || manual_mode){
+    if (PS4.L1() || manual_mode || PS4.R2() || PS4.L2()){
       message.left_output = int(LeftMotorOutput);
       message.right_output = int(RightMotorOutput);
       message.vertical_output = int(VerticalMotorOutput);
@@ -111,11 +122,11 @@ void loop() {
       message.right_output = 0;
       message.vertical_output = 0;
       message.manual = manual_mode;
+      message.catcher_output = 1350;
       esp_err_t outcome = esp_now_send(broadcastAddress, (uint8_t *) &message, sizeof(message));
     }
     
-    
-
+   
 //    if (outcome == ESP_OK) {
 //      Serial.println("Mesage sent successfully!");
 //    }

Main Code/main/main.ino

@@ -31,6 +31,7 @@ static const uint8_t l_motor = 7;
 static const uint8_t r_motor = 6;
 static const uint8_t vertical_motor_1 = 5;
 static const uint8_t vertical_motor_2 = 4;
+static const uint8_t catcher_servo = 3;
 
 
 //GLOBAL VARIABLES
@@ -86,9 +87,11 @@ typedef struct struct_message {
   int left_output;
   int right_output;
   int vertical_output;
+  int catcher_output;
   bool manual;
 } struct_message;
 
+void data_receive(const uint8_t * mac, const uint8_t *incomingData, int len);
 struct_message message;
 
 void setup() {
@@ -131,22 +134,22 @@ void setup() {
   pwm.begin();
   pwm.setOscillatorFrequency(OSCI_FREQ);
   pwm.setPWMFreq(BRUSHLESS_FREQ);  // Analog servos run at ~50 Hz updates
-  delay(1000);
-  pwm.writeMicroseconds(l_motor, 2000);
-  pwm.writeMicroseconds(r_motor, 2000);
-  pwm.writeMicroseconds(vertical_motor_1, 2000);
-  pwm.writeMicroseconds(vertical_motor_2, 2000);
-  delay(2000);
+//  delay(1000);
+//  pwm.writeMicroseconds(l_motor, 2000);
+//  pwm.writeMicroseconds(r_motor, 2000);
+//  pwm.writeMicroseconds(vertical_motor_1, 2000);
+//  pwm.writeMicroseconds(vertical_motor_2, 2000);
+//  delay(2000);
   pwm.writeMicroseconds(l_motor, 1500);
   pwm.writeMicroseconds(r_motor, 1500);
   pwm.writeMicroseconds(vertical_motor_1, 1500);
   pwm.writeMicroseconds(vertical_motor_2, 1500);
   delay(2000);
-  pwm.writeMicroseconds(l_motor, 1600);
-  pwm.writeMicroseconds(r_motor, 1600);
-  pwm.writeMicroseconds(vertical_motor_1, 1600);
-  pwm.writeMicroseconds(vertical_motor_2, 1600);
-  delay(2000);
+//  pwm.writeMicroseconds(l_motor, 1600);
+//  pwm.writeMicroseconds(r_motor, 1600);
+//  pwm.writeMicroseconds(vertical_motor_1, 1600);
+//  pwm.writeMicroseconds(vertical_motor_2, 1600);
+//  delay(2000);
 }
 
 void loop() {

Main Code/main/networking.ino

@@ -4,5 +4,8 @@ void data_receive(const uint8_t * mac, const uint8_t *incomingData, int len) {
   memcpy(&message, incomingData, sizeof(message));
   moveBlimp(message.left_output, message.right_output, message.vertical_output);
   manual_mode = message.manual;
+  pwm.writeMicroseconds(catcher_servo, message.catcher_output);
+  Serial.print("CATCHER: ");
+  Serial.println(message.catcher_output);
   command_time = millis();
 }