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
Run the server.py program:
sudo python3 adeept_raspclaws/server/server.py
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:
Run the GUI.py program (run it after accessing the client folder, or there may be an error).
Python GUI.py
The GUI interface will appear after run successfully.
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.
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) |