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,14 +199,15 @@ 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
     wb_motor_set_velocity(m1_motor, -motor_power.m1);
     wb_motor_set_velocity(m2_motor, motor_power.m2);
     wb_motor_set_velocity(m3_motor, -motor_power.m3);
     wb_motor_set_velocity(m4_motor, motor_power.m4);
-
+    
     // Save past time for next time step
     past_time = wb_robot_get_time();
     past_x_global = x_global;

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

@@ -41,14 +41,16 @@ float constrain(float value, const float minVal, const float maxVal) {
 
 double pastAltitudeError, pastPitchError, pastRollError, pastYawRateError;
 double pastVxError, pastVyError;
+double altIntergrator;
 
 void init_pid_attitude_fixed_height_controller() {
-  pastAltitudeError = 0;
-  pastYawRateError = 0;
-  pastPitchError = 0;
-  pastRollError = 0;
-  pastVxError = 0;
-  pastVyError = 0;
+    pastAltitudeError = 0;
+    pastYawRateError = 0;
+    pastPitchError = 0;
+    pastRollError = 0;
+    pastVxError = 0;
+    pastVyError = 0;
+    altIntergrator = 0;
 }
 
 void pid_attitude_fixed_height_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
@@ -74,11 +76,14 @@ 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;
-  pastAltitudeError = altitudeError;
+
+    altIntergrator += altitudeError*dt;
+    control_commands->altitude = gains_pid.kp_z * constrain(altitudeError, -1, 1) + gains_pid.kd_z*altitudeDerivativeError + gains_pid.ki_z*altIntergrator+48;
+    pastAltitudeError = altitudeError;
 }
 
 void motor_mixing(control_commands_t control_commands, motor_power_t *motorCommands) {
-  // Motor mixing
+    // Motor mixing
   motorCommands->m1 = control_commands.altitude - control_commands.roll + control_commands.pitch + control_commands.yaw;
   motorCommands->m2 = control_commands.altitude - control_commands.roll - control_commands.pitch - control_commands.yaw;
   motorCommands->m3 = control_commands.altitude + control_commands.roll - control_commands.pitch + control_commands.yaw;
@@ -87,7 +92,7 @@ void motor_mixing(control_commands_t control_commands, motor_power_t *motorComma
 
 void pid_attitude_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid, double dt,
                              control_commands_t *control_commands) {
-  // Calculate errors
+    // Calculate errors
   double pitchError = desired_state->pitch - actual_state.pitch;
   double pitchDerivativeError = (pitchError - pastPitchError) / dt;
   double rollError = desired_state->roll - actual_state.roll;
@@ -98,11 +103,11 @@ void pid_attitude_controller(actual_state_t actual_state, desired_state_t *desir
   control_commands->roll = gains_pid.kp_att_rp * constrain(rollError, -1, 1) + gains_pid.kd_att_rp * rollDerivativeError;
   control_commands->pitch = -gains_pid.kp_att_rp * constrain(pitchError, -1, 1) - gains_pid.kd_att_rp * pitchDerivativeError;
   control_commands->yaw = gains_pid.kp_att_y * constrain(yawRateError, -1, 1);
-
-  // Save error for the next round
+    
+    // Save error for the next round
   pastPitchError = pitchError;
   pastRollError = rollError;
-  pastYawRateError = yawRateError;
+    pastYawRateError = yawRateError;
 }
 
 void pid_horizontal_velocity_controller(actual_state_t actual_state, desired_state_t *desired_state, gains_pid_t gains_pid,
@@ -115,11 +120,11 @@ void pid_horizontal_velocity_controller(actual_state_t actual_state, desired_sta
   // PID control
   double pitchCommand = gains_pid.kp_vel_xy * constrain(vxError, -1, 1) + gains_pid.kd_vel_xy * vxDerivative;
   double rollCommand = -gains_pid.kp_vel_xy * constrain(vyError, -1, 1) - gains_pid.kd_vel_xy * vyDerivative;
-
+    
   desired_state->pitch = pitchCommand;
   desired_state->roll = rollCommand;
 
-  // Save error for the next round
-  pastVxError = vxError;
-  pastVyError = vyError;
+    // Save error for the next round
+    pastVxError = vxError;
+    pastVyError = vyError;
 }

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

@@ -0,0 +1,198 @@
+#  ...........       ____  _ __
+#  |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+#  | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  | / ,..´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#     +.......   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+
+# MIT License
+
+# Copyright (c) 2022 Bitcraze
+
+# @file crazyflie_controllers_py.py
+# Controls the crazyflie motors in webots in Python
+
+"""crazyflie_py_wallfollowing.py controller."""
+
+
+from controller import Robot
+from controller import Motor
+from controller import InertialUnit
+from controller import GPS
+from controller import Gyro
+from controller import Keyboard
+from controller import Camera
+from controller import DistanceSensor
+
+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_CF = 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, ref_distance_from_wall = 0.5, max_forward_speed = 0.3, init_state = WallFollowing.StateWF.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 ");
+    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
+        cosyaw = cos(yaw)
+        sinyaw = sin(yaw)
+        v_x = v_x_global * cosyaw + v_y_global * sinyaw
+        v_y = - v_x_global * sinyaw + v_y_global * cosyaw
+
+        ## 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
+
+        ## Example how to get sensor data
+        ## range_front_value = range_front.getValue();
+        ## cameraData = camera.getImage()
+        range_front_value = range_front.getValue()/1000
+        range_right_value = range_right.getValue()/1000
+
+        direction  = 1
+        cmd_vel_x, cmd_vel_y, cmd_ang_w, state_wf = wall_following.wall_follower(range_front_value, range_right_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_CF.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