Improvements crazyflie

projects/robots/bitcraze/crazyflie/controllers/crazyflie/crazyflie.c

@@ -42,6 +42,8 @@
 // Add external controller
 #include "pid_controller.h"
 
+#define FLYING_ALTITUDE 1.0
+
 int main(int argc, char **argv) {
   wb_robot_init();
 
@@ -102,14 +104,15 @@ int main(int argc, char **argv) {
   gains_pid.kp_vel_xy = 2;
   gains_pid.kd_vel_xy = 0.5;
   gains_pid.kp_z = 10;
-  gains_pid.ki_z = 50;
+  gains_pid.ki_z = 5;
   gains_pid.kd_z = 5;
   init_pid_attitude_fixed_height_controller();
 
+  double height_desired = FLYING_ALTITUDE;
+
   // Initialize struct for motor power
   motor_power_t motor_power;
 
-  printf(" Take off! \n");
   printf("\n");
 
   printf("====== Controls =======\n");
@@ -118,6 +121,7 @@ int main(int argc, char **argv) {
   printf(" All controllable movement is in body coordinates\n");
   printf("- Use the up, back, right and left button to move in the horizontal plane\n");
   printf("- Use Q and E to rotate around yaw\n ");
+  printf("- Use W and S to go up and down\n");
 
   while (wb_robot_step(timestep) != -1) {
     const double dt = wb_robot_get_time() - past_time;
@@ -150,33 +154,42 @@ int main(int argc, char **argv) {
     double forward_desired = 0;
     double sideways_desired = 0;
     double yaw_desired = 0;
+    double height_diff_desired = 0;
 
     // Control altitude
     int key = wb_keyboard_get_key();
     while (key > 0) {
       switch (key) {
         case WB_KEYBOARD_UP:
-          forward_desired = +0.2;
+          forward_desired = +0.5;
           break;
         case WB_KEYBOARD_DOWN:
-          forward_desired = -0.2;
+          forward_desired = -0.5;
           break;
         case WB_KEYBOARD_RIGHT:
-          sideways_desired = -0.2;
+          sideways_desired = -0.5;
           break;
         case WB_KEYBOARD_LEFT:
-          sideways_desired = +0.2;
+          sideways_desired = +0.5;
           break;
         case 'Q':
-          yaw_desired = 0.5;
+          yaw_desired = 1.0;
           break;
         case 'E':
-          yaw_desired = -0.5;
+          yaw_desired = -1.0;
+          break;
+        case 'W':
+          height_diff_desired = 0.1;
+          break;
+        case 'S':
+          height_diff_desired = -0.1;
           break;
       }
       key = wb_keyboard_get_key();
     }
 
+    height_desired += height_diff_desired * dt;
+
     // Example how to get sensor data
     // range_front_value = wb_distance_sensor_get_value(range_front));
     // const unsigned char *image = wb_camera_get_image(camera);
@@ -186,6 +199,7 @@ int main(int argc, char **argv) {
     // PID velocity controller with fixed height
     desired_state.vy = sideways_desired;
     desired_state.vx = forward_desired;
+    desired_state.altitude = height_desired;
     pid_velocity_fixed_height_controller(actual_state, &desired_state, gains_pid, dt, &motor_power);
 
     // Setting motorspeed

projects/robots/bitcraze/crazyflie/controllers/crazyflie/pid_controller.c

@@ -41,6 +41,7 @@ float constrain(float value, const float minVal, const float maxVal) {
 
 double pastAltitudeError, pastPitchError, pastRollError, pastYawRateError;
 double pastVxError, pastVyError;
+double altitudeIntegrator;
 
 void init_pid_attitude_fixed_height_controller() {
   pastAltitudeError = 0;
@@ -49,6 +50,7 @@ void init_pid_attitude_fixed_height_controller() {
   pastRollError = 0;
   pastVxError = 0;
   pastVyError = 0;
+  altitudeIntegrator = 0;
 }
 
 void pid_attitude_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
@@ -74,6 +76,10 @@ void pid_fixed_height_controller(actual_state_t actual_state, desired_state_t *d
   double altitudeDerivativeError = (altitudeError - pastAltitudeError) / dt;
   control_commands->altitude =
     gains_pid.kp_z * constrain(altitudeError, -1, 1) + gains_pid.kd_z * altitudeDerivativeError + gains_pid.ki_z;
+
+  altitudeIntegrator += altitudeError * dt;
+  control_commands->altitude = gains_pid.kp_z * constrain(altitudeError, -1, 1) + gains_pid.kd_z * altitudeDerivativeError +
+                               gains_pid.ki_z * altitudeIntegrator + 48;
   pastAltitudeError = altitudeError;
 }
 

projects/robots/bitcraze/crazyflie/controllers/crazyflie_py_wallfollowing/crazyflie_py_wallfollowing.py

@@ -0,0 +1,206 @@
+# -*- coding: utf-8 -*-
+#
+#  ...........       ____  _ __
+#  |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+#  | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  | / ,..´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#     +.......   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+
+# MIT License
+
+# Copyright (c) 2023 Bitcraze
+
+
+"""
+file: crazyflie_py_wallfollowing.py
+
+Controls the crazyflie and implements a wall following method in webots in Python
+
+Author:   Kimberly McGuire (Bitcraze AB)
+"""
+
+
+from controller import Robot
+from controller import Keyboard
+
+from math import cos, sin
+
+from pid_controller import pid_velocity_fixed_height_controller
+from wall_following import WallFollowing
+
+FLYING_ATTITUDE = 1
+
+if __name__ == '__main__':
+
+    robot = Robot()
+    timestep = int(robot.getBasicTimeStep())
+
+    # Initialize motors
+    m1_motor = robot.getDevice("m1_motor")
+    m1_motor.setPosition(float('inf'))
+    m1_motor.setVelocity(-1)
+    m2_motor = robot.getDevice("m2_motor")
+    m2_motor.setPosition(float('inf'))
+    m2_motor.setVelocity(1)
+    m3_motor = robot.getDevice("m3_motor")
+    m3_motor.setPosition(float('inf'))
+    m3_motor.setVelocity(-1)
+    m4_motor = robot.getDevice("m4_motor")
+    m4_motor.setPosition(float('inf'))
+    m4_motor.setVelocity(1)
+
+    # Initialize Sensors
+    imu = robot.getDevice("inertial_unit")
+    imu.enable(timestep)
+    gps = robot.getDevice("gps")
+    gps.enable(timestep)
+    gyro = robot.getDevice("gyro")
+    gyro.enable(timestep)
+    camera = robot.getDevice("camera")
+    camera.enable(timestep)
+    range_front = robot.getDevice("range_front")
+    range_front.enable(timestep)
+    range_left = robot.getDevice("range_left")
+    range_left.enable(timestep)
+    range_back = robot.getDevice("range_back")
+    range_back.enable(timestep)
+    range_right = robot.getDevice("range_right")
+    range_right.enable(timestep)
+
+    # Get keyboard
+    keyboard = Keyboard()
+    keyboard.enable(timestep)
+
+    # Initialize variables
+
+    past_x_global = 0
+    past_y_global = 0
+    past_time = 0
+    first_time = True
+
+    # Crazyflie velocity PID controller
+    PID_crazyflie = pid_velocity_fixed_height_controller()
+    PID_update_last_time = robot.getTime()
+    sensor_read_last_time = robot.getTime()
+
+    height_desired = FLYING_ATTITUDE
+
+    wall_following = WallFollowing(angle_value_buffer=0.01, reference_distance_from_wall=0.5,
+                                   max_forward_speed=0.3, init_state=WallFollowing.StateWallFollowing.FORWARD)
+
+    autonomous_mode = False
+
+    print("\n")
+
+    print("====== Controls =======\n\n")
+
+    print(" The Crazyflie can be controlled from your keyboard!\n")
+    print(" All controllable movement is in body coordinates\n")
+    print("- Use the up, back, right and left button to move in the horizontal plane\n")
+    print("- Use Q and E to rotate around yaw\n ")
+    print("- Use W and S to go up and down\n ")
+    print("- Press A to start autonomous mode\n")
+    print("- Press D to disable autonomous mode\n")
+
+    # Main loop:
+    while robot.step(timestep) != -1:
+
+        dt = robot.getTime() - past_time
+        actual_state = {}
+
+        if first_time:
+            past_x_global = gps.getValues()[0]
+            past_y_global = gps.getValues()[1]
+            past_time = robot.getTime()
+            first_time = False
+
+        # Get sensor data
+        roll = imu.getRollPitchYaw()[0]
+        pitch = imu.getRollPitchYaw()[1]
+        yaw = imu.getRollPitchYaw()[2]
+        yaw_rate = gyro.getValues()[2]
+        x_global = gps.getValues()[0]
+        v_x_global = (x_global - past_x_global)/dt
+        y_global = gps.getValues()[1]
+        v_y_global = (y_global - past_y_global)/dt
+        altitude = gps.getValues()[2]
+
+        # Get body fixed velocities
+        cos_yaw = cos(yaw)
+        sin_yaw = sin(yaw)
+        v_x = v_x_global * cos_yaw + v_y_global * sin_yaw
+        v_y = - v_x_global * sin_yaw + v_y_global * cos_yaw
+
+        # Initialize values
+        desired_state = [0, 0, 0, 0]
+        forward_desired = 0
+        sideways_desired = 0
+        yaw_desired = 0
+        height_diff_desired = 0
+
+        key = keyboard.getKey()
+        while key > 0:
+            if key == Keyboard.UP:
+                forward_desired += 0.5
+            elif key == Keyboard.DOWN:
+                forward_desired -= 0.5
+            elif key == Keyboard.RIGHT:
+                sideways_desired -= 0.5
+            elif key == Keyboard.LEFT:
+                sideways_desired += 0.5
+            elif key == ord('Q'):
+                yaw_desired = + 1
+            elif key == ord('E'):
+                yaw_desired = - 1
+            elif key == ord('W'):
+                height_diff_desired = 0.1
+            elif key == ord('S'):
+                height_diff_desired = - 0.1
+            elif key == ord('A'):
+                if autonomous_mode is False:
+                    autonomous_mode = True
+                    print("Autonomous mode: ON")
+            elif key == ord('D'):
+                if autonomous_mode is True:
+                    autonomous_mode = False
+                    print("Autonomous mode: OFF")
+            key = keyboard.getKey()
+
+        height_desired += height_diff_desired * dt
+
+        camera_data = camera.getImage()
+
+        # get range in meters
+        range_front_value = range_front.getValue() / 1000
+        range_right_value = range_right.getValue() / 1000
+        range_left_value = range_left.getValue() / 1000
+
+        # Choose a wall following direction
+        # if you choose direction left, use the right range value
+        # if you choose direction right, use the left range value
+        direction = WallFollowing.WallFollowingDirection.LEFT
+        range_side_value = range_right_value
+
+        # Get the velocity commands from the wall following state machine
+        cmd_vel_x, cmd_vel_y, cmd_ang_w, state_wf = wall_following.wall_follower(
+            range_front_value, range_side_value, yaw, direction, robot.getTime())
+
+        if autonomous_mode:
+            sideways_desired = cmd_vel_y
+            forward_desired = cmd_vel_x
+            yaw_desired = cmd_ang_w
+
+        # PID velocity controller with fixed height
+        motor_power = PID_crazyflie.pid(dt, forward_desired, sideways_desired,
+                                        yaw_desired, height_desired,
+                                        roll, pitch, yaw_rate,
+                                        altitude, v_x, v_y)
+
+        m1_motor.setVelocity(-motor_power[0])
+        m2_motor.setVelocity(motor_power[1])
+        m3_motor.setVelocity(-motor_power[2])
+        m4_motor.setVelocity(motor_power[3])
+
+        past_time = robot.getTime()
+        past_x_global = x_global
+        past_y_global = y_global