#include <Servo.h> // Include the Servo library
Servo servo1; // Create a new Servo object
Servo servo2;

int servo1Pos = 90; // Set the initial position for servo1
int servo2Pos = 90;

int trigPin = 9;    // Trigger pin of the ultrasonic sensor
int echoPin = 10;   // Echo pin of the ultrasonic sensor
long duration, distance;

int tempPin = A0; // Pin the temperature sensor is connected to

void setup() {
  servo1.attach(3); // Attach servo1 to pin 3
  servo2.attach(5); // Attach servo2 to pin 5
  Serial.begin(9600); // Begin serial communication at 9600 baud

  pinMode(trigPin, OUTPUT); // Set the trigger pin as an output
  pinMode(echoPin, INPUT); // Set the echo pin as an input
}

void loop() {
  // Move the servos to create the Carangiform locomotion
  servo1.write(servo1Pos + sin(millis() * 0.01) * 30);
  servo2.write(servo2Pos + sin(millis() * 0.01 + PI) * 30);

  // Read the ultrasonic sensor to perform obstacle avoidance
  digitalWrite(trigPin, LOW); // Set the trigger pin low
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); // Set the trigger pin high
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); // Set the trigger pin low
  duration = pulseIn(echoPin, HIGH); // Read the pulse width of the echo pin
  distance = duration * 0.034 / 2; // Calculate the distance in centimeters

  if (distance < 30) { // If an obstacle is detected within 30 cm
    servo1.write(servo1Pos + 30); // Rotate the robot to the left
    servo2.write(servo2Pos - 30);
    delay(1000); // Wait for 1 second
  }
  
  // Read the temperature sensor and output the readings to the serial monitor
  int tempReading = analogRead(tempPin);
  float voltage = tempReading * 5.0 / 1023.0; // Convert the reading to voltage
  float temperature = (voltage - 0.5) * 100.0; // Convert the voltage to temperature
  Serial.print("Temperature: ");
  Serial.print(temperature);
  Serial.println(" degrees Celsius");
}