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);
}

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);
}