From 7827234a25d3d606664fd4f4c97eab4dc6a2f4df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=85=95=E7=82=8E?= <29385962@qq.com> Date: Wed, 23 Feb 2022 07:26:44 +0000 Subject: [PATCH] update arduino/Betas/RGB_V1.5/main/main.ino. --- arduino/Betas/RGB_V1.5/main/main.ino | 206 ++++++++++++++++----------- 1 file changed, 126 insertions(+), 80 deletions(-) diff --git a/arduino/Betas/RGB_V1.5/main/main.ino b/arduino/Betas/RGB_V1.5/main/main.ino index 120d851..0e571c5 100644 --- a/arduino/Betas/RGB_V1.5/main/main.ino +++ b/arduino/Betas/RGB_V1.5/main/main.ino @@ -1,5 +1,5 @@ /** - 自平衡莱洛三角形 RGB版 HW:Ver 1.5 FW:Ver 1.1.1 + 自平衡莱洛三角形 RGB版 HW:Ver 1.5 FW:Ver 1.2.1 立创EDA https://oshwhub.com/muyan2020/zi-ping-heng-di-lai-luo-san-jiao_10-10-ban-ben_copy RGB版本程序 https://gitee.com/muyan3000/RGBFOC 基于45°(https://gitee.com/coll45/foc/)程序修改 arduino开发环境-灯哥开源FOChttps://gitee.com/ream_d/Deng-s-foc-controller，并安装Kalman。 @@ -11,7 +11,8 @@ 比如让平衡角度为90度，则输入：TA90，并且会存入eeprom的位置0中 注：wifi发送命令不能过快，因为每次都会保存进eeprom 在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(5) 中的值，设置为自己的极对数数字，磁铁数量/2 程序默认设置的供电电压为 12V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值 - 默认PID针对的电机是 GB2204 ，使用自己的电机需要修改PID参数，才能实现更好效果 + V1默认PID针对的电机是 GB2204 ，使用自己的电机需要修改PID参数，才能实现更好效果 + V2电机是2715 */ #include #include "Command.h" @@ -59,7 +60,7 @@ int touchDetected[4] = {}; //通过touchdetected持续计数判断是否按键 bool touch_touched[4] = {}; //单击判断 int touch_touched_times[4] = {}; //单击次数，单击切换模式，双击 int touch_touching_time[4] = {}; //持续触摸秒数，用于判断长按事件，长按关闭，长按开启，开启状态长按调光， -bool touch_STATE[4] = {1, 1, 1, 1}; // 定义按键触发对象状态变量初始值为true默认开启 +bool touch_STATE[4] = {1, 1, 1, 1}; // 定义按键触发对象状态变量初始值为true默认开启 T2 T3 T4 const char *username = "admin"; //web用户名 const char *userpassword = "reuleaux123"; //web用户密码 @@ -119,17 +120,17 @@ float LQR_K4_2 = 1.5; // float LQR_K4_3 = 1.42; // //电机参数 -BLDCMotor motor = BLDCMotor(5); +BLDCMotor motor = BLDCMotor(5); //电机极数 BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22); float target_velocity = 0; //目标速度 -float target_angle = 90; //平衡角度 例如TA89.3 设置平衡角度89.3 +float target_angle = 89.3; //平衡角度 例如TA89.3 设置平衡角度89.3 float target_voltage = 0; //目标电压 float swing_up_voltage = 1.8; //摇摆电压 左右摇摆的电压，越大越快到平衡态，但是过大会翻过头 float swing_up_angle = 20; //摇摆角度 离平衡角度还有几度时候，切换到自平衡控制 float v_i_1 = 20; //非稳态速度环I -float v_p_1 = 0.7; //非稳态速度环P +float v_p_1 = 0.5; //非稳态速度环P float v_i_2 = 10; //稳态速度环I -float v_p_2 = 0.3; //稳态速度环P +float v_p_2 = 0.2; //稳态速度环P //命令设置 Command comm; bool Motor_enable_flag = 0; @@ -227,6 +228,61 @@ void Debug_Log_func(String debuglog, bool debug_control = debug_log_control) { } } +bool AutoWifiConfig() +{ + //wifi初始化 + WiFi.mode(WIFI_AP); + while (!WiFi.softAP(ssid, password)) {}; //启动AP + Serial.println("AP启动成功"); + Serial.println("Ready"); + Serial.print("IP address: "); + Serial.println(WiFi.softAPIP()); + byte mac[6]; + WiFi.macAddress(mac); + WiFi.setHostname(ServerName); + Serial.printf("macAddress 0x%02X:0x%02X:0x%02X:0x%02X:0x%02X:0x%02X\r\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); + + while (!udp.listen(localUdpPort)) //等待udp监听设置成功 + { + } + udp.onPacket(onPacketCallBack); //注册收到数据包事件 + + ArduinoOTA.setHostname(ServerName); + //以下是启动OTA，可以通过WiFi刷新固件 + ArduinoOTA.onStart([]() { + String type; + if (ArduinoOTA.getCommand() == U_FLASH) { + type = "sketch"; + } else { // U_SPIFFS + type = "filesystem"; + } + + // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end() + Serial.println("Start updating " + type); + }); + ArduinoOTA.onEnd([]() { + Serial.println("\nEnd"); + }); + ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) { + Serial.printf("Progress: %u%%\r", (progress / (total / 100))); + }); + ArduinoOTA.onError([](ota_error_t error) { + Serial.printf("Error[%u]: ", error); + if (error == OTA_AUTH_ERROR) { + Serial.println("Auth Failed"); + } else if (error == OTA_BEGIN_ERROR) { + Serial.println("Begin Failed"); + } else if (error == OTA_CONNECT_ERROR) { + Serial.println("Connect Failed"); + } else if (error == OTA_RECEIVE_ERROR) { + Serial.println("Receive Failed"); + } else if (error == OTA_END_ERROR) { + Serial.println("End Failed"); + } + }); + ArduinoOTA.begin(); +} + void onPacketCallBack(AsyncUDPPacket packet) { char* da; @@ -274,7 +330,7 @@ void setup() { Serial.println("Failed to initialise EEPROM"); Serial.println("Restarting..."); delay(1000); - ESP.restart(); + esp_restart(); } // eeprom 读取 int k, j; @@ -351,62 +407,17 @@ void setup() { FastLED.show(); delay(15); } - delay(500); + delay(300); } sprintf(mac_tmp, "%02X\r\n", (uint32_t)(ESP.getEfuseMac() >> (24) )); sprintf(mac_tmp, "ESP32-%c%c%c%c%c%c", mac_tmp[4], mac_tmp[5], mac_tmp[2], mac_tmp[3], mac_tmp[0], mac_tmp[1] ); - //wifi初始化 - WiFi.mode(WIFI_AP); - while (!WiFi.softAP(ssid, password)) {}; //启动AP - Serial.println("AP启动成功"); - Serial.println("Ready"); - Serial.print("IP address: "); - Serial.println(WiFi.softAPIP()); - byte mac[6]; - WiFi.macAddress(mac); - WiFi.setHostname(ServerName); - Serial.printf("macAddress 0x%02X:0x%02X:0x%02X:0x%02X:0x%02X:0x%02X\r\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); - while (!udp.listen(localUdpPort)) //等待udp监听设置成功 - { + if ( touch_STATE[3] ) { + AutoWifiConfig(); + StartWebServer(); } - udp.onPacket(onPacketCallBack); //注册收到数据包事件 - ArduinoOTA.setHostname(ServerName); - //以下是启动OTA，可以通过WiFi刷新固件 - ArduinoOTA.onStart([]() { - String type; - if (ArduinoOTA.getCommand() == U_FLASH) { - type = "sketch"; - } else { // U_SPIFFS - type = "filesystem"; - } - - // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end() - Serial.println("Start updating " + type); - }); - ArduinoOTA.onEnd([]() { - Serial.println("\nEnd"); - }); - ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) { - Serial.printf("Progress: %u%%\r", (progress / (total / 100))); - }); - ArduinoOTA.onError([](ota_error_t error) { - Serial.printf("Error[%u]: ", error); - if (error == OTA_AUTH_ERROR) { - Serial.println("Auth Failed"); - } else if (error == OTA_BEGIN_ERROR) { - Serial.println("Begin Failed"); - } else if (error == OTA_CONNECT_ERROR) { - Serial.println("Connect Failed"); - } else if (error == OTA_RECEIVE_ERROR) { - Serial.println("Receive Failed"); - } else if (error == OTA_END_ERROR) { - Serial.println("End Failed"); - } - }); - ArduinoOTA.begin(); // kalman mpu6050 init Wire.begin(19, 18, 400000); // Set I2C frequency to 400kHz @@ -460,16 +471,16 @@ void setup() { motor.PID_velocity.I = v_i_1; //最大电机限制电压 - motor.voltage_limit = 6; + motor.voltage_limit = 12; // [V]s //速度低通滤波时间常数 - motor.LPF_velocity.Tf = 0.01f; + motor.LPF_velocity.Tf = 0.02; // angle P controller motor.P_angle.P = 20; //设置最大速度限制 - motor.velocity_limit = 40; + motor.velocity_limit = 180; // [rad/s] motor.useMonitoring(Serial); @@ -493,7 +504,6 @@ void setup() { Serial.println("SPIFFS Failed to Start."); } - StartWebServer(); Serial.print("System is ready \t Free Heap: "); Serial.println(ESP.getFreeHeap()); @@ -508,7 +518,11 @@ long loop_count = 0; double last_pitch; void loop() { Debug_Log_func("loop"); - ArduinoOTA.handle(); + if ( touch_STATE[3] ) { + ESP32Server.handleClient(); + //delay(1);//allow the cpu to switch to other tasks + ArduinoOTA.handle(); + } motor.loopFOC(); while (i2cRead(0x3B, i2cData, 14)); @@ -547,8 +561,8 @@ void loop() { if (abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5~30°,1.5~90° { target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaftVelocity()); - if (abs(target_velocity) > 140) - target_velocity = _sign(target_velocity) * 140; + if (abs(target_velocity) > motor.velocity_limit) + target_velocity = _sign(target_velocity) * motor.velocity_limit; motor.controller = MotionControlType::velocity; motor.move(target_velocity); @@ -629,6 +643,13 @@ void loop() { touch_touched[2] = false; } + if (touch_touched[3]) { + //Serial.print("\nLight2 touched "); + //Serial.println(touch_touched_times[2]); + + touch_touched[3] = false; + } + //灯光及按键处理 if ( touch_STATE[1] ) { pride(); @@ -638,7 +659,7 @@ void loop() { FastLED.clearData(); FastLED.show(); } - + TenthSecondsSinceStartTask(); } @@ -696,7 +717,7 @@ float controllerLQR(float p_angle, float p_vel, float m_vel) // - k = [13.3, 21, 0.3] // - x = [pendulum angle, pendulum velocity, motor velocity]' - if (abs(p_angle) > 3) //摆角大于2.5则进入非稳态，记录非稳态时间 + if (abs(p_angle) > 2.5) //摆角大于2.5则进入非稳态，记录非稳态时间 { last_unstable_time = millis(); if (stable) //如果是稳态进入非稳态则调整为目标角度 @@ -706,7 +727,7 @@ float controllerLQR(float p_angle, float p_vel, float m_vel) stable = 0; } } - if ((millis() - last_unstable_time) > 500 && !stable) //非稳态进入稳态超过500ms检测，更新目标角为目标角+摆角，假设进入稳态 + if ((millis() - last_unstable_time) > 1000 && !stable) //非稳态进入稳态超过500ms检测，更新目标角为目标角+摆角，假设进入稳态 { //target_angle -= _sign(target_velocity) * 0.4; target_angle = target_angle+p_angle; @@ -754,8 +775,6 @@ unsigned long LastMillis = 0; void TenthSecondsSinceStartTask() //100ms { unsigned long CurrentMillis = millis(); - ESP32Server.handleClient(); - delay(1);//allow the cpu to switch to other tasks if (abs(int(CurrentMillis - LastMillis)) > 100) { LastMillis = CurrentMillis; @@ -793,8 +812,8 @@ void OnSecond() //Serial.println(DateTimeStr); #if defined(BAT_VOLTAGE_SENSE_PIN) //电池电压检测 - //driver.voltage_power_supply = return_voltage_value(BAT_VOLTAGE_SENSE_PIN); bat_voltage = return_voltage_value(BAT_VOLTAGE_SENSE_PIN); + //driver.voltage_power_supply = bat_voltage; //Serial.println(driver.voltage_power_supply); if (bat_voltage < min_voltage && !battery_low) { @@ -821,28 +840,42 @@ void OnSecond() //battery_low = 0; } else { //电池电压低闪灯 if (millis() % 500 < 250) - digitalWrite(ACTIVE_PIN, 1); - else digitalWrite(ACTIVE_PIN, 0); + else + digitalWrite(ACTIVE_PIN, 1); } } } #endif - for (byte i = 0; i < 3; i++) { + for (byte i = 0; i < 4; i++) { if (touchDetected[i] > 0) { //检测到触摸中，一秒计数一次，未触摸则清零 touch_touching_time[i]++; //长按事件处理 - if (touch_touching_time[1] % 2 == 0) { //按住大于2秒关灯或者开灯 + if (touch_touching_time[i] % 2 == 0) { //按住大于2秒 switch (i) { case 0: break; case 1: touch_STATE[i] = !touch_STATE[i]; //灯光状态反处理 + Serial.println("LIGHTS_ON/OFF"); break; - case 2: - + case 3: + digitalWrite(ACTIVE_PIN, 1); + delay(500); + if(touch_STATE[i]==1){ + ESP32Server.close();//关闭网络服务 + WiFi.disconnect(); + WiFi.mode(WIFI_OFF); + Serial.println("WIFI_OFF"); + }else{ + AutoWifiConfig(); + StartWebServer(); + Serial.println("WIFI_ON"); + } + touch_STATE[i] = !touch_STATE[i]; //状态反处理 + break; } } @@ -864,6 +897,7 @@ void OnTenthSecond() // 100ms 十分之一秒 //Serial.println(rgb_brightness); FastLED.setBrightness(rgb_brightness); } + } if (TenthSecondsSinceStart % 10 == 0) //10次为1秒 @@ -902,8 +936,6 @@ String ProcessUpdate() //页面更新 ReturnString += ","; ReturnString += test_flag; ReturnString += ","; - ReturnString += bat_voltage; - ReturnString += ","; ReturnString += EEPROM.readFloat(0); ReturnString += ","; ReturnString += swing_up_voltage; @@ -917,6 +949,8 @@ String ProcessUpdate() //页面更新 ReturnString += v_i_2; ReturnString += ","; ReturnString += v_p_2; + ReturnString += ","; + ReturnString += bat_voltage; if (log_control) { ReturnString += ","; @@ -936,7 +970,7 @@ String ProcessUpdate() //页面更新 ReturnString += ","; ReturnString += gyroZangle; } else { - ReturnString += ",,,,,,,,"; + ReturnString += "0,0,0,0,0,0,0,0,0"; } ReturnString += ","; @@ -1237,6 +1271,7 @@ double return_voltage_value(int pin_no) double tmp; double ADCVoltage; double inputVoltage; + analogSetPinAttenuation(pin_no, ADC_6db); for (int i = 0; i < 20; i++) { @@ -1248,6 +1283,17 @@ double return_voltage_value(int pin_no) inputVoltage = tmp / 20; if(inputVoltage!=0) inputVoltage = inputVoltage + 0.001; +/* + + for (int i = 0; i < 20; i++) + { + tmp = tmp + analogRead(pin_no); + } + tmp = tmp / 20; + + ADCVoltage = ((tmp * 3.3) / 4095.0) + 0.165; + inputVoltage = ADCVoltage / (R2_VOLTAGE / (R1_VOLTAGE + R2_VOLTAGE)); // formula for calculating voltage in i.e. GND +*/ return inputVoltage; } @@ -1272,4 +1318,4 @@ void touchAttach(int touchID, uint8_t touchPin) { touch_touching_time[touchID] = 0; //持续触摸时间清零 touchDetected[touchID] = 0; //持续触摸计数清零 } -} \ No newline at end of file +}