1+ /* This example shows how to create WiFi Access point
2+ * and calibrate PID for balancing bot.
3+ *
4+ * Created 14/08/2024
5+ * By Augmented Robotics
6+ *
7+ * Check out https://roboheart.de/en_gb/ for more information about RoboHeart.
8+ */
9+
10+
11+ #include < RoboHeart.h>
12+ #include < WiFi.h>
13+ #include < AsyncTCP.h>
14+ #include < ESPAsyncWebServer.h>
15+
16+
17+ #include < RoboHeart.h>
18+ #include < RoboHeartTimer.h>
19+
20+ RoboHeart heart = RoboHeart(Serial);
21+
22+ // PID controller parameters
23+ int Kp = 0 ;
24+ int Kd = 0 ;
25+ int Ki = 0 ;
26+
27+ #define CONTROL_TICK_PERIOD_US 100.0
28+
29+ #define PID_CONTROL_PRESCALER \
30+ 5 // Control ticks passing before PID motorPower Calculation
31+ #define DC_CONTROL_PRESCALER 15 // Control ticks passing before DC Motor Control
32+ #define STATISTICS_PRESCALER \
33+ DC_CONTROL_PRESCALER * \
34+ 4 // Control ticks passing before all the debug printing is performed
35+
36+ // Motor control parameters
37+ float prevAngleDeg = 0 ; // Store previos angle
38+ float motorPower = 0 ; // Calculated Power to be supplied to the motors
39+ float errorSum = 0 ; // For Integral part of the PID controller
40+ float currentAngleDeg = 0 ; // Current Angle
41+ float targetAngleDeg = 0 .; // Target Angle for the Balancing bot
42+ float offsetAngleDeg = 0 .; // Stable vertical position angle
43+
44+ // Track the timer activation for each task individually
45+ unsigned long pidControlTick = 0 ;
46+ unsigned long dcControlTick = 0 ;
47+ unsigned long statisticsTick = 0 ;
48+
49+ // Store previous time
50+ unsigned long prevTimeIntervalMS = 0 ;
51+
52+ // handle -180 after crossing 180 Degrees
53+ float processAngle (float angle) {
54+ // handle -180 after crossing 180
55+ if (angle > 180 ) {
56+ return angle - 360 ;
57+ }
58+ return angle;
59+ }
60+
61+ // Periodic timer executes the control ticks
62+ void tick () {
63+ // reduce code to minimal in order to avoid errors
64+ pidControlTick++;
65+ dcControlTick++;
66+ statisticsTick++;
67+ }
68+
69+ // When Button is pressed the current angle is saved
70+ // and later used to indicate stable vertical position
71+ // of the Balancing Bot.
72+ void processPinInterrupt () {
73+ heart.resetGyro ();
74+ errorSum = 0 ;
75+ }
76+
77+ // Periodic timer executes the control ticks
78+ PeriodicTimer timer = PeriodicTimer(tick, CONTROL_TICK_PERIOD_US, Serial);
79+
80+ #define ROBOHEART_AP_SSID " RoboHeart PID Fitting"
81+ #define ROBOHEART_AP_PASSWORD " Hercules4Makers"
82+
83+ AsyncWebServer server (80 );
84+ // Current time
85+ unsigned long currentTime = millis();
86+ // Previous time
87+ unsigned long previousTime = 0 ;
88+ // Define timeout time in milliseconds (example: 2000ms = 2s)
89+ const long timeoutTime = 2000 ;
90+
91+ const char * PARAM_INPUT_1 = " param_Kp"
92+ const char * PARAM_INPUT_2 = " param_Ki"
93+ const char * PARAM_INPUT_3 = " param_Kd"
94+
95+ const char index_html[] PROGMEM = R"rawliteral(
96+ <!DOCTYPE HTML><html><head>
97+ <title>RoboHeart PID Fitting Form</title>
98+ <meta name="viewport" content="width=device-width, initial-scale=1">
99+ </head><body>
100+ <body><h1>RoboHeart PID Fitting</h1>
101+ <form action="/setKp">
102+ Kp: <input type="text" name="param_Kp">
103+ <input type="submit" value="Submit">
104+ </form><br>
105+ <form action="/setKi">
106+ Ki: <input type="text" name="param_Ki">
107+ <input type="submit" value="Submit">
108+ </form><br>
109+ <form action="/setKd">
110+ Kd: <input type="text" name="param_Kd">
111+ <input type="submit" value="Submit">
112+ </form>
113+ </body></html>)rawliteral" ;
114+
115+ void setup ()
116+ {
117+ Serial.begin (115200 );
118+ Serial.println (" \n Creating AP."
119+ WiFi.mode (WIFI_AP);
120+ WiFi.softAP (ROBOHEART_AP_SSID, ROBOHEART_AP_PASSWORD);
121+ Serial.print (" AP Created with IP Gateway "
122+ Serial.println (WiFi.softAPIP ());
123+
124+ // Send web page with input fields to client
125+ server.on (" /"
126+ request->send_P (200 , " text/html"
127+ });
128+
129+ // Send a GET request to <ESP_IP>/setKp?param_Kp=<inputMessage>
130+ server.on (" /setKp"
131+ String inputMessage;
132+ String inputParam;
133+ // GET Kp value on <ESP_IP>/setKp?param_Kp=<inputMessage>
134+ if (request->hasParam (PARAM_INPUT_1)) {
135+ inputMessage = request->getParam (PARAM_INPUT_1)->value ();
136+ inputParam = PARAM_INPUT_1;
137+ }
138+ else {
139+ inputMessage = " No message sent"
140+ inputParam = " none"
141+ }
142+ Serial.println (inputMessage);
143+ request->send (200 , " text/html" " HTTP GET request sent to your ESP on input field ("
144+ + inputParam + " ) with value: "
145+ " <br><a href=\" /\" >Return to Home Page</a>"
146+ });
147+
148+ // Send a GET request to <ESP_IP>/setKi?param_Ki=<inputMessage>
149+ server.on (" /setKi"
150+ String inputMessage;
151+ String inputParam;
152+ // GET Kp value on <ESP_IP>/setKi?param_Ki=<inputMessage>
153+ if (request->hasParam (PARAM_INPUT_2)) {
154+ inputMessage = request->getParam (PARAM_INPUT_2)->value ();
155+ inputParam = PARAM_INPUT_2;
156+ }
157+ else {
158+ inputMessage = " No message sent"
159+ inputParam = " none"
160+ }
161+ Serial.println (inputMessage);
162+ request->send (200 , " text/html" " HTTP GET request sent to your ESP on input field ("
163+ + inputParam + " ) with value: "
164+ " <br><a href=\" /\" >Return to Home Page</a>"
165+ });
166+
167+ // Send a GET request to <ESP_IP>/setKd?param_Kd=<inputMessage>
168+ server.on (" /setKi"
169+ String inputMessage;
170+ String inputParam;
171+ // GET Kp value on <ESP_IP>/setKd?param_Kd=<inputMessage>
172+ if (request->hasParam (PARAM_INPUT_3)) {
173+ inputMessage = request->getParam (PARAM_INPUT_3)->value ();
174+ inputParam = PARAM_INPUT_3;
175+ }
176+ else {
177+ inputMessage = " No message sent"
178+ inputParam = " none"
179+ }
180+ Serial.println (inputMessage);
181+ request->send (200 , " text/html" " HTTP GET request sent to your ESP on input field ("
182+ + inputParam + " ) with value: "
183+ " <br><a href=\" /\" >Return to Home Page</a>"
184+ });
185+
186+ server.begin ();
187+
188+ // Initialize RoboHeart with or without request for IMU automatic
189+ // calibration
190+ heart.begin ();
191+
192+ // Run IMU calibration and calculation of absolute rotation
193+ heart.setAutomaticRotation ();
194+
195+ // Configure Button which is used to set the
196+ // stable vertical position of the Balancing Bot.
197+ pinMode (BUTTON_ROBOHEART, INPUT);
198+ attachInterrupt (BUTTON_ROBOHEART, processPinInterrupt, FALLING);
199+
200+ delay (100 ); // Resolve false triggering of button during flashing
201+ offsetAngleDeg = 0 ;
202+ targetAngleDeg = 0 ;
203+
204+ // Save current time and start timer
205+ prevTimeIntervalMS = millis ();
206+ timer.start ();
207+ }
208+
209+ void loop (){
210+ // Perform PID control every CONTROL_TICK_PERIOD_US*PID_CONTROL_PRESCALER
211+ if (pidControlTick >= PID_CONTROL_PRESCALER) {
212+ unsigned long curTimeIntervalMS = millis ();
213+ pidControlTick = 0 ;
214+ currentAngleDeg = processAngle (heart.getRotationX ());
215+
216+ float error = currentAngleDeg - targetAngleDeg;
217+ errorSum = constrain (errorSum + error, -Kp * 50 , Kp * 50 );
218+
219+ float sampleTimeS = .001 * (curTimeIntervalMS - prevTimeIntervalMS);
220+
221+ // Calculate and sum output for P, I and D values
222+ motorPower = Kp * (error) + Ki * (errorSum)*sampleTimeS -
223+ Kd * (currentAngleDeg - prevAngleDeg) / sampleTimeS;
224+ prevAngleDeg = currentAngleDeg;
225+ prevTimeIntervalMS = curTimeIntervalMS;
226+ }
227+
228+ // Perform Motor control every CONTROL_TICK_PERIOD_US*DC_CONTROL_PRESCALER
229+ if (dcControlTick >= DC_CONTROL_PRESCALER) {
230+ dcControlTick = 0 ;
231+ if (motorPower > 0 ) {
232+ heart.motorA .reverse (motorPower);
233+ heart.motorB .reverse (motorPower);
234+ } else if (motorPower < 0 ) {
235+ heart.motorA .forward (-motorPower);
236+ heart.motorB .forward (-motorPower);
237+ }
238+ }
239+
240+ // Print statistics every CONTROL_TICK_PERIOD_US*STATISTICS_PRESCALER
241+ if (statisticsTick >= STATISTICS_PRESCALER) {
242+ statisticsTick = 0 ;
243+ Serial.print (" P :"
244+ Serial.print (motorPower);
245+ Serial.print (" A :"
246+ Serial.print (currentAngleDeg);
247+ Serial.print (" AT :"
248+ Serial.print (targetAngleDeg);
249+ Serial.print (" ES :"
250+ Serial.println (errorSum);
251+ }
252+
253+ delay (3 );
254+ }
0 commit comments