Difference between revisions of "S24: Team X"

void periodic_callbacks__1Hz(uint32_t callback_count) {
  if (callback_count == 0) {
    delay__us(450000); // start up delay
    return;
  }
  ultra_sonic_sensor_data_s data;
  ultrasonic_sensor__run_once();
}

void periodic_callbacks__10Hz(uint32_t callback_count) {
  sensors__transmit_once();
  bridge_get_data();
  sensor_msg_bank__handle_msg();
  heading_sensor_run_once();
}

void ultrasonic_sensor__run_once() {
  // left, middle, right respectively
  for (int i = 0; i < 8; i++) {
    uint16_t adc_raw[3];

    // start range and wait till all sensors are done
    ultrasonic_range_once();

    adc_raw[0] = adc__get_adc_value(ADC__CHANNEL_2);
    adc_raw[1] = adc__get_adc_value(ADC__CHANNEL_4);
    adc_raw[2] = adc__get_adc_value(ADC__CHANNEL_5);

    // printf("adc2: %d\n", adc_raw[0]);
    sensor_data.left = ((float)adc_raw[0] * SCALE_FACTOR);   // / 2.0;
    sensor_data.middle = ((float)adc_raw[1] * SCALE_FACTOR); // / 2.0;
    sensor_data.right = ((float)adc_raw[2] * SCALE_FACTOR);  // / 2.0;

    if (sensor_data.left < 36) {
      gpio__reset(board_io__get_led3());
    } else {
      gpio__set(board_io__get_led3());
    }

    if (sensor_data.middle < 36) {
      gpio__reset(board_io__get_led2());
    } else {
      gpio__set(board_io__get_led2());
    }

    if (sensor_data.right < 36) {
      gpio__reset(board_io__get_led1());
    } else {
      gpio__set(board_io__get_led1());
    }
  }
}

void bridge_get_data(void) {
  char byte;
  const uint32_t zero_timeout = 0;

  while (uart__get(bridge_uart, &byte, zero_timeout)) {
    printf("received byte: %c\n", byte);
  }
}

void bridge_send_data(void *buf, uint8_t buf_size, uint8_t msg_id) {
  // Format the float value from the buffer
  char tx_buf[30];
  if (msg_id == 0) {
    float speed = *(float *)buf;
    snprintf(tx_buf, sizeof(tx_buf), "spd: %f\n", (double)speed);
  } else if (msg_id == 1) {
    dbc_GEO_STATUS_s *msg = (dbc_GEO_STATUS_s *)buf;
    snprintf(tx_buf, sizeof(tx_buf), "hd: %d, br: %d\n", msg->GEO_STATUS_COMPASS_HEADING,
             msg->GEO_STATUS_COMPASS_BEARING);
  }
  uart_printf(bridge_uart, "%s", tx_buf);
  // printf("sent: %s\n", tx_buf);
}

static void process_uart_data_buf(void) {
  int temp;
  char *token;
  int coord_received = 0;

  token = strtok(uart_rx_buf, ",");
  temp = atoi(token);
  if (temp != 0) {
    dest_latitude = temp / 1000000.0f;
    printf("la: %f ", (double)dest_latitude);
    coord_received++;
  }

  token = strtok(NULL, ",");
  temp = atoi(token);
  if (temp != 0) {
    dest_longitude = temp / 1000000.0f;
    printf("lo: %f\n", (double)dest_longitude);
    coord_received++;
  }
  if (coord_received == 2) {
    dbc_GPS_DESTINATION_LOCATION_s dest_location;
    dest_location.GPS_DEST_LATITUDE_SCALED_100000 = dest_latitude * 100000;
    dest_location.GPS_DEST_LONGITUDE_SCALED_100000 = dest_longitude * 100000;
    can__msg_t can_msg = {};
    dbc_message_header_t header;
    header = dbc_encode_GPS_DESTINATION_LOCATION(can_msg.data.bytes, &dest_location);

    can_msg.msg_id = header.message_id;
    can_msg.frame_fields.data_len = header.message_dlc;
    can__tx(can1, &can_msg, 0);
  }
}

void periodic_callbacks__10Hz(uint32_t callback_count) {
  motor_msg_bank__handle_msg();
  motor_msg_bank__service_mia_10hz();
  motor_pwm__set_pwm();
  if (callback_count % 7 == 0) {
    motor_rpm_driver_calc_speed();
  }
  motor_speed_pid_run_once();
}

void periodic_callbacks__100Hz(uint32_t callback_count) {
  motor_pwm__process_input();
}

void motor_pwm__set_pwm(void) {
  motor_pwm__set_motor_pwm();
  motor_pwm__set_servo_pwm();
}

static void motor_pwm__set_servo_pwm(void) {
  static float set_servo_duty_cycle = servo_pwm_idle_duty_cycle;

  if (motor_cmd_local.MOTOR_CMD_steer == 0) {
    // idle
    set_servo_duty_cycle = servo_pwm_idle_duty_cycle;
  } else if (motor_cmd_local.MOTOR_CMD_steer == 1) {
    // slightly right
    set_servo_duty_cycle = 17.00;
  } else if (motor_cmd_local.MOTOR_CMD_steer == 2) {
    // max right
    set_servo_duty_cycle = 19.00;
  } else if (motor_cmd_local.MOTOR_CMD_steer == -1) {
    // slightly left
    set_servo_duty_cycle = 13.00;
  } else if (motor_cmd_local.MOTOR_CMD_steer == -2) {
    // max left
    set_servo_duty_cycle = 11.00;
  }

  pwm1__set_duty_cycle(servo_pwm, set_servo_duty_cycle);
}

static void motor_pwm__set_motor_pwm(void) {
  static float set_motor_duty_cycle = motor_pwm_idle_duty_cycle;

  if (motor_stop_local.MOTOR_STOP_stop) {
    motor_speed_pid_set_target_pwm(motor_pwm_idle_duty_cycle);
  } else {
    if (!ignore_cmds) {
      if (motor_cmd_local.MOTOR_CMD_forward == -1) { // Reverse
        set_motor_duty_cycle = 14.50;
        if (reverse_flag == false) {
          motor_speed_pid_set_target_pwm(motor_pwm_idle_duty_cycle);
          ignore_cmds = true;
          start_time = sys_time__get_uptime_ms();
          reverse_flag = true;
          return;
        }
      } else if (motor_cmd_local.MOTOR_CMD_forward == 0) { // Stop
        set_motor_duty_cycle = motor_pwm_idle_duty_cycle;
      } else if (motor_cmd_local.MOTOR_CMD_forward == 3) { // Forward
        set_motor_duty_cycle = 16.2;                       // 15.7 originally?, 16.2

        if (reverse_flag == true) {
          motor_speed_pid_set_target_pwm(motor_pwm_idle_duty_cycle);
          ignore_cmds = true;
          start_time = sys_time__get_uptime_ms();
          reverse_flag = false;
          return;
        }
      }
      motor_speed_pid_set_target_pwm(set_motor_duty_cycle);
    } else if (sys_time__get_uptime_ms() - start_time >= DIRECTION_CHANGE_DELAY) {
      ignore_cmds = false;
    } else {
    }
  }
}

void periodic_callbacks__1Hz(uint32_t callback_count) {
  int gps_lock_status = geo_sensors_gps__get_gps_lock_status();
  // printf("GPS lock status: %d\n", gps_lock_status);
  if (!gps_lock_status) {
    gpio__toggle(board_io__get_led0());
  } else {
    gpio__reset(board_io__get_led0());
  }
}

void periodic_callbacks__10Hz(uint32_t callback_count) {

  geo_msg_bank__handle_msg();
  geo_msg_bank__service_mia_10hz();
  geo_msg_bank__transmit_msg();

  geo_sensors_gps__run_once();
  // geo_sensors_compass_lsm__run_once();
}

bool geo_sensors_gps__run_once(void) {
  bool gps_transfer_success = false;

  gps_transfer_success = geo_sensors_gps__transfer_data_from_uart_driver_to_line_buffer();
  gps__parse_coordinates_from_line();

  return gps_transfer_success;
}

bool geo_sensors_gps__transfer_data_from_uart_driver_to_line_buffer(void) {
  char byte_read;
  const uint32_t zero_timeout = 0;
  bool gps_status_reading = false;

  while (uart__get(uart_gps, &byte_read, zero_timeout)) {
    gps_status_reading = true;
    line_buffer__add_byte(&line, byte_read);
  }

  return gps_status_reading;
}

static void gps__parse_coordinates_from_line(void) {
  char gps_line[1000];
  if (line_buffer__remove_line(&line, gps_line, sizeof(gps_line))) {
    // Check if the line is a GPGGA stream
    if (strncmp(gps_line, "$GPGGA,", 7) == 0) {
      char *token;
      int field_idx = 0;
      int latitude_flag = 1;
      int longitude_flag = 1;
      float latitude = 0.0;
      float longitude = 0.0;

      token = strtok(gps_line, ",");
      while (token != NULL) {
        token = strtok(NULL, ",");
        field_idx++;

        if (field_idx == 2) { // Latitude is field 2 in GPGGA
          latitude = atof(token);
        } else if (field_idx == 3) { // N/S direction is field 3 in GPGGA
          char c = token[0];
          if (c == 'S') {
            latitude_flag *= -1;
          }
        } else if (field_idx == 4) { // Longitude is field 4 in GPGGA
          longitude = atof(token);
        } else if (field_idx == 5) { // E/W direction is field 5 in GPGGA
          char c = token[0];
          if (c == 'W') {
            longitude_flag *= -1;
          }
        } else if (field_idx == 6) {
          char c = token[0];
          // printf("GPS QUALITY: %c\n", c);
          if (c == '0') {
            gps_quality = 0;
          } else if (c == '1') {
            gps_quality = 1;
            // printf("GPS lock successful\n");
          } else {
            // printf("Trying to acquire GPS fix\n");
          }
        } else if (field_idx > 6) {
          break;
        }
      }

      int latitude_days = latitude / 100;
      int longitude_days = longitude / 100;
      float latitude_minutes = latitude - 3700.00;
      float longitude_minutes = longitude - 12100.00;

      latitude_minutes /= 60;
      longitude_minutes /= 60;

      parsed_coordinates.latitude = ((1.0 * latitude_days) + latitude_minutes) * latitude_flag;
      parsed_coordinates.longitude = ((1.0 * longitude_days) + longitude_minutes) * longitude_flag;
    }
  }

void geo_sensors_compass_lsm__run_once(void) { geo_sensors_compass__read(); }

void geo_sensors_compass__read(void) {
  uint8_t buffer[6];
  float mag[3];

  i2c__read_slave_data(current_i2c, magnetometer_read, LSM303AGR_MAG_OUTX_L_REG, buffer, 6);

  mag[0] = (int16_t)(buffer[1] << 8 | buffer[0]);
  mag[1] = (int16_t)(buffer[3] << 8 | buffer[2]);
  mag[2] = (int16_t)(buffer[5] << 8 | buffer[4]);

  geo_sensors_compass__calculate_heading(mag);
}

static void geo_sensors_compass__calculate_heading(float mag[3]) {
  float magnitude = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);

  const float epsilon = 1e-6;
  if (magnitude < epsilon) {
    current_compass_heading = 0;

  } else {
    float mag_xy = mag[1] / magnitude;
    float mag_xx = mag[0] / magnitude;
    current_compass_heading = (atan2(mag_xy, mag_xx)) * 180.0 / PI;

    if (current_compass_heading < 0) {
      current_compass_heading = 360 + current_compass_heading;
    }
  }
}

void geo_logic__haversine_calculation(void) {
  // Input: Latitude longitude, Output: heading, bearing, distance to destination
  gps_coordinates_t current_gps = geo_sensors_gps__get_coordinates();
  float current_latitude = current_gps.latitude;
  float current_longitude = current_gps.longitude;

  gps_coordinates_t destination_gps = geo_msg_bank__get_gps_destination_location();
  float destination_latitude = destination_gps.latitude;
  float destination_longitude = destination_gps.longitude;

  // Convert latitudes and longitudes from degrees to radians
  float phi1 = (float)(current_latitude * (float)PI_VAL / (float)180.0);
  float phi2 = (float)(destination_latitude * PI_VAL / (float)180.0);
  float delta_phi = (float)(destination_latitude - current_latitude) * PI_VAL / (float)180.0;
  float delta_lambda = (float)(destination_longitude - current_longitude) * PI_VAL / (float)180.0;

  // Haversine formula
  float a =
      sin(delta_phi / 2) * sin(delta_phi / 2) + cos(phi1) * cos(phi2) * sin(delta_lambda / 2) * sin(delta_lambda / 2);
  float c = 2 * atan2(sqrt(a), sqrt(1 - a));
  distance_to_destination = RADIUS_EARTH_KM * c * 1000;

  // Bearing calculation
  float y = sin(delta_lambda) * cos(phi2);
  float x = cos(phi1) * sin(phi2) - sin(phi1) * cos(phi2) * cos(delta_lambda);
  float bearing_rad = atan2(y, x);
  compass_bearing = fmod((bearing_rad * (float)180.0 / (float)PI_VAL + (float)360.0), 360.0);

  compass_heading = geo_msg_bank__get_heading();

  current_geo_status.GEO_STATUS_DISTANCE_TO_DESTINATION = distance_to_destination;
  current_geo_status.GEO_STATUS_COMPASS_BEARING = (uint8_t)(compass_bearing / 20);
  current_geo_status.GEO_STATUS_COMPASS_HEADING = (uint8_t)(compass_heading);
}

gps_coordinates_t find_next_point(gps_coordinates_t origin, gps_coordinates_t destination) {
  gps_coordinates_t closest_point;
  float min_distance = FLT_MAX;
  float distance;
  int checkpoint_idx = -1;

  for (int i = 0; i < NUM_OF_CHECKPOINTS; i++) {
    distance = get_distance_between_points(origin, locations_we_can_travel[i]);
    if (distance < min_distance) {
      if (get_distance_between_points(origin, locations_we_can_travel[i]) < 5) {
        printf("reached checkpoint\n");
        continue;
      }
      // Check if point is actually closer to destincation
      if (get_distance_between_points(locations_we_can_travel[i], destination) <
          get_distance_between_points(origin, destination)) {
        min_distance = distance;
        checkpoint_idx = i;
        printf("setting checkpoint\n");
      } else {
        continue;
      }
    }
  }
  if (checkpoint_idx < 0) {
    closest_point = destination;
    printf("Going destination\n");
  } else {
    printf("setting closest_point\n");
    closest_point = locations_we_can_travel[checkpoint_idx];
  }
  printf("p: %f, %f", closest_point.latitude, closest_point.longitude);
  return closest_point;
}

void periodic_callbacks__10Hz(uint32_t callback_count) {
  driver_msg_bank__handle_msg();
  driver_msg_bank__service_mia_10hz();
  driver_msg_bank__transmit_msg();
  i2c_lcd__clear();
  driver__lcd_display_data();
}

void periodic_callbacks__100Hz(uint32_t callback_count) {
  driver__process_input();
}

dbc_MOTOR_CMD_s driver__get_motor_commands(void) {
  driver__avoid_obstacle();
  return motor_commands;}

static void driver__avoid_obstacle(void) {
  obstacle_below_threshold_s below_thresholds = driver__closest_obstacle();

  if (!(below_thresholds.MIDDLE) && !(below_thresholds.LEFT) && !(below_thresholds.RIGHT)) {
    driver__go_to_destination();
  } else {
    driver__steering();
  }
  driver__led_indicators();}

dbc_MOTOR_CMD_s driver__get_motor_commands(void) {
  driver__avoid_obstacle();
  return motor_commands;
}

// slight turns for destination movement
static void driver__go_to_destination(void) {
  int angle_difference = local_geo_status.GEO_STATUS_COMPASS_HEADING - local_geo_status.GEO_STATUS_COMPASS_BEARING;
  if (local_geo_status.GEO_STATUS_DISTANCE_TO_DESTINATION > 0) {
    if (angle_difference > 0) {
      // go right
      motor_commands.MOTOR_CMD_steer = 1;
      motor_commands.MOTOR_CMD_forward = 3;
    } else if (angle_difference < 0) {
      // go left
      motor_commands.MOTOR_CMD_steer = -1;
      motor_commands.MOTOR_CMD_forward = 3;
    } else { // go straight
      motor_commands.MOTOR_CMD_steer = 0;
      motor_commands.MOTOR_CMD_forward = 3;
    }
  } else { // reached destination
    motor_commands.MOTOR_CMD_forward = 0;
    motor_commands.MOTOR_CMD_steer = 0;
  }}

void driver__lcd_display_data(void) {
  char debug_str[15];

  if (local_motor_stop.MOTOR_STOP_stop) {
    i2c_lcd__clear();
    i2c_lcd__starting_position(0, 0);
    i2c_lcd__printf("   MOTOR STOPPED!");
    i2c_lcd__starting_position(1, 0);
    i2c_lcd__printf(" PLS RESTART BRIDGE");
    i2c_lcd__starting_position(1, 0);
    i2c_lcd__printf("     TO CONTINUE");
  } else if (local_geo_status.GEO_STATUS_DISTANCE_TO_DESTINATION) {
    sprintf(debug_str, "%.1lf", local_geo_status.GEO_STATUS_DISTANCE_TO_DESTINATION);

    i2c_lcd__starting_position(0, 0);
    i2c_lcd__printf("   PROJECT TEAM X");

    i2c_lcd__starting_position(1, 0);
    i2c_lcd__printf("Distance:");
    i2c_lcd__starting_position(1, 10);
    i2c_lcd__printf(debug_str);

    sprintf(debug_str, "%d", (int)local_geo_status.GEO_STATUS_COMPASS_HEADING);
    i2c_lcd__starting_position(2, 0);
    i2c_lcd__printf("     Heading:");
    i2c_lcd__starting_position(2, 13);
    i2c_lcd__printf(debug_str);

    sprintf(debug_str, "%d", (int)local_geo_status.GEO_STATUS_COMPASS_BEARING);
    i2c_lcd__starting_position(3, 0);
    i2c_lcd__printf("     Bearing:");
    i2c_lcd__starting_position(3, 13);
    i2c_lcd__printf(debug_str);
  } else if (local_geo_status.GEO_STATUS_DISTANCE_TO_DESTINATION < 2) {
    i2c_lcd__clear();
    i2c_lcd__starting_position(1, 0);
    i2c_lcd__printf("DESTINATION REACHED!");
  }
}

void i2c_lcd__printf(char *line) {
  while (*line)
    i2c_lcd__send_data(*line++);
}

void i2c_lcd__starting_position(uint8_t y, uint8_t x) {

  char Starting_Addr = 0x80;
  switch (y) {
  case 0:
    Starting_Addr |= 0x00;
    break;
  case 1:
    Starting_Addr |= 0x40;
    break;
  case 2:
    Starting_Addr |= 0x14;
    break;
  case 3:
    Starting_Addr |= 0x54;
    break;
  default:;
  }

  Starting_Addr += x;

  i2c_lcd__send_command(0x80 | Starting_Addr);
}

 @IBAction func buttonPressed(_ sender: UIButton) {
        print("Scan bluetooth devices Button pressed")
        
        // Start scanning for Bluetooth devices if Bluetooth is turned on
        if centralManager.state == .poweredOn {
            centralManager.scanForPeripherals(withServices: nil, options: nil)
            print("Scanning for Bluetooth devices...")
        } else {
            print("Bluetooth is not turned on")
        }
    }

extension ViewController: CBCentralManagerDelegate {
    func centralManagerDidUpdateState(_ central: CBCentralManager) {
        switch central.state {
        case .poweredOn:
            print("Bluetooth is On")
        case .poweredOff:
            print("Bluetooth is Off")
        case .resetting:
            print("Bluetooth is Resetting")
        case .unauthorized:
            print("Bluetooth not authorized")
        case .unsupported:
            print("Bluetooth not supported")
        case .unknown:
            print("Bluetooth unknown state")
        @unknown default:
            print("A new case was added that is not handled")
        }
        
    }
    
    func centralManager(_ central: CBCentralManager, didDiscover peripheral: CBPeripheral, advertisementData: [String : Any], rssi RSSI: NSNumber) {
        
        if let name = peripheral.name, name != "Unknown Device" {
            // Avoid adding duplicates
            if !discoveredDevices.contains(where: { $0.identifier == peripheral.identifier }) {
                discoveredDevices.append(peripheral)
                DispatchQueue.main.async { [weak self] in self?.displayContent.reloadData()
                }
            }
        }
        
    }
    func centralManager(_ central: CBCentralManager, didConnect peripheral: CBPeripheral) {
        print("Connected to peripheral: \(peripheral)")
        
        selectedPeripheral = peripheral // Make sure to store the reference to the connected didFailToConnectperipheral
        peripheral.delegate = self // Set the delegate to self for handling peripheral events
        peripheral.discoverServices(nil)
    
    }
    
    func centralManager(_ central: CBCentralManager, didDisconnectPeripheral peripheral: CBPeripheral, error: Error?) {
        print("Disconnected from peripheral: \(peripheral)")
        if let error = error {
            print("Error: \(error.localizedDescription)")
        }
    }
    
    func centralManager(_ central: CBCentralManager, didFailToConnect peripheral: CBPeripheral, error: Error?) {
        print("Failed to connect to peripheral: \(peripheral), error: \(error?.localizedDescription ?? "Unknown error")")
        
        // Present an alert to the user with the option to try connecting again
        let alert = UIAlertController(title: "Connection Failed", message: "Failed to connect to the selected device. Would you like to try again?", preferredStyle: .alert)
        
        // Add a "Try Again" action to the alert
        alert.addAction(UIAlertAction(title: "Try Again", style: .default) { _ in
            // Retry connecting to the selected peripheral
            central.connect(peripheral, options: nil)
        })
        
        // Add a "Cancel" action to the alert
        alert.addAction(UIAlertAction(title: "Cancel", style: .cancel, handler: nil))
        
        // Present the alert
        self.present(alert, animated: true, completion: nil)
    }
}

import UIKit
import MapKit

protocol MapViewControllerDelegate: AnyObject {
    func didChooseLocation(latitude: Double, longitude: Double)
}


class CustomMapViewController: UIViewController, MKMapViewDelegate {

    @IBOutlet weak var mapView: MKMapView!
    weak var delegate: MapViewControllerDelegate?
    
    var lastSelectedLatitude: Double?
    var lastSelectedLongitude: Double?
    
    override func viewDidLoad() {
        super.viewDidLoad()
        mapView.delegate = self

        print("Map View loaded!")
        // Add a tap gesture recognizer to the map view
        let tapRecognizer = UITapGestureRecognizer(target: self, action: #selector(handleMapTap(_:)))
        mapView.addGestureRecognizer(tapRecognizer)
        
        // Define the center of the region your map should display
        // 37.33919199814887, -121.88103575173972
        let center = CLLocationCoordinate2D(latitude: 37.33919199814887, longitude: -121.88103575173972) // SJSU parking lot
        
        // Set the region of the map
        let region = MKCoordinateRegion(center: center, span: span)
        mapView.setRegion(region, animated: true)
    }

    @objc func handleMapTap(_ gestureReconizer: UITapGestureRecognizer) {
        let location = gestureReconizer.location(in: mapView)
        let coordinate = mapView.convert(location, toCoordinateFrom: mapView)

        // Remove all existing annotations
        mapView.removeAnnotations(mapView.annotations)
        
        // Add annotation:
        let annotation = MKPointAnnotation()
        annotation.coordinate = coordinate
        mapView.addAnnotation(annotation)
        
        lastSelectedLatitude = coordinate.latitude
        lastSelectedLongitude = coordinate.longitude
        
        print("Latitude: \(coordinate.latitude), Longitude: \(coordinate.longitude)")
        
    }
    
    @IBAction func buttonPressed(_ sender: UIButton) {
        print("Sending GPS destination location to Bridge Controller!")
        
        if let currentlatitude = lastSelectedLatitude, let currentlongitude = lastSelectedLongitude {
            delegate?.didChooseLocation(latitude: currentlatitude, longitude: currentlongitude)
            print("Coordinates sent: Latitude: \(currentlatitude), Longitude: \(currentlongitude)")
        } else {
            print("No location selected.")
        }
    }
    
}

@IBAction func buttonPressed2(_ sender: UIButton) {
        print("STOP Button pressed")
        
        sendStopToPeripheral()
    }

extension ViewController {
    func sendStopToPeripheral() {
        guard let peripheral = selectedPeripheral,
              let characteristic = writeCharacteristic,
              characteristic.properties.contains(.write) || characteristic.properties.contains(.writeWithoutResponse),
              let stopData = "S".data(using: .utf8),
              let stringRepresentation = String(data: stopData, encoding: .utf8) else {
                print("Cannot send data: Peripheral or characteristic not set up correctly.")
            return
          
        }

        
        peripheral.writeValue(stopData, for: characteristic, type: .withResponse) // or .withoutResponse
        print("stopData sent: \(stringRepresentation)")
        
        if characteristic.uuid == CBUUID(string: "6E400002-B5A3-F393-E0A9-E50E24DCCA9E") {
            //writeCharacteristic = characteristic
            print("Adafruit BLE write characteristic FOUND")
            peripheral.writeValue(stopData, for: characteristic, type: .withResponse) // or .withoutResponse
            print("stopData sent: \(stopData)")
            print("stopData sent str: \(stringRepresentation)")
        }
        else{
            print("Adafruit BLE write characteristic NOT FOUND")
        }
    }
}

func peripheral(_ peripheral: CBPeripheral, didDiscoverCharacteristicsFor service: CBService, error: Error?) {
    if let error = error {
        print("Error discovering characteristics: \(error.localizedDescription)")
    }
    guard let characteristics = service.characteristics else {
        print("No characteristics discovered for service: \(service.uuid)")
        return
    }
    
    for characteristic in characteristics {
        print("Discovered characteristic: \(characteristic.uuid)")
        
        // Check if this characteristic supports write operations
        if characteristic.properties.contains(.write) || characteristic.properties.contains(.writeWithoutResponse) {
            writeCharacteristic = characteristic
            print("Write characteristic found: \(characteristic.uuid)")
            
        }
        
        if characteristic.uuid == CBUUID(string: "6E400003-B5A3-F393-E0A9-E50E24DCCA9E") {
            readCharacteristic = characteristic
            print("Read characteristic found: \(characteristic.uuid)")
            
            // Now that we have the read characteristic, we can read data
            peripheral.setNotifyValue(true, for: characteristic) // Enable notifications for this characteristic
            peripheral.readValue(for: characteristic) // Read the initial value
            print("Reading data...")
        }
        
    }
}