Index>Robot Kit>RaspClaws Spider Robot Kit for RPi>Lesson 2 Introducing Steady Function

Lesson 2 Introducing Steady Function

1961

Lesson 2 Introducing Steady Function

The steady function is developed based on MPU6050.

2.1 Function Overview

After enabling the self-balancing function, the robot camera's viewing angle will remain horizontal. When this function is working normally, the robot cannot perform any other operations. Click again to disable the feature.

2.2 Running the steady Function

The web terminal and GUI are not connected. Therefore, to control the robot via GUI, you need to manually run server.py on Raspberry Pi. (similar to manually running webserver.py, the only difference is the server.py program)

1. Power on the robot. It may take 30-50s.

2. After it's booted, remotely log into the Raspberry pi. Terminate the auto-run program webServer.py, and run the server.py program.

Terminate the auto-run program webServer.py:

sudo killall python3

image.png 

Run the server.py program:

sudo python3 adeept_raspclaws/server/server.py

image.png 

3. After the server.py run successfully on the Raspberry Pi, run GUI.py on Windows.  

Run GUI.py in cmd. If you right click on the GUI.py file in the folder, there may be an error message.

Access the folder of the robot program:

image.png 

Run the GUI.py program (run it after accessing the client folder, or there may be an error).

Python GUI.py

image.png 

The GUI interface will appear after run successfully.

image.png 

4. Enter the IP address of the Raspberry Pi in the GUI on PC, click Connect, and you can control the robot now. For instance, enter 192.168.3.242.

image.png 

5. Click the Steady button and The raspclaws camera will remain level. 

6. Click Steady again to disable the  function.

5.5 Main Program

The code of this tutorial lies in the SpiderG.py. Here only the structure of the program is mentioned. The code run based on some function or parameters in the SpiderG.py.

1. def ctrl_range(raw, max_genout, min_genout):    

2.     '''''  

3.     This function is used to limit the servo PWM to a certain range.  

4.     '''    

5.     if raw > max_genout:    

6.         raw_output = max_genout    

7.     elif raw < min_genout:    

8.         raw_output = min_genout    

9.     else:    

10.         raw_output = raw    

11.     return int(raw_output)    

12.      

13. def status_GenOut(height_input, pitch_input, roll_input):    

14.     '''''  

15.     Pose control function has been introduced in the previous chapter. 

16.     '''    

17.     FL_input = wiggle_v*pitch_input + wiggle_v*roll_input    

18.     FR_input = wiggle_v*pitch_input - wiggle_v*roll_input    

19.     

20.     HL_input = - wiggle_v*pitch_input + wiggle_v*roll_input    

21.     HR_input = - wiggle_v*pitch_input - wiggle_v*roll_input    

22. def leg_FL_status():    

23. goal_dict['FLB'] = FLB_init_pwm    

24. goal_dict['FLM'] = ctrl_range(int(FLM_init_pwm + (height_input + FL_input)*FLM_direction),     

25.                                       max_dict['FLM'], min_dict['FLM'])    

26.     goal_dict['FLE'] = FLE_init_pwm    

27.         

28. def leg_FR_status():    

29. goal_dict['FRB'] = FRB_init_pwm    

30. goal_dict['FRM'] = ctrl_range(int(FRM_init_pwm + (height_input + FR_input)*FRM_direction),     

31.                                       max_dict['FRM'], min_dict['FRM'])    

32.     goal_dict['FRE'] = FRE_init_pwm    

33.     

34. def leg_HL_status():    

35.     goal_dict['HLB'] = HLB_init_pwm    

36. goal_dict['HLM'] = ctrl_range(int(HLM_init_pwm + (height_input + HL_input)*HLM_direction),     

37.                                       max_dict['FRM'], min_dict['FRM'])    

38.  goal_dict['HLE'] = HLE_init_pwm    

39.     

40. def leg_HR_status():    

41. goal_dict['HRB'] = HRB_init_pwm    

42. goal_dict['HRM'] = ctrl_range(int(HRM_init_pwm + (height_input + HR_input)*HRM_direction),     

43.                                       max_dict['FRM'], min_dict['FRM'])    

44.     goal_dict['HRE'] = HRE_init_pwm    

45.    

46. leg_FL_status()    

47. leg_FR_status()    

48. leg_HL_status()    

49. leg_HR_status()    

50. print(goal_dict['FLM'])    

51.     

52. def steady():    

53.     global sensor    

54.     '''''  

55.    Determining whether the self-stabilization function is turned on.  

56.     '''    

57.     if steadyMode:    

58.         '''''''  

59.         Determining whether the MPU6050 is connected.  

60.         '''    

61.         if MPU_connection:    

62.           try:    

63.                '''''  

64.                Reading the data of MPU6050.  

65.                '''    

66.                 accelerometer_data = sensor.get_accel_data()                   

67.                 '''''  

68.                 The data read by the Kalman filter algorithm. 

69.                 '''    

70.                 X = accelerometer_data['x']    

71.                 X = kalman_filter_X.kalman(X)    

72.                 Y = accelerometer_data['y']    

73.                 Y = kalman_filter_Y.kalman(Y)                   

74.                 '''''  

75.               Calculating angle deviation.  

76.                 '''    

77.                 X_error = X-X_steady    

78.                 Y_error = Y-Y_steady                   

79.                 '''''  

80.                 mpu_tor is the action threshold of the self-stabilization mode. If the deviation exceeds this threshold, the robot will start to move. If the deviation is less than this threshold, the robot will not move. Since the deviation is inevitable, in order to avoid the robot constantly shaking, we use this threshold to judge whether angle compensation is required.  

81.                 '''    

82.                 if abs(X_error)>mpu_tor or abs(Y_error)>mpu_tor:    

83.                     '''''  

84.                The error is applied to the pose control function to compensate for the deviation. The P value is the proportional integral in automatic control. The larger the value, the more sensitive the robot movements. However, excessive proportional integral will cause the robot to overshoot and cause severe jitter.  

85.  

86.                     '''    

87.                     status_GenOut(0, X_error*P, Y_error*P)    

88.                     '''''  

89.                   The calculated target posture 

90.                     '''    

91.                     direct_M_move()    

92.            except:    

93.                 time.sleep(0.1)    

94.                 '''''  

95.                There is a certain probability that the MPU6050 will cause a connection failure when performing high-frequency reading. If the connection fails, then you can re-establish the I2C connection with the MPU6050. 

96.  '''  

97.                   sensor = mpu6050(0x68)