├── .gitignore ├── LICENSE ├── README.md ├── assets └── img │ └── small.jpg ├── software ├── robot_dog_esp32 │ ├── HAL.ino │ ├── HAL_ESP32PWM.ino │ ├── HAL_PCA9685.ino │ ├── WiFi.ino │ ├── cli.h │ ├── cli.ino │ ├── cliGet.ino │ ├── cliSet.ino │ ├── config.h │ ├── config_small.h │ ├── config_wifi.example.h │ ├── def.h │ ├── failsafe.ino │ ├── gait.ino │ ├── helpers.ino │ ├── i2cscan.ino │ ├── imu.ino │ ├── libs │ │ ├── HAL_body │ │ │ ├── HAL_body.cpp │ │ │ └── HAL_body.h │ │ ├── IK │ │ │ ├── IK.cpp │ │ │ ├── IK.h │ │ │ ├── IK_simple.cpp │ │ │ ├── IK_simple.h │ │ │ ├── geometry.h │ │ │ └── leg.h │ │ ├── balance │ │ │ ├── balance.cpp │ │ │ └── balance.h │ │ ├── gait │ │ │ ├── gait.cpp │ │ │ └── gait.h │ │ ├── planner │ │ │ ├── planner.cpp │ │ │ └── planner.h │ │ └── transition │ │ │ ├── transition.cpp │ │ │ └── transition.h │ ├── packagesProcess.ino │ ├── powerSensor.ino │ ├── robot_dog_esp32.ino │ ├── servo.ino │ ├── settings.ino │ ├── subscription.h │ ├── subscription.ino │ ├── testHAL.ino │ ├── web │ │ └── index.html.gz.h │ └── webServer.ino └── web │ ├── README.md │ ├── gulpfile.js │ ├── package.json │ └── src │ ├── index.html │ └── s.js └── tools └── servoCalib └── servoCalib.ino /.gitignore: -------------------------------------------------------------------------------- 1 | software/robot_dog_esp32/config_wifi.h 2 | software/web/node_modules 3 | software/web/package-lock.json 4 | software/robot_dog_esp32/web/index.html.gz 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Gleb Devyatkin 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Small Robot dog (quadruped) 2 | 3 | ## Disclaimer 4 | **This project has been done mostly for fun about a year ago. Some libraries have been updated, so it basically does not work any more. Not every planned feature was released. No PCB is made and it creates a lot of trouble for most of you. I'm sorry to say, but I'm not going to provide any support for it any more. It's exhausting and sometimes just destroys any intentions to continue. This is not comercial project and I'm not going to do something like Donation. Maybe I'm disappointing someone. Sorry. Project closed.** 5 | 6 | ![Small robot dog](https://github.com/SovGVD/esp32-robot-dog-code/blob/master/assets/img/small.jpg?raw=true) 7 | 8 | ## Hardware 9 | - ESP32 10 | - IMU (not implemented) 11 | - 12 servos TowerPro mg90d 12 | - Two 18650 13 | 14 | ## Software 15 | - Arduino IDE compatible 16 | 17 | ## TODO 18 | - [ ] use power sensor and IMU 19 | 20 | ## How to 21 | ### Calibrate servos (create `servoMainProfile`) 22 | - Print servo_calib tool and install servo into it: circle plate with dots, 10deg each from 0 to 180. 23 | - use tools/servoCalib.ino and connect servo to 14 pin 24 | - open Arduino IDE terminal and input `1500` (and press Enter) - this should be servo middle and it should point to the middle dot of printed tool 25 | - decrease value to find `minAngle` and `degMin` values for it (start with `800` and decrease it until servo stop move, than return back one step, e.g. set 790 - servo moved, 780 - servo moved, 760 - servo don't move, use 780) 26 | - do the same to find `maxAngle` and `degMax` but make value and start from 2100 and increase values 27 | - great, now we know our servo limits (or at least what are the limits for lib+servo), time to find more accurate servo positions 28 | - input values until you will not find proper positions for deg30, deg50...deg130, deg150 29 | 30 | ### Legs 31 | #### Assembling 32 | - to assembly legs correctly print leg_calib tool/template one as it is and one mirrored for the other side of robot for Beta and Gamma angles, as also Alpha angle tool 33 | - power up servo and connect ESP32 to you computer, open Arduino IDE terminal 34 | - input `set servo_to_calib` to set all servo to position expected for printer tool 35 | - assemble legs as closer as possible to expected leg parts positions according to the tool (90deg, 45deg, 90deg) 36 | 37 | #### Calibration 38 | - repeat steps 2,3 of Legs->Assembling instruction 39 | - input `set help` to see the list of available commands, we are interested in `XX_HAL_trim_xxxx`, e.g. `LF_HAL_trim_alpha`, where `LF` - left front leg, and `alpha` is the angle name 40 | - put Alpha leg tool on top of the robot legs servo, surface of tool should be (near)perfectly align with servos body, if not, use `XX_HAL_trim_alpha value_in_deg` command to set servo trim value, e.g. `set LF_HAL_trim_alpha -3`, it should not be too big, in other cases you need to repeat `Assemble` step 41 | - using tool for Beta and Gamma angles, calibrate/trim other servos 42 | -------------------------------------------------------------------------------- /assets/img/small.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SovGVD/esp32-robot-dog-code/109ca0a2d886f05ac3027c4b26ad63b8dfb65f8d/assets/img/small.jpg -------------------------------------------------------------------------------- /software/robot_dog_esp32/HAL.ino: -------------------------------------------------------------------------------- 1 | void initHAL() 2 | { 3 | Serial.print("HAL "); 4 | initServo(); 5 | initLegs(); 6 | Serial.println(); 7 | } 8 | 9 | void initLegs() { 10 | 11 | } 12 | 13 | void updateHAL() { 14 | updateLegs(); 15 | } 16 | 17 | void doHAL() { 18 | //if (!isHALEnabled()) return; 19 | servoSet(); 20 | } 21 | 22 | void readLegsSensors() { 23 | // TODO that will be i2c ADC, not analog read 24 | // TODO enable if sensors available 25 | // TODO don't set to legs, this should be part of gait 26 | for (int i = 0; i < LEG_NUM; i++) { 27 | //legs[i].sensor.onGround = analogRead(legs[i].sensor.pin) > legs[i].sensor.threshold; 28 | } 29 | } 30 | 31 | void updateLegs() { 32 | if (!isHALEnabled()) return; 33 | 34 | readLegsSensors(); 35 | // TODO check `isSolved` 36 | #ifdef DEBUG_HAL_LEG 37 | Serial.println("LEGLF"); 38 | #endif 39 | legs[LEGLF].angle = ikLegLF.solve().angle; 40 | 41 | #ifdef DEBUG_HAL_LEG 42 | Serial.println("LEGRF"); 43 | #endif 44 | legs[LEGRF].angle = ikLegRF.solve().angle; 45 | 46 | #ifdef DEBUG_HAL_LEG 47 | Serial.println("LEGLH"); 48 | #endif 49 | legs[LEGLH].angle = ikLegLH.solve().angle; 50 | 51 | #ifdef DEBUG_HAL_LEG 52 | Serial.println("LEGRH"); 53 | #endif 54 | legs[LEGRH].angle = ikLegRH.solve().angle; 55 | } 56 | 57 | double limitServoAngle(double angle) 58 | { 59 | // TODO this is not right solution, we should restart calc to solve the angle instead of just limit if something wrong 60 | // also this is nothing about real mechanical limits, so need TODO 61 | 62 | // default servo can handle angles from 0 to 180 (PI) degrees only 63 | if (angle < 0) angle = 0; 64 | if (angle > PI) angle = PI; 65 | 66 | return angle; 67 | } 68 | 69 | double getHALAngle(double angle, double mid, double trimAngle, double gearRatio, bool inverse) { 70 | angle = (angle - mid) * gearRatio + M_PI_2; 71 | if (inverse) angle = M_PI - angle; 72 | angle = angle + trimAngle; 73 | return angle; 74 | } 75 | 76 | double getHALTrim (leg &_leg, int angleId) 77 | { 78 | switch (angleId) { 79 | case ALPHA: 80 | return _leg.hal.trim.alpha; 81 | break; 82 | case BETA: 83 | return _leg.hal.trim.beta; 84 | break; 85 | case GAMMA: 86 | return _leg.hal.trim.gamma; 87 | break; 88 | } 89 | 90 | return 0; 91 | } 92 | 93 | bool setHALTrim (leg &_leg, int angleId, double deg) 94 | { 95 | double rad = degToRad(deg); 96 | if (rad >= LEG_TRIM_LIMIT || rad <= -LEG_TRIM_LIMIT) { 97 | return false; 98 | } 99 | 100 | switch (angleId) { 101 | case ALPHA: 102 | _leg.hal.trim.alpha = rad; 103 | break; 104 | case BETA: 105 | _leg.hal.trim.beta = rad; 106 | break; 107 | case GAMMA: 108 | _leg.hal.trim.gamma = rad; 109 | break; 110 | } 111 | 112 | settingsSaveTrimLeg(_leg); 113 | _leg.hal.trim = settingsLoadTrimLeg(_leg); 114 | 115 | return true; 116 | } 117 | 118 | bool isHALEnabled() 119 | { 120 | // This not correct, we are not enbale/disable HAL, but just disable servo position calculations 121 | return HALEnabled; 122 | } 123 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/HAL_ESP32PWM.ino: -------------------------------------------------------------------------------- 1 | #if PWM_CONTROLLER_TYPE == ESP32PWM 2 | int esp32pwmServos[12]; 3 | 4 | 5 | void initServoHAL() { 6 | Serial.print("ESP32_ISR "); 7 | Serial.println(ESP32_ISR_SERVO_VERSION); 8 | ESP32_ISR_Servos.useTimer(USE_ESP32_TIMER_NO); 9 | 10 | /** 11 | * Init servos 12 | */ 13 | for (uint8_t i = 0; i < LEG_NUM; i++) { 14 | esp32pwmServos[i*3+0] = ESP32_ISR_Servos.setupServo(legs[i].hal.pin.alpha, SERVO_MIN, SERVO_MAX); 15 | esp32pwmServos[i*3+1] = ESP32_ISR_Servos.setupServo(legs[i].hal.pin.beta, SERVO_MIN, SERVO_MAX); 16 | esp32pwmServos[i*3+2] = ESP32_ISR_Servos.setupServo(legs[i].hal.pin.gamma, SERVO_MIN, SERVO_MAX); 17 | } 18 | } 19 | 20 | uint16_t angleToPulse(double angleRad) { 21 | double angleDeg = radToDeg(angleRad); 22 | 23 | if (angleDeg < servoMainProfile.minAngle) angleDeg = servoMainProfile.minAngle; 24 | if (angleDeg > servoMainProfile.maxAngle) angleDeg = servoMainProfile.maxAngle; 25 | 26 | // TODO how to make it better??? 27 | 28 | if (angleDeg < 30) { 29 | return mapf(angleDeg, servoMainProfile.minAngle, 30, servoMainProfile.degMin, servoMainProfile.deg30); 30 | } 31 | 32 | if (angleDeg < 50) { 33 | return mapf(angleDeg, 30, 50, servoMainProfile.deg30, servoMainProfile.deg50); 34 | } 35 | 36 | if (angleDeg < 70) { 37 | return mapf(angleDeg, 50, 70, servoMainProfile.deg50, servoMainProfile.deg70); 38 | } 39 | 40 | if (angleDeg < 90) { 41 | return mapf(angleDeg, 70, 90, servoMainProfile.deg70, servoMainProfile.deg90); 42 | } 43 | 44 | if (angleDeg < 110) { 45 | return mapf(angleDeg, 90, 110, servoMainProfile.deg90, servoMainProfile.deg110); 46 | } 47 | 48 | if (angleDeg < 130) { 49 | return mapf(angleDeg, 110, 130, servoMainProfile.deg110, servoMainProfile.deg130); 50 | } 51 | 52 | if (angleDeg < 150) { 53 | return mapf(angleDeg, 130, 150, servoMainProfile.deg130, servoMainProfile.deg150); 54 | } 55 | 56 | if (angleDeg <= servoMainProfile.maxAngle) { 57 | return mapf(angleDeg, 150, servoMainProfile.maxAngle, servoMainProfile.deg150, servoMainProfile.degMax); 58 | } 59 | 60 | 61 | return 1500; // TODO actualy we should fail here... 62 | } 63 | 64 | 65 | void setLegPWM(leg &_leg) 66 | { 67 | uint8_t l = _leg.id.id*3; 68 | ESP32_ISR_Servos.setPulseWidth(esp32pwmServos[l+0], angleToPulse(limitServoAngle(getHALAngle(_leg.angle.alpha, _leg.hal.mid.alpha, _leg.hal.trim.alpha, _leg.hal.ratio.alpha, _leg.inverse.alpha)))); 69 | ESP32_ISR_Servos.setPulseWidth(esp32pwmServos[l+1], angleToPulse(limitServoAngle(getHALAngle(_leg.angle.beta, _leg.hal.mid.beta, _leg.hal.trim.beta, _leg.hal.ratio.beta, _leg.inverse.beta )))); 70 | ESP32_ISR_Servos.setPulseWidth(esp32pwmServos[l+2], angleToPulse(limitServoAngle(getHALAngle(_leg.angle.gamma, _leg.hal.mid.gamma, _leg.hal.trim.gamma, _leg.hal.ratio.gamma, _leg.inverse.gamma)))); 71 | 72 | // Serial.print("Servo on leg "); 73 | // Serial.println(_leg.id.id); 74 | // Serial.print("Servo ID "); 75 | // Serial.println(esp32pwmServos[l+0]); 76 | // Serial.println(esp32pwmServos[l+1]); 77 | // Serial.println(esp32pwmServos[l+2]); 78 | // 79 | // Serial.print(" "); 80 | // Serial.print(angleToPulse(limitServoAngle(getHALAngle(_leg.angle.alpha, _leg.hal.mid.alpha, _leg.hal.trim.alpha, _leg.hal.ratio.alpha, _leg.inverse.alpha)))); 81 | // Serial.print(" "); 82 | // Serial.println(radToDeg(limitServoAngle(getHALAngle(_leg.angle.alpha, _leg.hal.mid.alpha, _leg.hal.trim.alpha, _leg.hal.ratio.alpha, _leg.inverse.alpha)))); 83 | // 84 | // Serial.print(" "); 85 | // Serial.print(angleToPulse(limitServoAngle(getHALAngle(_leg.angle.beta, _leg.hal.mid.beta, _leg.hal.trim.beta, _leg.hal.ratio.beta, _leg.inverse.beta )))); 86 | // Serial.print(" "); 87 | // Serial.println(radToDeg(limitServoAngle(getHALAngle(_leg.angle.beta, _leg.hal.mid.beta, _leg.hal.trim.beta, _leg.hal.ratio.beta, _leg.inverse.beta )))); 88 | // 89 | // Serial.print(" "); 90 | // Serial.print(angleToPulse(limitServoAngle(getHALAngle(_leg.angle.gamma, _leg.hal.mid.gamma, _leg.hal.trim.gamma, _leg.hal.ratio.gamma, _leg.inverse.gamma)))); 91 | // Serial.print(" "); 92 | // Serial.println(radToDeg(limitServoAngle(getHALAngle(_leg.angle.gamma, _leg.hal.mid.gamma, _leg.hal.trim.gamma, _leg.hal.ratio.gamma, _leg.inverse.gamma)))); 93 | } 94 | 95 | void runServoCalibrate(leg &_leg) 96 | { 97 | uint8_t l = _leg.id.id*3; 98 | ESP32_ISR_Servos.setPulseWidth(esp32pwmServos[l+0], servoMainProfile.deg90); 99 | ESP32_ISR_Servos.setPulseWidth(esp32pwmServos[l+1], servoMainProfile.deg90); 100 | ESP32_ISR_Servos.setPulseWidth(esp32pwmServos[l+2], servoMainProfile.deg90); 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/HAL_PCA9685.ino: -------------------------------------------------------------------------------- 1 | #if PWM_CONTROLLER_TYPE == PCA9685 2 | 3 | void initServoHAL() { 4 | pwm = Adafruit_PWMServoDriver(); 5 | pwm.begin(); 6 | pwm.setOscillatorFrequency(27000000); // The int.osc. is closer to 27MHz 7 | pwm.setPWMFreq(SERVO_FREQ); // This is the maximum PWM frequency of servo 8 | } 9 | 10 | uint16_t angleToPulse(double angleRad) { 11 | // TODO, angle expected to be between 0 and 180 deg (0 and PI) 12 | 13 | double pulse = mapf(angleRad, 0, M_PI, 600, 2400); 14 | // from Adafruit PWM lib 15 | double pulselength; 16 | pulselength = 1000000; // 1,000,000 us per second 17 | 18 | // Read prescale 19 | uint16_t prescale = pwm.readPrescale(); 20 | 21 | // Calculate the pulse for PWM based on Equation 1 from the datasheet section 22 | // 7.3.5 23 | prescale += 1; 24 | pulselength *= prescale; 25 | pulselength /= pwm.getOscillatorFrequency(); 26 | pulse /= pulselength; 27 | 28 | return pulse; 29 | } 30 | 31 | void setLegPWM(leg &_leg) 32 | { 33 | pwm.setPWM(_leg.hal.pin.alpha, 0, angleToPulse(limitServoAngle(getHALAngle(_leg.angle.alpha, _leg.hal.mid.alpha, _leg.hal.trim.alpha, _leg.hal.ratio.alpha, _leg.inverse.alpha)))); 34 | pwm.setPWM(_leg.hal.pin.beta, 0, angleToPulse(limitServoAngle(getHALAngle(_leg.angle.beta, _leg.hal.mid.beta, _leg.hal.trim.beta, _leg.hal.ratio.beta, _leg.inverse.beta )))); 35 | pwm.setPWM(_leg.hal.pin.gamma, 0, angleToPulse(limitServoAngle(getHALAngle(_leg.angle.gamma, _leg.hal.mid.gamma, _leg.hal.trim.gamma, _leg.hal.ratio.gamma, _leg.inverse.gamma)))); 36 | } 37 | 38 | void runServoCalibrate(leg &_leg) 39 | { 40 | // TODO 41 | } 42 | 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/WiFi.ino: -------------------------------------------------------------------------------- 1 | void initWiFi() { 2 | Serial.print("WiFi"); 3 | WiFi.onEvent(WiFiEvent); 4 | if (WiFiMode == AP_MODE) { 5 | Serial.println("_AP"); 6 | WiFiSetMode(AP_MODE); 7 | } else { 8 | Serial.println("_CLIENT"); 9 | WiFiSetMode(WiFiMode); 10 | } 11 | } 12 | 13 | void WiFiEvent(WiFiEvent_t event){ 14 | switch(event) { 15 | case SYSTEM_EVENT_AP_START: 16 | WiFi.softAPsetHostname(wifiSsid[WiFiMode]); 17 | WiFiIP = WiFi.softAPIP(); 18 | Serial.print("WiFi AP IP: "); 19 | Serial.println(WiFiIP); 20 | break; 21 | case SYSTEM_EVENT_AP_STOP: 22 | break; 23 | case SYSTEM_EVENT_STA_START: 24 | WiFi.setHostname(wifiSsid[WiFiMode]); 25 | break; 26 | case SYSTEM_EVENT_STA_GOT_IP: 27 | WiFiIP = WiFi.localIP(); 28 | Serial.print("WiFi STA IP: "); 29 | Serial.println(WiFiIP); 30 | break; 31 | default: 32 | break; 33 | } 34 | } 35 | 36 | void WiFiSetMode(int setWiFiMode) { 37 | WiFiMode = settingsSaveWiFi(setWiFiMode); 38 | if (WiFiMode == AP_MODE) { 39 | WiFi.mode(WIFI_AP); 40 | WiFi.softAP(wifiSsid[WiFiMode], wifiPass[WiFiMode]); 41 | } else { 42 | WiFi.begin(wifiSsid[WiFiMode], wifiPass[WiFiMode]); 43 | } 44 | } 45 | 46 | void menuWiFiSetMode(int setWiFiMode) { 47 | WiFiSetMode(setWiFiMode); 48 | } 49 | 50 | void updateWiFi() { 51 | 52 | } 53 | 54 | double WiFiInfo(double info) { 55 | cliSerial->print("IP: "); 56 | cliSerial->println(WiFiIP); 57 | cliSerial->print("SSID: "); 58 | cliSerial->println(wifiSsid[WiFiMode]); 59 | 60 | if (WiFiMode == AP_MODE) { 61 | cliSerial->print("Pass: "); 62 | cliSerial->println(wifiPass[WiFiMode]); 63 | } 64 | 65 | return 1; 66 | } 67 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/cli.h: -------------------------------------------------------------------------------- 1 | #define CLI_VALUES 4 2 | #define CLI_VALUE_LEN 32 3 | #define CLI_DP 6 // decimal point 4 | #define CLI_GET 1 5 | #define CLI_SET 2 6 | #define CLI_RUN 3 7 | #define CLI_TERMINAL_SIGN "> " 8 | #define CLI_DELIMITER ":" 9 | 10 | typedef double (*cliFunction)(double); 11 | 12 | const typedef struct cliCommand_t { 13 | char commandName[CLI_VALUE_LEN]; 14 | cliFunction func; 15 | } cliCommand; 16 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/cli.ino: -------------------------------------------------------------------------------- 1 | #define CLI_MENU_COMMANDS_GET 30 2 | #define CLI_MENU_COMMANDS_SET 28 3 | #define CLI_MENU_COMMANDS_RUN 5 4 | 5 | char cliChar; 6 | bool cliErrorState = false; 7 | bool cliHideReturn = false; 8 | char cliValues[CLI_VALUES][CLI_VALUE_LEN]; 9 | unsigned int cliPosCommand = 0; 10 | unsigned int cliPosState = 0; 11 | 12 | const cliCommand cliCommandsGet[CLI_MENU_COMMANDS_GET] = { 13 | { "help", cliGetHelp }, 14 | { "LF_HAL_trim_alpha", cliGetHALTrimLFAlpha }, 15 | { "LF_HAL_trim_beta", cliGetHALTrimLFBeta }, 16 | { "LF_HAL_trim_gamma", cliGetHALTrimLFGamma }, 17 | { "RF_HAL_trim_alpha", cliGetHALTrimRFAlpha }, 18 | { "RF_HAL_trim_beta", cliGetHALTrimRFBeta }, 19 | { "RF_HAL_trim_gamma", cliGetHALTrimRFGamma }, 20 | { "LH_HAL_trim_alpha", cliGetHALTrimLHAlpha }, 21 | { "LH_HAL_trim_beta", cliGetHALTrimLHBeta }, 22 | { "LH_HAL_trim_gamma", cliGetHALTrimLHGamma }, 23 | { "RH_HAL_trim_alpha", cliGetHALTrimRHAlpha }, 24 | { "RH_HAL_trim_beta", cliGetHALTrimRHBeta }, 25 | { "RH_HAL_trim_gamma", cliGetHALTrimRHGamma }, 26 | { "HAL", cliGetHALState }, 27 | { "LF_angle_alpha", cliGetAngleLFAlpha }, 28 | { "LF_angle_beta", cliGetAngleLFBeta }, 29 | { "LF_angle_gamma", cliGetAngleLFGamma }, 30 | { "RF_angle_alpha", cliGetAngleRFAlpha }, 31 | { "RF_angle_beta", cliGetAngleRFBeta }, 32 | { "RF_angle_gamma", cliGetAngleRFGamma }, 33 | { "LH_angle_alpha", cliGetAngleLHAlpha }, 34 | { "LH_angle_beta", cliGetAngleLHBeta }, 35 | { "LH_angle_gamma", cliGetAngleLHGamma }, 36 | { "RH_angle_alpha", cliGetAngleRHAlpha }, 37 | { "RH_angle_beta", cliGetAngleRHBeta }, 38 | { "RH_angle_gamma", cliGetAngleRHGamma }, 39 | { "angles", cliGetAngles }, 40 | { "power", cliGetPower }, 41 | { "imu", cliGetIMU }, 42 | { "debug", cliGetDebug } 43 | }; 44 | 45 | const cliCommand cliCommandsSet[CLI_MENU_COMMANDS_SET] = { 46 | { "help", cliSetHelp }, 47 | { "LF_HAL_trim_alpha", cliSetHALTrimLFAlpha }, 48 | { "LF_HAL_trim_beta", cliSetHALTrimLFBeta }, 49 | { "LF_HAL_trim_gamma", cliSetHALTrimLFGamma }, 50 | { "RF_HAL_trim_alpha", cliSetHALTrimRFAlpha }, 51 | { "RF_HAL_trim_beta", cliSetHALTrimRFBeta }, 52 | { "RF_HAL_trim_gamma", cliSetHALTrimRFGamma }, 53 | { "LH_HAL_trim_alpha", cliSetHALTrimLHAlpha }, 54 | { "LH_HAL_trim_beta", cliSetHALTrimLHBeta }, 55 | { "LH_HAL_trim_gamma", cliSetHALTrimLHGamma }, 56 | { "RH_HAL_trim_alpha", cliSetHALTrimRHAlpha }, 57 | { "RH_HAL_trim_beta", cliSetHALTrimRHBeta }, 58 | { "RH_HAL_trim_gamma", cliSetHALTrimRHGamma }, 59 | { "HAL", cliSetHALState }, 60 | { "LF_angle_alpha", cliSetAngleLFAlpha }, 61 | { "LF_angle_beta", cliSetAngleLFBeta }, 62 | { "LF_angle_gamma", cliSetAngleLFGamma }, 63 | { "RF_angle_alpha", cliSetAngleRFAlpha }, 64 | { "RF_angle_beta", cliSetAngleRFBeta }, 65 | { "RF_angle_gamma", cliSetAngleRFGamma }, 66 | { "LH_angle_alpha", cliSetAngleLHAlpha }, 67 | { "LH_angle_beta", cliSetAngleLHBeta }, 68 | { "LH_angle_gamma", cliSetAngleLHGamma }, 69 | { "RH_angle_alpha", cliSetAngleRHAlpha }, 70 | { "RH_angle_beta", cliSetAngleRHBeta }, 71 | { "RH_angle_gamma", cliSetAngleRHGamma }, 72 | { "servo_calib", cliSetServoCalib }, 73 | { "servo_to_init", cliSetServoToInit } 74 | }; 75 | 76 | const cliCommand cliCommandsRun[CLI_MENU_COMMANDS_RUN] = { 77 | { "help", cliRunHelp }, 78 | { "i2cscan", runI2CScan }, 79 | { "calibrateimu", calibrateIMU }, 80 | { "wifiinfo", WiFiInfo }, 81 | { "sbscr", subscriptionState } 82 | }; 83 | 84 | void initCLI() { 85 | cliInitHelp(); 86 | } 87 | 88 | void cliInitHelp() 89 | { 90 | cliSerial->println(); 91 | cliSerial->println(" CLI: type `get|set|run|sbs help` to see available methods."); 92 | cliSerial->println(); 93 | 94 | } 95 | 96 | void updateCLI() 97 | { 98 | if (cliSerial->available()) { 99 | cliChar = cliSerial->read(); 100 | 101 | if (cliPosCommand >= CLI_VALUES || cliPosState >= CLI_VALUE_LEN) { 102 | cliFailCommand(); 103 | } 104 | 105 | switch (cliChar) { 106 | case 10: 107 | case 13: 108 | cliRunCommand(); 109 | break; 110 | 111 | case 32: 112 | cliPosState = 0; 113 | cliPosCommand++; 114 | break; 115 | 116 | default: 117 | cliValues[cliPosCommand][cliPosState] = cliChar; 118 | cliPosState++; 119 | break; 120 | } 121 | } 122 | } 123 | 124 | void cliRunCommand() 125 | { 126 | cliSerial->print(CLI_TERMINAL_SIGN); 127 | if (strcmp(cliValues[0], "get") == 0) { 128 | cliRunCommandGet(); 129 | } else if (strcmp(cliValues[0], "set") == 0) { 130 | cliRunCommandSet(); 131 | } else if (strcmp(cliValues[0], "run") == 0) { 132 | cliRunCommandRun(); 133 | } else if (strcmp(cliValues[0], "sbs") == 0) { 134 | cliRunCommandSubscription(); 135 | } else { 136 | //cliUnknownCommand(); 137 | } 138 | 139 | cliClearError(); 140 | cliClearCommand(); 141 | } 142 | 143 | void cliUnknownCommand() 144 | { 145 | cliSerial->println("UNKNOWN"); 146 | cliInitHelp(); 147 | } 148 | 149 | void cliRunCommandGet() { 150 | cliSerial->print("GET "); 151 | cliSerial->println(cliValues[1]); 152 | for (int i = 0; i < CLI_MENU_COMMANDS_GET; i++) { 153 | if (strcmp(cliValues[1], cliCommandsGet[i].commandName) == 0) { 154 | double r = cliCommandsGet[i].func(atof(cliValues[2])); 155 | if (!cliErrorState && !cliHideReturn) { 156 | cliSerial->println(r, CLI_DP); 157 | } 158 | } 159 | } 160 | cliHideReturn = false; 161 | } 162 | 163 | void cliRunCommandSubscription() { 164 | cliSerial->print("SBS "); 165 | cliSerial->println(cliValues[1]); 166 | cliSerial->println(cliValues[2]); 167 | cliSerial->println(cliValues[3]); 168 | for (int i = 1; i < CLI_MENU_COMMANDS_GET; i++) { // ignore `help` 169 | if (strcmp(cliValues[1], cliCommandsGet[i].commandName) == 0) { 170 | subscribe(i, atoi(cliValues[2]) == 1, atoi(cliValues[3]) == 1); 171 | } 172 | } 173 | cliHideReturn = false; 174 | } 175 | 176 | 177 | void cliRunCommandSet() { 178 | cliSerial->print("SET "); 179 | cliSerial->print(cliValues[1]); 180 | cliSerial->print(" "); 181 | cliSerial->println(atof(cliValues[2]), CLI_DP); 182 | for (int i = 0; i < CLI_MENU_COMMANDS_SET; i++) { 183 | if (strcmp(cliValues[1], cliCommandsSet[i].commandName) == 0) { 184 | double r = cliCommandsSet[i].func(atof(cliValues[2])); 185 | if (!cliErrorState && !cliHideReturn) { 186 | cliSerial->println(r, CLI_DP); 187 | } 188 | } 189 | } 190 | cliHideReturn = false; 191 | } 192 | 193 | void cliRunCommandRun() { 194 | cliSerial->print("RUN "); 195 | cliSerial->println(cliValues[1]); 196 | for (int i = 0; i < CLI_MENU_COMMANDS_RUN; i++) { 197 | if (strcmp(cliValues[1], cliCommandsRun[i].commandName) == 0) { 198 | cliCommandsRun[i].func(atof(cliValues[2])); 199 | } 200 | } 201 | cliHideReturn = false; 202 | } 203 | 204 | void cliDisplayHelp(cliCommand *_commands, int num) 205 | { 206 | cliHideReturn = true; 207 | for (int i = 1; i < num; i++) { // first command is `help`, we don't need to print it 208 | cliSerial->print(i); 209 | cliSerial->print(CLI_DELIMITER); 210 | cliSerial->println(_commands[i].commandName); 211 | } 212 | 213 | } 214 | 215 | double cliGetHelp(double id) { 216 | int num = CLI_MENU_COMMANDS_GET; 217 | cliDisplayHelp(cliCommandsGet, num); 218 | return num; 219 | } 220 | 221 | double cliSetHelp(double id) { 222 | int num = CLI_MENU_COMMANDS_SET; 223 | cliDisplayHelp(cliCommandsSet, num); 224 | return num; 225 | } 226 | 227 | double cliRunHelp(double id) { 228 | int num = CLI_MENU_COMMANDS_RUN; 229 | cliDisplayHelp(cliCommandsRun, num); 230 | return num; 231 | } 232 | 233 | void cliFailCommand() { 234 | cliError("Command too long"); 235 | } 236 | 237 | void cliError(String msg) { 238 | cliClearCommand(); 239 | cliWasError(); 240 | cliSerial->print("ERROR: "); 241 | cliSerial->println(msg); 242 | } 243 | 244 | void cliWasError() 245 | { 246 | cliErrorState = true; 247 | } 248 | 249 | void cliClearError() 250 | { 251 | cliErrorState = false; 252 | } 253 | 254 | void cliClearCommand() { 255 | for (cliPosCommand = 0; cliPosCommand < CLI_VALUES; cliPosCommand++) { 256 | for (cliPosState = 0; cliPosState < CLI_VALUE_LEN; cliPosState++) { 257 | cliValues[cliPosCommand][cliPosState] = 0; 258 | } 259 | } 260 | 261 | cliPosCommand = 0; 262 | cliPosState = 0; 263 | } 264 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/cliGet.ino: -------------------------------------------------------------------------------- 1 | // HAL 2 | 3 | // HAL - trim 4 | double _cliGetHALTrim(leg &_leg, int angleId) { 5 | return radToDeg(getHALTrim(_leg, angleId)); 6 | } 7 | 8 | double cliGetHALTrimLFAlpha(double id) { 9 | return _cliGetHALTrim(legs[LEGLF], ALPHA); 10 | } 11 | double cliGetHALTrimLFBeta(double id) { 12 | return _cliGetHALTrim(legs[LEGLF], BETA); 13 | } 14 | double cliGetHALTrimLFGamma(double id) { 15 | return _cliGetHALTrim(legs[LEGLF], GAMMA); 16 | } 17 | double cliGetHALTrimRFAlpha(double id) { 18 | return _cliGetHALTrim(legs[LEGRF], ALPHA); 19 | } 20 | double cliGetHALTrimRFBeta(double id) { 21 | return _cliGetHALTrim(legs[LEGRF], BETA); 22 | } 23 | double cliGetHALTrimRFGamma(double id) { 24 | return _cliGetHALTrim(legs[LEGRF], GAMMA); 25 | } 26 | double cliGetHALTrimLHAlpha(double id) { 27 | return _cliGetHALTrim(legs[LEGLH], ALPHA); 28 | } 29 | double cliGetHALTrimLHBeta(double id) { 30 | return _cliGetHALTrim(legs[LEGLH], BETA); 31 | } 32 | double cliGetHALTrimLHGamma(double id) { 33 | return _cliGetHALTrim(legs[LEGLH], GAMMA); 34 | } 35 | double cliGetHALTrimRHAlpha(double id) { 36 | return _cliGetHALTrim(legs[LEGRH], ALPHA); 37 | } 38 | double cliGetHALTrimRHBeta(double id) { 39 | return _cliGetHALTrim(legs[LEGRH], BETA); 40 | } 41 | double cliGetHALTrimRHGamma(double id) { 42 | return _cliGetHALTrim(legs[LEGRH], GAMMA); 43 | } 44 | 45 | // HAL - state 46 | 47 | double cliGetHALState(double id) 48 | { 49 | return isHALEnabled() ? 1.0 : 0.0; 50 | } 51 | 52 | // Leg angle 53 | 54 | double _cliGetAngle(leg &_leg, int angleId) 55 | { 56 | return getAngleDeg(_leg, angleId); 57 | } 58 | 59 | double cliGetAngleLFAlpha(double id) { return _cliGetAngle(legs[LEGLF], ALPHA); } 60 | double cliGetAngleLFBeta(double id) { return _cliGetAngle(legs[LEGLF], BETA); } 61 | double cliGetAngleLFGamma(double id) { return _cliGetAngle(legs[LEGLF], GAMMA); } 62 | 63 | double cliGetAngleRFAlpha(double id) { return _cliGetAngle(legs[LEGRF], ALPHA); } 64 | double cliGetAngleRFBeta(double id) { return _cliGetAngle(legs[LEGRF], BETA); } 65 | double cliGetAngleRFGamma(double id) { return _cliGetAngle(legs[LEGRF], GAMMA); } 66 | 67 | double cliGetAngleLHAlpha(double id) { return _cliGetAngle(legs[LEGLH], ALPHA); } 68 | double cliGetAngleLHBeta(double id) { return _cliGetAngle(legs[LEGLH], BETA); } 69 | double cliGetAngleLHGamma(double id) { return _cliGetAngle(legs[LEGLH], GAMMA); } 70 | 71 | double cliGetAngleRHAlpha(double id) { return _cliGetAngle(legs[LEGRH], ALPHA); } 72 | double cliGetAngleRHBeta(double id) { return _cliGetAngle(legs[LEGRH], BETA); } 73 | double cliGetAngleRHGamma(double id) { return _cliGetAngle(legs[LEGRH], GAMMA); } 74 | 75 | void _cliGetAngles(leg &_leg) 76 | { 77 | cliSerial->print(radToDeg(_leg.angle.alpha), CLI_DP); cliSerial->print(" -> "); 78 | cliSerial->println(radToDeg(getHALAngle(_leg.angle.alpha, _leg.hal.mid.alpha, _leg.hal.trim.alpha, _leg.hal.ratio.alpha, _leg.inverse.alpha)), CLI_DP); 79 | 80 | cliSerial->print(radToDeg(_leg.angle.beta), CLI_DP); cliSerial->print(" -> "); 81 | cliSerial->println(radToDeg(getHALAngle(_leg.angle.beta, _leg.hal.mid.beta, _leg.hal.trim.beta, _leg.hal.ratio.beta, _leg.inverse.beta )), CLI_DP); 82 | 83 | cliSerial->print(radToDeg(_leg.angle.gamma), CLI_DP); cliSerial->print(" -> "); 84 | cliSerial->println(radToDeg(getHALAngle(_leg.angle.gamma, _leg.hal.mid.gamma, _leg.hal.trim.gamma, _leg.hal.ratio.gamma, _leg.inverse.gamma)), CLI_DP); 85 | } 86 | 87 | double cliGetAngles(double id) 88 | { 89 | cliHideReturn = true; 90 | 91 | cliSerial->println("LF"); 92 | _cliGetAngles(legs[LEGLF]); 93 | 94 | cliSerial->println("RF"); 95 | _cliGetAngles(legs[LEGRF]); 96 | 97 | cliSerial->println("LH"); 98 | _cliGetAngles(legs[LEGLH]); 99 | 100 | cliSerial->println("RH"); 101 | _cliGetAngles(legs[LEGRH]); 102 | 103 | cliHideReturn = true; 104 | return 0.0; 105 | 106 | } 107 | 108 | double cliGetPower(double id) 109 | { 110 | cliSerial->print(getPowerSensorVoltage(), 6); 111 | cliSerial->println(" V"); 112 | cliSerial->print(getPowerSensorCurrent(), 6); 113 | cliSerial->println(" A"); 114 | 115 | cliHideReturn = true; 116 | return 0.0; 117 | } 118 | 119 | double cliGetIMU(double id) 120 | { 121 | cliSerial->print("Pitch: "); 122 | cliSerial->println(IMU_DATA[PITCH], 6); 123 | cliSerial->print("Roll: "); 124 | cliSerial->println(IMU_DATA[ROLL], 6); 125 | cliSerial->print("Yaw: "); 126 | cliSerial->println(IMU_DATA[YAW], 6); 127 | 128 | cliHideReturn = true; 129 | return 0.0; 130 | } 131 | 132 | double cliGetDebug(double id) 133 | { 134 | 135 | switch((int)id) { 136 | case 1: 137 | cliSerial->print("Main loop: "); 138 | cliSerial->println(loopTime); 139 | 140 | cliSerial->print("Service fast loop: "); 141 | cliSerial->println(serviceFastLoopTime); 142 | 143 | cliSerial->print("Service loop: "); 144 | cliSerial->println(serviceLoopTime); 145 | 146 | break; 147 | 148 | case 2: 149 | cliSerial->print("Body "); 150 | cliPrintPoint(body.position, 3); 151 | cliPrintAngle(body.orientation, 4); 152 | cliSerial->println(); 153 | 154 | for (int i = 0; i < LEG_NUM; i++) { 155 | cliSerial->print("Leg "); 156 | cliSerial->print(i); 157 | cliSerial->print(" "); 158 | 159 | cliPrintPoint(legs[i].foot, 2); 160 | cliSerial->println(); 161 | } 162 | break; 163 | 164 | default: 165 | cliSerial->println("Unknown debug ID"); 166 | } 167 | 168 | cliHideReturn = true; 169 | return 0.0; 170 | } 171 | 172 | 173 | void cliPrintPoint(point p, int n) 174 | { 175 | cliSerial->print("{"); 176 | cliSerial->print(p.x, n); 177 | cliSerial->print(", "); 178 | cliSerial->print(p.y, n); 179 | cliSerial->print(", "); 180 | cliSerial->print(p.z, n); 181 | cliSerial->print("} "); 182 | } 183 | 184 | void cliPrintAngle(angle p, int n) 185 | { 186 | cliSerial->print("{"); 187 | cliSerial->print(degToRad(p.pitch), n); 188 | cliSerial->print(", "); 189 | cliSerial->print(degToRad(p.roll), n); 190 | cliSerial->print(", "); 191 | cliSerial->print(degToRad(p.yaw), n); 192 | cliSerial->print("} "); 193 | } 194 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/cliSet.ino: -------------------------------------------------------------------------------- 1 | // TODO... too much copy-paste, IDK how to avoid it=( 2 | // HAL 3 | // HAL - trim 4 | 5 | bool _cliSetHALTrim (leg &_leg, int angleId, double deg) { 6 | return setHALTrim(_leg, angleId, deg); 7 | } 8 | 9 | double _cliSetHALTrimError() 10 | { 11 | cliError("Out of limit"); 12 | 13 | return 0; 14 | } 15 | 16 | double cliSetHALTrimLFAlpha(double deg) { 17 | if (_cliSetHALTrim (legs[LEGLF], ALPHA, deg)) { 18 | return _cliGetHALTrim(legs[LEGLF], ALPHA); 19 | } 20 | 21 | return _cliSetHALTrimError(); 22 | } 23 | 24 | double cliSetHALTrimLFBeta(double deg) { 25 | if (_cliSetHALTrim (legs[LEGLF], BETA, deg)) { 26 | return _cliGetHALTrim(legs[LEGLF], BETA); 27 | } 28 | 29 | return _cliSetHALTrimError(); 30 | } 31 | 32 | double cliSetHALTrimLFGamma(double deg) { 33 | if (_cliSetHALTrim (legs[LEGLF], GAMMA, deg)) { 34 | return _cliGetHALTrim(legs[LEGLF], GAMMA); 35 | } 36 | 37 | return _cliSetHALTrimError(); 38 | } 39 | 40 | 41 | double cliSetHALTrimRFAlpha(double deg) { 42 | if (_cliSetHALTrim (legs[LEGRF], ALPHA, deg)) { 43 | return _cliGetHALTrim(legs[LEGRF], ALPHA); 44 | } 45 | 46 | return _cliSetHALTrimError(); 47 | } 48 | 49 | double cliSetHALTrimRFBeta(double deg) { 50 | if (_cliSetHALTrim (legs[LEGRF], BETA, deg)) { 51 | return _cliGetHALTrim(legs[LEGRF], BETA); 52 | } 53 | 54 | return _cliSetHALTrimError(); 55 | } 56 | 57 | double cliSetHALTrimRFGamma(double deg) { 58 | if (_cliSetHALTrim (legs[LEGRF], GAMMA, deg)) { 59 | return _cliGetHALTrim(legs[LEGRF], GAMMA); 60 | } 61 | 62 | return _cliSetHALTrimError(); 63 | } 64 | 65 | 66 | double cliSetHALTrimLHAlpha(double deg) { 67 | if (_cliSetHALTrim (legs[LEGLH], ALPHA, deg)) { 68 | return _cliGetHALTrim(legs[LEGLH], ALPHA); 69 | } 70 | 71 | return _cliSetHALTrimError(); 72 | } 73 | 74 | double cliSetHALTrimLHBeta(double deg) { 75 | if (_cliSetHALTrim (legs[LEGLH], BETA, deg)) { 76 | return _cliGetHALTrim(legs[LEGLH], BETA); 77 | } 78 | 79 | return _cliSetHALTrimError(); 80 | } 81 | 82 | double cliSetHALTrimLHGamma(double deg) { 83 | if (_cliSetHALTrim (legs[LEGLH], GAMMA, deg)) { 84 | return _cliGetHALTrim(legs[LEGLH], GAMMA); 85 | } 86 | 87 | return _cliSetHALTrimError(); 88 | } 89 | 90 | 91 | double cliSetHALTrimRHAlpha(double deg) { 92 | if (_cliSetHALTrim (legs[LEGRH], ALPHA, deg)) { 93 | return _cliGetHALTrim(legs[LEGRH], ALPHA); 94 | } 95 | 96 | return _cliSetHALTrimError(); 97 | } 98 | 99 | double cliSetHALTrimRHBeta(double deg) { 100 | if (_cliSetHALTrim (legs[LEGRH], BETA, deg)) { 101 | return _cliGetHALTrim(legs[LEGRH], BETA); 102 | } 103 | 104 | return _cliSetHALTrimError(); 105 | } 106 | 107 | double cliSetHALTrimRHGamma(double deg) { 108 | if (_cliSetHALTrim (legs[LEGRH], GAMMA, deg)) { 109 | return _cliGetHALTrim(legs[LEGRH], GAMMA); 110 | } 111 | 112 | return _cliSetHALTrimError(); 113 | } 114 | 115 | // HAL - state 116 | 117 | double cliSetHALState(double state) 118 | { 119 | HALEnabled = state == 0 ? false : true; 120 | return cliGetHALState(state); 121 | } 122 | 123 | // Leg angle 124 | 125 | double _cliSetAngle(leg &_leg, int angleId, double deg) 126 | { 127 | if (!cliSetHALState(0) && setAngleDeg(_leg, angleId, deg)) { 128 | return _cliGetAngle(_leg, angleId); 129 | } 130 | 131 | return _cliSetAngleError(); 132 | } 133 | 134 | double _cliSetAngleError() 135 | { 136 | cliError("Unable to set angle"); 137 | 138 | return 0; 139 | } 140 | 141 | double cliSetAngleLFAlpha(double deg) { return _cliSetAngle(legs[LEGLF], ALPHA, deg); } 142 | double cliSetAngleLFBeta(double deg) { return _cliSetAngle(legs[LEGLF], BETA, deg); } 143 | double cliSetAngleLFGamma(double deg) { return _cliSetAngle(legs[LEGLF], GAMMA, deg); } 144 | 145 | double cliSetAngleRFAlpha(double deg) { return _cliSetAngle(legs[LEGRF], ALPHA, deg); } 146 | double cliSetAngleRFBeta(double deg) { return _cliSetAngle(legs[LEGRF], BETA, deg); } 147 | double cliSetAngleRFGamma(double deg) { return _cliSetAngle(legs[LEGRF], GAMMA, deg); } 148 | 149 | double cliSetAngleLHAlpha(double deg) { return _cliSetAngle(legs[LEGLH], ALPHA, deg); } 150 | double cliSetAngleLHBeta(double deg) { return _cliSetAngle(legs[LEGLH], BETA, deg); } 151 | double cliSetAngleLHGamma(double deg) { return _cliSetAngle(legs[LEGLH], GAMMA, deg); } 152 | 153 | double cliSetAngleRHAlpha(double deg) { return _cliSetAngle(legs[LEGRH], ALPHA, deg); } 154 | double cliSetAngleRHBeta(double deg) { return _cliSetAngle(legs[LEGRH], BETA, deg); } 155 | double cliSetAngleRHGamma(double deg) { return _cliSetAngle(legs[LEGRH], GAMMA, deg); } 156 | 157 | // Servo calibration 158 | double cliSetServoCalib(double i) { 159 | HALEnabled = false; 160 | delay(1000); // that it terrible, but we need to wait to make sure HAL disabled 161 | 162 | setServoToMiddle(); 163 | 164 | return cliGetHALState(1); 165 | } 166 | 167 | double cliSetServoToInit(double i) { 168 | HALEnabled = false; 169 | delay(1000); // that it terrible, but we need to wait to make sure HAL disabled 170 | 171 | setServoToInit(); 172 | 173 | return cliGetHALState(1); 174 | 175 | } 176 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/config.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Configuration file 3 | */ 4 | 5 | //Cores loop time 6 | #define LOOP_TIME 4000 // microseconds (10^-6 seconds) 7 | #define SERVICE_LOOP_TIME 100000 8 | #define SERVICE_FAST_LOOP_TIME 5000 9 | 10 | //Fail safe 11 | #define FS_WS_THR 400 // cycles, 1 second = FS_WS_THR*LOOP_TIME, e.g. 1 000 * 1 000 = 1 000 000 = 1 second 12 | 13 | // Serial 14 | #define SERIAL_BAUD 115200 15 | 16 | /* 17 | * I2C devices 18 | * - PWM PCA9685 19 | * - IMU MPU 9250 20 | */ 21 | 22 | #define PWM_CONTROLLER_ADDRESS 0x40 23 | //#define PWM_CONTROLLER_TYPE PCA9685 24 | #define PWM_CONTROLLER_TYPE ESP32PWM 25 | 26 | #define IMU_ADDRESS 0x68 27 | #define IMU_TYPE MPU9250 28 | 29 | #define POWER_SENSOR INA219 30 | 31 | // default angles for init state 32 | #define LEG_ANGLE_ALPHA M_PI_2 33 | #define LEG_ANGLE_BETA M_PI_2 34 | #define LEG_ANGLE_GAMMA M_PI_2 35 | 36 | // angle limits 37 | #define LEG_ANGLE_ALPHA_MIN 0 38 | #define LEG_ANGLE_ALPHA_MAX M_PI 39 | 40 | #define LEG_ANGLE_BETA_MIN 0 41 | #define LEG_ANGLE_BETA_MAX M_PI 42 | 43 | #define LEG_ANGLE_GAMMA_MIN 0 44 | #define LEG_ANGLE_GAMMA_MAX M_PI 45 | 46 | #define LEG_TRIM_INC 0.002 // radian 47 | #define LEG_TRIM_LIMIT 0.24 // See settingsUint8ToDouble() 48 | 49 | // Servo config (set as max as possible for init, but use servoProfile) 50 | #define SERVO_MIN 600 51 | #define SERVO_MAX 2400 52 | #define SERVO_FREQ 330 53 | 54 | //Fail safe 55 | #define FS_WS_THR 20 // 1 second = FS_WS_THR*LOOP_TIME 56 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/config_small.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * Specific for robot config 4 | */ 5 | // default legs state (TODO measure it!!!) 6 | #define LEG_BODY_X 39.5 7 | #define LEG_BODY_Y_F 70.5 // not sure 8 | #define LEG_BODY_Y_H 70.5 9 | #define LEG_BODY_Z 0.0 10 | #define LEG_POINT_X 56.0 11 | #define LEG_POINT_Y_F 70.5 // not sure 12 | #define LEG_POINT_Y_H 70.5 13 | #define LEG_POINT_Z 80.0 14 | 15 | // Robot config 16 | #define LEG_NUM 4 17 | 18 | 19 | // Size of leg parts in mm 20 | #define LEG_SIZE_L1 0.0 21 | #define LEG_SIZE_L2 51.9 22 | #define LEG_SIZE_L3 50.0 23 | 24 | point balanceOffset = {0, 0, 0}; 25 | 26 | leg legs[LEG_NUM] = { 27 | // LEFT FRONT 28 | { 29 | {LEGLF, "LF"}, 30 | { -LEG_BODY_X, LEG_BODY_Y_F, LEG_BODY_Z}, 31 | {-LEG_POINT_X, LEG_POINT_Y_F, -LEG_POINT_Z}, 32 | { LEG_SIZE_L1, LEG_SIZE_L2, LEG_SIZE_L3}, 33 | {LEG_ANGLE_ALPHA_MIN, LEG_ANGLE_BETA_MIN, LEG_ANGLE_GAMMA_MIN}, 34 | {LEG_ANGLE_ALPHA_MAX, LEG_ANGLE_BETA_MAX, LEG_ANGLE_GAMMA_MAX}, 35 | { 36 | {M_PI_2, M_PI_2, M_PI_2}, // hardware middle 37 | { 25, 26, 27}, // 3 servo pins 38 | { 0, 0, 0}, // servo middle trim 39 | { 2, 1, 1} // gear ratio, first and last one is 16 groves on servo and 24 groves on leg 40 | }, 41 | { -LEG_BODY_X, LEG_BODY_Y_F, LEG_BODY_Z}, 42 | { -LEG_POINT_X, LEG_POINT_Y_F, -LEG_POINT_Z}, 43 | {LEG_ANGLE_ALPHA, LEG_ANGLE_BETA, LEG_ANGLE_GAMMA}, 44 | {true, true, false, false, true, true}, 45 | {true, 0, 0} 46 | }, 47 | // RIGHT FRONT 48 | { 49 | {LEGRF, "RF"}, 50 | { LEG_BODY_X, LEG_BODY_Y_F, LEG_BODY_Z}, 51 | {LEG_POINT_X, LEG_POINT_Y_F, -LEG_POINT_Z}, 52 | {LEG_SIZE_L1, LEG_SIZE_L2, LEG_SIZE_L3}, 53 | {LEG_ANGLE_ALPHA_MIN, LEG_ANGLE_BETA_MIN, LEG_ANGLE_GAMMA_MIN}, 54 | {LEG_ANGLE_ALPHA_MAX, LEG_ANGLE_BETA_MAX, LEG_ANGLE_GAMMA_MAX}, 55 | { 56 | {M_PI_2, M_PI_2, M_PI_2}, 57 | { 16, 18, 17}, 58 | { 0, 0, 0}, 59 | { 2, 1, 1} 60 | }, 61 | { LEG_BODY_X, LEG_BODY_Y_F, LEG_BODY_Z}, 62 | { LEG_POINT_X, LEG_POINT_Y_F, -LEG_POINT_Z}, 63 | {LEG_ANGLE_ALPHA, LEG_ANGLE_BETA, LEG_ANGLE_GAMMA}, 64 | {false, true, false, true, false, false}, 65 | {true, 0, 0} 66 | }, 67 | // LEFT HIND 68 | { 69 | {LEGLH, "LH"}, 70 | { -LEG_BODY_X, -LEG_BODY_Y_H, LEG_BODY_Z}, 71 | {-LEG_POINT_X, -LEG_POINT_Y_H, -LEG_POINT_Z}, 72 | { LEG_SIZE_L1, LEG_SIZE_L2, LEG_SIZE_L3}, 73 | {LEG_ANGLE_ALPHA_MIN, LEG_ANGLE_BETA_MIN, LEG_ANGLE_GAMMA_MIN}, 74 | {LEG_ANGLE_ALPHA_MAX, LEG_ANGLE_BETA_MAX, LEG_ANGLE_GAMMA_MAX}, 75 | { 76 | {M_PI_2, M_PI_2, M_PI_2}, 77 | { 13, 12, 14}, 78 | { 0, 0, 0}, 79 | { 2, 1, 1} 80 | }, 81 | { -LEG_BODY_X, -LEG_BODY_Y_H, LEG_BODY_Z}, 82 | { -LEG_POINT_X, -LEG_POINT_Y_H, -LEG_POINT_Z}, 83 | {LEG_ANGLE_ALPHA, LEG_ANGLE_BETA, LEG_ANGLE_GAMMA}, 84 | {true, true, false, true, true, true}, 85 | {true, 0, 0} 86 | }, 87 | // RIGHT HIND 88 | { 89 | {LEGRH, "RH"}, 90 | { LEG_BODY_X, -LEG_BODY_Y_H, LEG_BODY_Z}, 91 | {LEG_POINT_X, -LEG_POINT_Y_H, -LEG_POINT_Z}, 92 | {LEG_SIZE_L1, LEG_SIZE_L2, LEG_SIZE_L3}, 93 | {LEG_ANGLE_ALPHA_MIN, LEG_ANGLE_BETA_MIN, LEG_ANGLE_GAMMA_MIN}, 94 | {LEG_ANGLE_ALPHA_MAX, LEG_ANGLE_BETA_MAX, LEG_ANGLE_GAMMA_MAX}, 95 | { 96 | {M_PI_2, M_PI_2, M_PI_2}, 97 | { 4, 2, 15}, 98 | { 0, 0, 0}, 99 | { 2, 1, 1} 100 | }, 101 | { LEG_BODY_X, -LEG_BODY_Y_H, LEG_BODY_Z}, 102 | { LEG_POINT_X, -LEG_POINT_Y_H, -LEG_POINT_Z}, 103 | {LEG_ANGLE_ALPHA, LEG_ANGLE_BETA, LEG_ANGLE_GAMMA}, 104 | {false, true, false, false, false, false}, 105 | {true, 0, 0} 106 | } 107 | }; 108 | 109 | /** 110 | * Not all servos are perfect 111 | * 112 | * This is for TowerPro MG90D (or maybe I have clone, who knows) 113 | */ 114 | servoProfile servoMainProfile = { 115 | 20, // minAngle 116 | 160, // maxAngle 117 | 780, // degMin 118 | 890, // deg30 119 | 1090, // deg50 120 | 1300, // deg70 121 | 1500, // deg90 - middle 122 | 1700, // deg110 123 | 1900, // deg130 124 | 2110, // deg150 125 | 2200 // degMax 126 | }; 127 | 128 | /** 129 | * My Trim settings 130 | * 0 leg trim {-11.000790, -3.093972, 2.979381} 131 | * 1 leg trim {-0.572958, 4.927437, -3.093972} 132 | * 2 leg trim {9.969466, 0.000000, 2.979381} 133 | * 3 leg trim {4.927437, 0.000000, -7.104677} 134 | */ 135 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/config_wifi.example.h: -------------------------------------------------------------------------------- 1 | // First credentials for Access point (AP) mode 2 | const char* wifiSsid[] = {"APssid", "ssid1", "ssid2"}; 3 | const char* wifiPass[] = {"APpass", "123", "456"}; 4 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/def.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Some usuful defines 3 | */ 4 | 5 | #define DISPLAY_DIGITS 1 // Number of digits to display after the decimal point 6 | 7 | 8 | #define PCA9685 100 9 | #define MPU9650 101 10 | #define SD1306 102 11 | #define ESP32PWM 103 12 | #define INA219 104 13 | 14 | /** 15 | * Axis 16 | */ 17 | #define ROLL 0 18 | #define PITCH 1 19 | #define YAW 2 20 | 21 | #define SOFTWARE_CORE_VERSION 1 22 | 23 | /* Math */ 24 | #define RADTODEG 180/PI 25 | #define DEGTORAD PI/180 26 | 27 | /* WiFi */ 28 | #define AP_MODE 0 29 | 30 | /* Packages code */ 31 | #define P_TELEMETRY_LEN 14 32 | 33 | #define P_MOVE 77 // ASCII "M" 34 | #define P_TELEMETRY 84 // "T" 35 | 36 | /* Debug (SLOW!!!)*/ 37 | //#define DEBUG_HAL_LEG 38 | 39 | typedef struct { 40 | uint8_t minAngle; // minimal servo angle, deg 41 | uint8_t maxAngle; // maximal servo angle, deg 42 | uint16_t degMin; // PWM pulse at minAngle deg 43 | uint16_t deg30; // PWM pulse at 30 deg 44 | uint16_t deg50; 45 | uint16_t deg70; 46 | uint16_t deg90; // PWM pusle at 90 deg 47 | uint16_t deg110; 48 | uint16_t deg130; 49 | uint16_t deg150; // PWM pulse at 150 deg 50 | uint16_t degMax; // PWM pusle at maxAngle deg 51 | } servoProfile; 52 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/failsafe.ino: -------------------------------------------------------------------------------- 1 | void initFailsafe () { 2 | 3 | } 4 | 5 | void updateFailsafe() { 6 | if (FS_WS_count >= FS_WS_THR) { 7 | setFailsafe(); 8 | return; 9 | } 10 | 11 | FS_FAIL = false; 12 | } 13 | 14 | void setFailsafe() { 15 | vector.move.x = 0; 16 | vector.move.y = 0; 17 | vector.move.z = 0; 18 | vector.rotate.yaw = 0; 19 | 20 | FS_WS_count = FS_WS_THR; 21 | FS_FAIL = true; 22 | } 23 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/gait.ino: -------------------------------------------------------------------------------- 1 | // Gait 2 | gaitConfig GAIT_CONFIG = { 3 | { 4 | { SWING, STANCE, STANCE, SWING }, 5 | { STANCE, SWING, SWING, STANCE } 6 | }, 7 | 2, // items in sequence 8 | LOOP_TIME, // loop time in microseconds to correclty calculate number of sub moves per gate item 9 | 30, // above the ground on swing, 25mm 10 | 100000, // swing duration, microseconds 11 | 200000 // sequence items duration, microseconds 12 | }; 13 | 14 | gait gaitLegLF(GAIT_CONFIG, legs[LEGLF]); 15 | gait gaitLegRF(GAIT_CONFIG, legs[LEGRF]); 16 | gait gaitLegLH(GAIT_CONFIG, legs[LEGLH]); 17 | gait gaitLegRH(GAIT_CONFIG, legs[LEGRH]); 18 | 19 | 20 | void initGait() { 21 | ticksPerGaitItem = GAIT_CONFIG.duration/GAIT_CONFIG.loopTime; 22 | ticksToNextGaitItem = ticksPerGaitItem; 23 | } 24 | 25 | uint8_t getNextGait() 26 | { 27 | nextGait = currentGait + 1; 28 | if (nextGait >= GAIT_CONFIG.sequenceLength) nextGait = 0; 29 | return nextGait; 30 | } 31 | 32 | void updateGait() { 33 | ticksToNextGaitItem--; 34 | 35 | gaitItemProgress = 1 - (float)ticksToNextGaitItem/(float)ticksPerGaitItem; 36 | 37 | figure transitionProgress = bodyTransition.linear(gaitItemProgress); 38 | body.position = transitionProgress.position; 39 | body.orientation = transitionProgress.orientation; 40 | 41 | 42 | 43 | bodyUpdate.update(); 44 | 45 | 46 | 47 | gaitProgress[LEGLF] = gaitLegLF.next(); 48 | gaitProgress[LEGRF] = gaitLegRF.next(); 49 | gaitProgress[LEGLH] = gaitLegLH.next(); 50 | gaitProgress[LEGRH] = gaitLegRH.next(); 51 | 52 | 53 | // TODO we need predict next position of robot to begin move of CoM at 0.8 progress of current and until 0.2 progress of future gait 54 | //bodyBalance.setBody(bodyBalance.getCenter()); 55 | 56 | if (ticksToNextGaitItem <= 0) { 57 | ticksToNextGaitItem = ticksPerGaitItem; 58 | currentGait++; 59 | if (currentGait >= GAIT_CONFIG.sequenceLength) currentGait = 0; 60 | 61 | // set future position - this needs to be done on 0.8 progress for CoM and balance transition, that is also should include body linear transition 62 | walkPlanner.predictPosition(); 63 | 64 | // body linear transition (TODO, include balance here) 65 | bodyTransitionParams.initialValue = body; 66 | bodyTransitionParams.targetValue = walkPlanner.getBodyPosition(); 67 | bodyTransition.set(bodyTransitionParams); 68 | 69 | // run gait workers 70 | for (int i = 0; i < LEG_NUM; i++) { 71 | if (GAIT_CONFIG.sequence[currentGait].leg[i] == SWING) { 72 | switch(i) { 73 | case LEGLF: 74 | gaitLegLF.start(legs[i].foot, walkPlanner.getLegPosition(i)); 75 | break; 76 | case LEGRF: 77 | gaitLegRF.start(legs[i].foot, walkPlanner.getLegPosition(i)); 78 | break; 79 | case LEGLH: 80 | gaitLegLH.start(legs[i].foot, walkPlanner.getLegPosition(i)); 81 | break; 82 | case LEGRH: 83 | gaitLegRH.start(legs[i].foot, walkPlanner.getLegPosition(i)); 84 | break; 85 | } 86 | } 87 | } 88 | } 89 | } 90 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/helpers.ino: -------------------------------------------------------------------------------- 1 | double radToDeg(double angleRad) 2 | { 3 | return angleRad*RADTODEG; 4 | } 5 | 6 | double degToRad(double angleDeg) 7 | { 8 | return angleDeg*DEGTORAD; 9 | } 10 | 11 | double mapf(double val, double in_min, double in_max, double out_min, double out_max) { 12 | return (val - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 13 | } 14 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/i2cscan.ino: -------------------------------------------------------------------------------- 1 | double runI2CScan(double id) { 2 | // We need to run that function on different cores 3 | cliFunctionFAST = runI2CScanService; 4 | runCommandFASTCore = true; 5 | 6 | return 1; 7 | } 8 | 9 | double runI2CScanService(double id) 10 | { 11 | cliSerial->println("i2c scan"); 12 | for (byte i = 8; i < 120; i++) 13 | { 14 | Wire.beginTransmission (i); // Begin I2C transmission Address (i) 15 | if (Wire.endTransmission () == 0) // Receive 0 = success (ACK response) 16 | { 17 | cliSerial->print(i, DEC); 18 | cliSerial->print(" (0x"); 19 | cliSerial->print(i, HEX); 20 | cliSerial->print("), "); 21 | } 22 | } 23 | 24 | return 1; 25 | } 26 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/imu.ino: -------------------------------------------------------------------------------- 1 | void initIMU() 2 | { 3 | Serial.print("IMU "); 4 | setupIMU(); 5 | Serial.println(); 6 | } 7 | 8 | void setupIMU() 9 | { 10 | IMU = MPU9250_WE(IMU_ADDRESS); 11 | 12 | if(!IMU.init()){ 13 | Serial.println("IMU does not respond"); 14 | } 15 | 16 | IMU.setAccRange(MPU9250_ACC_RANGE_2G); 17 | IMU.enableAccDLPF(true); 18 | IMU.setAccDLPF(MPU9250_DLPF_6); 19 | } 20 | 21 | double calibrateIMU(double id) 22 | { 23 | Serial.println("Calibrating ACC and GYRO in 5 seconds. Put device on flat leveled surface."); 24 | delay(5000); 25 | Serial.print("Calibration..."); 26 | IMU.autoOffsets(); 27 | Serial.println("Done."); 28 | 29 | return 1; 30 | } 31 | 32 | void updateIMU() 33 | { 34 | xyzFloat angle = IMU.getAngles(); 35 | 36 | // TODO not sure about correct mapping!!! 37 | IMU_DATA[ROLL] = angle.x; 38 | IMU_DATA[PITCH] = angle.y; 39 | IMU_DATA[YAW] = angle.z; 40 | 41 | } 42 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/HAL_body/HAL_body.cpp: -------------------------------------------------------------------------------- 1 | #include "math.h" 2 | #include "HAL_body.h" 3 | 4 | HAL_body::HAL_body(moveVector &vector, figure &bodyObj, leg legs[LEG_NUM]) 5 | { 6 | _vector = &vector; // TODO that is wrong... we should set body pitch/roll somewhere else (e.g. in IMU process that still not in the code...) 7 | _body = &bodyObj; 8 | for (int i = 0; i < LEG_NUM; i++) { 9 | _legs[i] = &legs[i]; 10 | } 11 | } 12 | 13 | void HAL_body::update() 14 | { 15 | /** 16 | * x' = x * (cosYaw*cosPitch) + y * (cosYaw*sinPitch*sinRoll - sinYaw*cosRoll) + z * (cosYaw*sinPitch*cosRoll + sinYaw*sinRoll) 17 | * y' = x * (sinYaw*cosPitch) + y * (sinYaw*sinPitch*sinRoll + cosYaw*cosRoll) + z * (sinYaw*sinPitch*cosRoll - cosYaw*sinRoll) 18 | * z' = x * (-sinPitch) + y * (cosPitch*sinRoll) + z * (cosPitch*cosRoll) 19 | * @see: https://en.wikipedia.org/wiki/Rotation_matrix#General_rotations 20 | */ 21 | 22 | double sinPitch = sin(_body->orientation.pitch + _vector->rotate.pitch * 0.2); // TODO THIS IS WRONG, it should not be there!!! 23 | double cosPitch = cos(_body->orientation.pitch + _vector->rotate.pitch * 0.2); 24 | 25 | double sinRoll = sin(_body->orientation.roll + _vector->rotate.roll * 0.2); 26 | double cosRoll = cos(_body->orientation.roll + _vector->rotate.roll * 0.2); 27 | 28 | double sinYaw = sin(_body->orientation.yaw); 29 | double cosYaw = cos(_body->orientation.yaw); 30 | 31 | for (int i = 0; i < LEG_NUM; i++) { 32 | _legs[i]->body.x = _body->position.x 33 | + _legs[i]->defaultBody.x * (cosYaw*cosPitch) 34 | + _legs[i]->defaultBody.y * (cosYaw*sinPitch*sinRoll - sinYaw*cosRoll) 35 | + _legs[i]->defaultBody.z * (cosYaw*sinPitch*cosRoll + sinYaw*sinRoll); 36 | 37 | _legs[i]->body.y = _body->position.y 38 | + _legs[i]->defaultBody.x * (sinYaw*cosPitch) 39 | + _legs[i]->defaultBody.y * (sinYaw*sinPitch*sinRoll + cosYaw*cosRoll) 40 | + _legs[i]->defaultBody.z * (sinYaw*sinPitch*cosRoll - cosYaw*sinRoll); 41 | 42 | _legs[i]->body.z = _body->position.z 43 | + _legs[i]->defaultBody.x * (-sinPitch) 44 | + _legs[i]->defaultBody.y * (cosPitch*sinRoll) 45 | + _legs[i]->defaultBody.z * (cosPitch*cosRoll); 46 | } 47 | 48 | } 49 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/HAL_body/HAL_body.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Body state recalculate 3 | * 4 | * MIT Licence 5 | * Developed by Gleb Devyatkin (SovGVD) in 2021 6 | */ 7 | 8 | #ifndef HAL_BODY_h 9 | #define HAL_BODY_h 10 | 11 | #include "../IK/geometry.h" 12 | #include "../IK/leg.h" 13 | 14 | class HAL_body 15 | { 16 | public: 17 | HAL_body(moveVector &vector, figure &bodyObj, leg legs[LEG_NUM]); 18 | void update(); 19 | private: 20 | moveVector *_vector; 21 | figure *_body; 22 | leg *_legs[LEG_NUM]; 23 | 24 | }; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/IK/IK.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Inverse Kinematics for quadruped (four legs) robot 3 | * Boston Dynamics Spot style 4 | * 5 | * MIT Licence 6 | * Developed by Gleb Devyatkin (SovGVD) in 2020 7 | */ 8 | 9 | #include "math.h" 10 | #include "leg.h" 11 | #include "IK.h" 12 | 13 | IK::IK(leg &legObj, figure &bodyObj) 14 | { 15 | _leg = &legObj; 16 | _body = &bodyObj; 17 | } 18 | 19 | void IK::set(leg &legObj, figure &bodyObj) 20 | { 21 | _leg = &legObj; 22 | _body = &bodyObj; 23 | } 24 | 25 | iksolver IK::solve() 26 | { 27 | legangle angle; 28 | iksolver s; 29 | 30 | // normalize by body rotation 31 | double tmpSin = sin(_body->orientation.yaw * -1); 32 | double tmpCos = cos(_body->orientation.yaw * -1); 33 | 34 | point legFoot = { 35 | (_leg->foot.x - _body->position.x) * tmpCos - (_leg->foot.y - _body->position.y) * tmpSin, 36 | (_leg->foot.x - _body->position.x) * tmpSin + (_leg->foot.y - _body->position.y) * tmpCos, 37 | _leg->foot.z 38 | }; 39 | 40 | point legBody = { 41 | (_leg->body.x - _body->position.x) * tmpCos - (_leg->body.y - _body->position.y) * tmpSin, 42 | (_leg->body.x - _body->position.x) * tmpSin + (_leg->body.y - _body->position.y) * tmpCos, 43 | _leg->body.z 44 | }; 45 | 46 | 47 | // TODO: what can I do with limits? 48 | double lx = legFoot.x - legBody.x; if (_leg->inverse.x) { lx = -lx; }; 49 | double ly = legFoot.y - legBody.y; if (_leg->inverse.y) { ly = -ly; }; 50 | double lz = legFoot.z - legBody.z; if (_leg->inverse.z) { lz = -lz; }; 51 | 52 | double a = lx*lx + lz*lz; // square of hypotenuse (points between leg.body and leg.foot in XZ-plane) 53 | double sqrta = sqrt(a); 54 | double dxz = a - _leg->size.l1*_leg->size.l1; // square of hypotenuse between BETA angle joint and ground in XZ plane 55 | double dyz = ly*ly + dxz; // square of hypotenuse between BETA angle joint and ground in YZ plane 56 | double l2p2 = _leg->size.l2*_leg->size.l2; // square of l2 57 | double l3p2 = _leg->size.l3*_leg->size.l3; // square of l3 58 | 59 | angle.alpha = M_PI - ikAsin(lx/sqrta) - ikAcos(_leg->size.l1/sqrta); 60 | 61 | angle.beta = M_PI_2 - ikAcos( (l3p2 - l2p2 - dyz) / (-2 * sqrt(dyz) * _leg->size.l2)) - ikAsin(ly/sqrt(dxz)); 62 | 63 | angle.gamma = ikAcos( (dyz - l2p2 - l3p2) / (-2 * _leg->size.l2 * _leg->size.l3) ); 64 | 65 | s.isSolved = true; // TODO 66 | s.angle = angle; 67 | 68 | return s; 69 | } 70 | 71 | /*point solveByAngle() 72 | { 73 | // TODO 74 | }*/ 75 | 76 | double IK::normalizeAngleRad(double angleRad) 77 | { 78 | // TODO It is not working as expected 79 | //angleRad = fmod(angleRad,M_2_PI); 80 | //if (angleRad < 0) angleRad += M_2_PI; 81 | return angleRad; 82 | } 83 | 84 | // this can be update to use table of angles (faster) insted of using trigonometry functions 85 | // or at least add cache 86 | double IK::ikAtan2(double x, double y) 87 | { 88 | return normalizeAngleRad(atan2(x, y)); 89 | } 90 | 91 | double IK::ikAcos(double angleRad) 92 | { 93 | return normalizeAngleRad(acos(angleRad)); 94 | } 95 | 96 | double IK::ikAsin(double angleRad) 97 | { 98 | return normalizeAngleRad(asin(angleRad)); 99 | } 100 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/IK/IK.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Inverse Kinematics for quadruped (four legs) robot 3 | * Boston Dynamics Spot style 4 | * 5 | * MIT Licence 6 | * Developed by Gleb Devyatkin (SovGVD) in 2020 7 | */ 8 | 9 | // WARNING: this is WIP and this library was not tested in any software or hardware!!! 10 | 11 | /** 12 | * All axis aligned with MPU9250 13 | * 14 | * 15 | * | Z-> Y 16 | * | | 17 | * --------0-+ v X 18 | * | ...l1 19 | * 0---0 20 | * 21 | * 22 | * beta. alpha. 23 | * . . 24 | * . | ^ Z .| ^ Z 25 | * 0 | | ^ 0--0 | 26 | * ------ / -+ X-> Y . | +--- X <-Y 27 | * / ...l2 d | 28 | * gamma...0 . 0 29 | * \ . | 30 | * \ ...l3 v | 31 | * 32 | * 33 | */ 34 | 35 | /** 36 | * 0---0 (legLH) 0---0 (legLF) 37 | * | | 38 | * +--0---------------0--+ 39 | * | | 40 | * | | 41 | * | | 42 | * +--0---------------0--* 43 | * | | 44 | * 0---0 (legRH) 0---0 (legRF) 45 | */ 46 | #ifndef IK_h 47 | #define IK_h 48 | 49 | #include "geometry.h" 50 | #include "leg.h" 51 | 52 | typedef struct { 53 | bool isSolved; 54 | legangle angle; 55 | } iksolver; 56 | 57 | class IK 58 | { 59 | public: 60 | IK(leg &legObj, figure &bodyObj); 61 | void set(leg &legObj, figure &bodyObj); 62 | iksolver solve(); 63 | //point solveByAngle(); // use to init position by initial angles 64 | double normalizeAngleRad(double angleRad); 65 | private: 66 | leg *_leg; 67 | figure *_body; 68 | double ikAtan2(double x, double y); 69 | double ikAcos(double angleRad); 70 | double ikAsin(double angleRad); 71 | point bodyBalance = {0, 0, 0}; // TODO this is just for tests 72 | }; 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/IK/IK_simple.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Inverse Kinematics for quadruped (four legs) robot 3 | * Boston Dynamics Spot style 4 | * 5 | * This IK is simple because leg is in one plane. 6 | * 7 | * _ 8 | * .|\ . 9 | * -----| . \ . L -----| 10 | * --- -----0/ \ a . E -----0--. --- 11 | * ^ ----/.. \ \ . G ----/. | ^ 12 | * | ---` . . \ _\| . ---` . | | 13 | * lz . . / . . P 0--` a 14 | * | . 0 . . L | | ^Z 15 | * v . \ . . A | v | 16 | * -------- . 0 . N 0 ------ 0-->X 17 | * || . E 18 | * . 19 | * 20 | * LEG PLANE 21 | * 22 | * -----0 0 23 | * ^ `. ^`. 24 | * | `. \ `. 25 | * a 0 b 0 26 | * | / \ / ^legplane 27 | * v / v/ | 28 | * -------0 0 Y<--0 29 | * |<-ly->| 30 | * 31 | * 32 | * MIT Licence 33 | * Developed by Gleb Devyatkin (SovGVD) in 2021 34 | */ 35 | 36 | #include "math.h" 37 | #include "leg.h" 38 | #include "IK_simple.h" 39 | 40 | IK::IK(leg &legObj, figure &bodyObj) 41 | { 42 | _leg = &legObj; 43 | _body = &bodyObj; 44 | } 45 | 46 | void IK::set(leg &legObj, figure &bodyObj) 47 | { 48 | _leg = &legObj; 49 | _body = &bodyObj; 50 | } 51 | 52 | iksolver IK::solve() 53 | { 54 | legangle angle; 55 | iksolver s; 56 | 57 | // normalize by body rotation 58 | // We need it to make IK simple ignoring body transitions, and at the same time still have absolute positions for each robot points 59 | double tmpSin = sin(_body->orientation.yaw * -1); 60 | double tmpCos = cos(_body->orientation.yaw * -1); 61 | 62 | point legFoot = { 63 | (_leg->foot.x - _body->position.x) * tmpCos - (_leg->foot.y - _body->position.y) * tmpSin, 64 | (_leg->foot.x - _body->position.x) * tmpSin + (_leg->foot.y - _body->position.y) * tmpCos, 65 | _leg->foot.z 66 | }; 67 | 68 | point legBody = { 69 | (_leg->body.x - _body->position.x) * tmpCos - (_leg->body.y - _body->position.y) * tmpSin, 70 | (_leg->body.x - _body->position.x) * tmpSin + (_leg->body.y - _body->position.y) * tmpCos, 71 | _leg->body.z 72 | }; 73 | 74 | 75 | // TODO: what can I do with limits? 76 | double lx = legFoot.x - legBody.x; if (_leg->inverse.x) { lx = -lx; }; 77 | double ly = legFoot.y - legBody.y; if (_leg->inverse.y) { ly = -ly; }; 78 | double lz = legFoot.z - legBody.z; if (_leg->inverse.z) { lz = -lz; }; 79 | 80 | 81 | double a = sq(lx) + sq(lz); // square of hypotenuse (points between leg.body and leg.foot in XZ-plane) 82 | double sqrta = sqrt(a); 83 | double b = sq(ly) + a; // square of hypotenuse (points between leg.body and leg.foot in YLeg-plane) 84 | double sqrtb = sqrt(b); // square of hypotenuse (points between leg.body and leg.foot in YZ-plane) 85 | double l2p2 = sq(_leg->size.l2); // square of l2 TODO this is const actually 86 | double l3p2 = sq(_leg->size.l3); // square of l3 TODO this is const actually 87 | 88 | 89 | // XZ plane 90 | angle.alpha = M_PI_2 - ikAsin(lx/sqrta); 91 | 92 | // YLeg plane 93 | angle.beta = M_PI_2 - ikAsin(ly/sqrtb) - ikAcos( (b+l2p2-l3p2) / (2 * sqrtb * _leg->size.l2)); 94 | 95 | // YLeg plane 96 | angle.gamma = ikAcos((l2p2 + l3p2 - b) / (2 * _leg->size.l2 * _leg->size.l3)); 97 | 98 | s.isSolved = true; // TODO 99 | s.angle = angle; 100 | 101 | return s; 102 | } 103 | 104 | /*point solveByAngle() 105 | { 106 | // TODO 107 | }*/ 108 | 109 | double IK::normalizeAngleRad(double angleRad) 110 | { 111 | // TODO It is not working as expected 112 | //angleRad = fmod(angleRad,M_2_PI); 113 | //if (angleRad < 0) angleRad += M_2_PI; 114 | return angleRad; 115 | } 116 | 117 | // this can be update to use table of angles (faster) insted of using trigonometry functions 118 | // or at least add cache 119 | double IK::ikAtan2(double x, double y) 120 | { 121 | return normalizeAngleRad(atan2(x, y)); 122 | } 123 | 124 | double IK::ikAcos(double angleRad) 125 | { 126 | return normalizeAngleRad(acos(angleRad)); 127 | } 128 | 129 | double IK::ikAsin(double angleRad) 130 | { 131 | return normalizeAngleRad(asin(angleRad)); 132 | } 133 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/IK/IK_simple.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Inverse Kinematics for quadruped (four legs) robot 3 | * Boston Dynamics Spot style 4 | * 5 | * MIT Licence 6 | * Developed by Gleb Devyatkin (SovGVD) in 2021 7 | */ 8 | 9 | /** 10 | * All axis aligned with MPU9250 11 | * 12 | * 13 | * | Z-> Y 14 | * | | 15 | * --------0-+ v X 16 | * | ...l1 17 | * 0---0 18 | * 19 | * 20 | * beta. alpha. 21 | * . . 22 | * . | ^ Z .| ^ Z 23 | * 0 | | ^ 0--0 | 24 | * ------ / -+ X-> Y . | +--- X <-Y 25 | * / ...l2 d | 26 | * gamma...0 . 0 27 | * \ . | 28 | * \ ...l3 v | 29 | * 30 | * 31 | */ 32 | 33 | /** 34 | * 0---0 (legLH) 0---0 (legLF) 35 | * | | 36 | * +--0---------------0--+ 37 | * | | 38 | * | | 39 | * | | 40 | * +--0---------------0--* 41 | * | | 42 | * 0---0 (legRH) 0---0 (legRF) 43 | */ 44 | #ifndef IK_h 45 | #define IK_h 46 | 47 | #include "geometry.h" 48 | #include "leg.h" 49 | 50 | typedef struct { 51 | bool isSolved; 52 | legangle angle; 53 | } iksolver; 54 | 55 | class IK 56 | { 57 | public: 58 | IK(leg &legObj, figure &bodyObj); 59 | void set(leg &legObj, figure &bodyObj); 60 | iksolver solve(); 61 | //point solveByAngle(); // use to init position by initial angles 62 | double normalizeAngleRad(double angleRad); 63 | private: 64 | leg *_leg; 65 | figure *_body; 66 | double ikAtan2(double x, double y); 67 | double ikAcos(double angleRad); 68 | double ikAsin(double angleRad); 69 | point bodyBalance = {0, 0, 0}; // TODO this is just for tests 70 | }; 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/IK/geometry.h: -------------------------------------------------------------------------------- 1 | #ifndef geometry_h 2 | #define geometry_h 3 | 4 | #define ALPHA 0 5 | #define BETA 1 6 | #define GAMMA 2 7 | 8 | #define XAXIS 0 9 | #define YAXIS 1 10 | #define ZAXIS 2 11 | #define PITCHAXIS 3 12 | #define ROLLAXIS 4 13 | #define YAWAXIS 5 14 | 15 | // pount in 3D 16 | typedef struct { 17 | double x; 18 | double y; 19 | double z; 20 | } point; 21 | 22 | // angle in 3D 23 | typedef struct { 24 | double roll; // X axis 25 | double pitch; // Y axis 26 | double yaw; // Z axis 27 | } angle; 28 | 29 | //body 30 | typedef struct { 31 | point position; 32 | angle orientation; 33 | } figure; 34 | 35 | //move 36 | typedef struct { 37 | point move; 38 | angle rotate; 39 | } moveVector; 40 | 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/IK/leg.h: -------------------------------------------------------------------------------- 1 | #ifndef leg_h 2 | #define leg_h 3 | 4 | #include "geometry.h" 5 | #define LEGLF 0 // left front leg 6 | #define LEGRF 1 // right front leg 7 | #define LEGLH 2 // left hind leg 8 | #define LEGRH 3 // right hind leg 9 | 10 | // see IK.h 11 | typedef struct { 12 | double alpha; 13 | double beta; 14 | double gamma; 15 | } legangle; 16 | 17 | //Leg gears ratio 18 | typedef struct { 19 | double alpha; 20 | double beta; 21 | double gamma; 22 | } leggear; 23 | 24 | // see IK.h 25 | typedef struct { 26 | double l1; 27 | double l2; 28 | double l3; 29 | } legsize; 30 | 31 | // Hardware pin numbers 32 | typedef struct { 33 | int alpha; 34 | int beta; 35 | int gamma; 36 | } legpin; 37 | 38 | // Leg sensor 39 | typedef struct { 40 | bool onGround; // State 41 | int pin; // Settings 42 | int threshold; 43 | } legsensor; 44 | 45 | // Leg IK helpers (this is moslty need to set where is leg: left or right side, to inverse some values) 46 | typedef struct { 47 | bool x; 48 | bool y; 49 | bool z; 50 | bool alpha; 51 | bool beta; 52 | bool gamma; 53 | } leginverse; 54 | 55 | // Leg HAL values 56 | typedef struct { 57 | legangle mid; // Hardware middle 58 | legpin pin; 59 | legangle trim; 60 | leggear ratio; 61 | } leghal; 62 | 63 | typedef struct { 64 | int id; 65 | char title[4]; 66 | } legid; 67 | 68 | // Leg structure 69 | typedef struct { 70 | const legid id; 71 | const point defaultBody; 72 | const point defaultFoot; 73 | const legsize size; 74 | const legangle min; // setup limits of angles 75 | const legangle max; // setup limits of angles 76 | leghal hal; 77 | point body; 78 | point foot; 79 | legangle angle; 80 | leginverse inverse; 81 | legsensor sensor; 82 | } leg; 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/balance/balance.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Balance helper methods 3 | * 4 | * MIT Licence 5 | * Developed by Gleb Devyatkin (SovGVD) in 2020 6 | */ 7 | 8 | #include "balance.h" 9 | 10 | balance::balance(point &offset, figure &bodyObj, leg &legLF, leg &legRF, leg &legLH, leg &legRH) 11 | { 12 | _offset = &offset; // this should be calculated? base on servos and legs positions and weight 13 | _body = &bodyObj; 14 | _legLF = &legLF; 15 | _legRF = &legRF; 16 | _legLH = &legLH; 17 | _legRH = &legRH; 18 | } 19 | 20 | 21 | point balance::getCenter() 22 | { 23 | // TODO... can we ignore Z? as it will be projection of BODY center of mass (CoM) to legs-on-ground plain 24 | // This is not technicaly correct, we are not geting center of mass, but just the point where it should be as projection 25 | _legsOnGround = 0; 26 | _CoM = {0, 0, 0}; 27 | 28 | // this is terrible 29 | _legToBalance(_legLF); 30 | _legToBalance(_legRF); 31 | _legToBalance(_legLH); 32 | _legToBalance(_legRH); 33 | 34 | 35 | 36 | _CoM.x = _CoM.x/_legsOnGround + _offset->x; 37 | _CoM.y = _CoM.y/_legsOnGround + _offset->y; 38 | _CoM.z = _CoM.z/_legsOnGround + _offset->z; 39 | 40 | /*Serial.print(_legsOnGround); 41 | Serial.print("|CoM "); 42 | Serial.print(_CoM.x); 43 | Serial.print(","); 44 | Serial.print(_CoM.y); 45 | Serial.print(","); 46 | Serial.println(_CoM.z);*/ 47 | 48 | return _CoM; 49 | } 50 | 51 | void balance::_legToBalance(leg *_leg) { 52 | if (_leg->sensor.onGround) { 53 | _CoM.x += _leg->foot.x; 54 | _CoM.y += _leg->foot.y; 55 | _CoM.z += _leg->foot.z; 56 | _legsOnGround++; 57 | } 58 | } 59 | 60 | void balance::setBody(point CoM) 61 | { 62 | // TODO that is kind of wrong, as we need to use body projection 63 | _body->position.x = CoM.x; 64 | _body->position.y = CoM.y; 65 | //_body->position.z = CoM.z; 66 | } 67 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/balance/balance.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Balance helper methods 3 | * 4 | * MIT Licence 5 | * Developed by Gleb Devyatkin (SovGVD) in 2020 6 | */ 7 | 8 | #ifndef balance_h 9 | #define balance_h 10 | 11 | #include "../IK/geometry.h" 12 | #include "../IK/leg.h" 13 | 14 | class balance 15 | { 16 | public: 17 | balance(point &offset, figure &bodyObj, leg &legLF, leg &legRF, leg &legLH, leg &legRH); 18 | point getCenter(); 19 | void setBody(point CoM); 20 | private: 21 | point *_offset; 22 | figure *_body; 23 | leg *_legLF; 24 | leg *_legRF; 25 | leg *_legLH; 26 | leg *_legRH; 27 | uint8_t _legsOnGround; 28 | point _CoM; 29 | void _legToBalance(leg *_leg); 30 | }; 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/gait/gait.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT Licence 3 | * Developed by Gleb Devyatkin (SovGVD) in 2020 4 | */ 5 | 6 | #include "gait.h" 7 | 8 | gait::gait(gaitConfig &config, leg &legObj) 9 | { 10 | _config = &config; 11 | _leg = &legObj; 12 | } 13 | 14 | /** 15 | * Return state if this gait is in progress 16 | */ 17 | double gait::next() { 18 | if (ticksToStop > 0) { 19 | ticksToStop--; 20 | progress = 1 - (float)ticksToStop/(float)ticksMax; 21 | 22 | _leg->foot = _transition.swing(progress).position; 23 | _leg->sensor.onGround = false; 24 | } else { 25 | progress = 0; 26 | // TODO use real sesors and stop gait if leg touch the ground 27 | _leg->sensor.onGround = true; 28 | } 29 | 30 | return progress; 31 | } 32 | 33 | void gait::start(point from, point to) { 34 | ticksToStop = _config->swingDuration/_config->loopTime; 35 | ticksMax = ticksToStop; 36 | 37 | tParams = { 38 | {from, {0,0,0}}, 39 | {to, {0,0,0}}, 40 | _config->offTheGround 41 | }; 42 | // TODO add something to compare `point` 43 | if (from.x == to.x && from.y == to.y && from.z == to.z) { 44 | // don't move if from and to equal 45 | ticksToStop = 0; 46 | } 47 | _transition.set(tParams); 48 | } 49 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/gait/gait.h: -------------------------------------------------------------------------------- 1 | #ifndef gait_h 2 | #define gait_h 3 | 4 | #include "../IK/IK.h" 5 | #include "../transition/transition.h" 6 | #include "../transition/transition.cpp" 7 | 8 | 9 | /* Gait actions */ 10 | #define IGNORE 0 // Leg ignored 11 | #define STANCE 1 // Leg on ground 12 | #define SWING 2 // Leg off ground 13 | 14 | 15 | typedef struct gaitSequence_t { 16 | const uint8_t leg[4]; 17 | } gaitSequence; 18 | 19 | typedef struct gaitConfig_t { 20 | const gaitSequence sequence[16]; 21 | const uint8_t sequenceLength; 22 | const double loopTime; // loop time in microseconds to correclty calculate number of sub moves per gate item, milliseconds 23 | double offTheGround; // in mm 24 | double swingDuration; // duration of swing in microseconds (1sec = 1000 millisec, 1sec = 1000000microsec) 25 | double duration; // duration of gaitSequence item, in microseconds, should be equal or longer than swingDuration 26 | } gaitConfig; 27 | 28 | class gait 29 | { 30 | public: 31 | gait(gaitConfig &config, leg &legObj); 32 | void start(point from, point to); 33 | double next(); 34 | private: 35 | gaitConfig *_config; 36 | leg *_leg; 37 | 38 | transition _transition; 39 | transitionParameters tParams; 40 | 41 | double progress; 42 | uint16_t ticksToStop = 0; 43 | uint16_t ticksMax = 0; 44 | uint8_t _currentGait = 255; 45 | 46 | }; 47 | #endif 48 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/planner/planner.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Planner 3 | * 4 | * MIT Licence 5 | * Developed by Gleb Devyatkin (SovGVD) in 2020 6 | */ 7 | // TODO use gait data to avoid unnecessary legs update 8 | // TODO do this with matrix and for 3D 9 | 10 | #include "planner.h" 11 | 12 | planner::planner(moveVector &vector, figure &bodyObj, leg &legLF, leg &legRF, leg &legLH, leg &legRH) 13 | { 14 | _vector = &vector; 15 | _body = &bodyObj; 16 | _legLF = &legLF; 17 | _legRF = &legRF; 18 | _legLH = &legLH; 19 | _legRH = &legRH; 20 | } 21 | 22 | void planner::predictPosition(uint8_t steps = 1) 23 | { 24 | // TODO normalize angle !!! very important !!! 25 | _predictedBody.orientation.pitch = _body->orientation.pitch; 26 | _predictedBody.orientation.roll = _body->orientation.roll; 27 | _predictedBody.orientation.yaw = _body->orientation.yaw + rotateInc * _vector->rotate.yaw; 28 | 29 | 30 | double tmpSin = sin(_predictedBody.orientation.yaw); 31 | double tmpCos = cos(_predictedBody.orientation.yaw); 32 | 33 | // TODO use matrix 34 | _predictedBody.position.x = _body->position.x + moveInc * (_vector->move.x * tmpCos - _vector->move.y * tmpSin); 35 | _predictedBody.position.y = _body->position.y + moveInc * (_vector->move.x * tmpSin + _vector->move.y * tmpCos); 36 | _predictedBody.position.z = _body->position.z; 37 | 38 | 39 | // This is terible (code) 40 | // Im trying to get new position of legs based on rotation of default position for XY-plane 41 | // this is OK for first time, but terrible for anything else 42 | 43 | _predictedLegLFfoot.x = _predictedBody.position.x + _legLF->defaultFoot.x * tmpCos - _legLF->defaultFoot.y * tmpSin; 44 | _predictedLegLFfoot.y = _predictedBody.position.y + _legLF->defaultFoot.x * tmpSin + _legLF->defaultFoot.y * tmpCos; 45 | _predictedLegLFfoot.z = _legLF->defaultFoot.z; 46 | 47 | _predictedLegRFfoot.x = _predictedBody.position.x + _legRF->defaultFoot.x * tmpCos - _legRF->defaultFoot.y * tmpSin; 48 | _predictedLegRFfoot.y = _predictedBody.position.y + _legRF->defaultFoot.x * tmpSin + _legRF->defaultFoot.y * tmpCos; 49 | _predictedLegRFfoot.z = _legRF->defaultFoot.z; 50 | 51 | _predictedLegLHfoot.x = _predictedBody.position.x + _legLH->defaultFoot.x * tmpCos - _legLH->defaultFoot.y * tmpSin; 52 | _predictedLegLHfoot.y = _predictedBody.position.y + _legLH->defaultFoot.x * tmpSin + _legLH->defaultFoot.y * tmpCos; 53 | _predictedLegLHfoot.z = _legLH->defaultFoot.z; 54 | 55 | _predictedLegRHfoot.x = _predictedBody.position.x + _legRH->defaultFoot.x * tmpCos - _legRH->defaultFoot.y * tmpSin; 56 | _predictedLegRHfoot.y = _predictedBody.position.y + _legRH->defaultFoot.x * tmpSin + _legRH->defaultFoot.y * tmpCos; 57 | _predictedLegRHfoot.z = _legRH->defaultFoot.z; 58 | } 59 | 60 | figure planner::getBodyPosition() 61 | { 62 | return _predictedBody; 63 | } 64 | 65 | point planner::getLegPosition(uint8_t legId) 66 | { 67 | // this is looks so dump 68 | switch(legId) { 69 | case LEGLF: return _predictedLegLFfoot; 70 | case LEGRF: return _predictedLegRFfoot; 71 | case LEGLH: return _predictedLegLHfoot; 72 | case LEGRH: return _predictedLegRHfoot; 73 | } 74 | 75 | // i hope it will never happens 76 | return {0,0,0}; 77 | } 78 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/planner/planner.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Planner 3 | * Almost, it is just move body and legs to the new point 4 | * Also it looks like a good place to check if new possition possible 5 | * 6 | * MIT Licence 7 | * Developed by Gleb Devyatkin (SovGVD) in 2020 8 | */ 9 | 10 | #ifndef planner_h 11 | #define planner_h 12 | 13 | #include "../IK/geometry.h" 14 | #include "../IK/leg.h" 15 | 16 | class planner 17 | { 18 | public: 19 | planner(moveVector &vector, figure &bodyObj, leg &legLF, leg &legRF, leg &legLH, leg &legRH); 20 | void predictPosition(uint8_t steps); 21 | figure getBodyPosition(); 22 | point getLegPosition(uint8_t legId); 23 | private: 24 | moveVector *_vector; 25 | figure *_body; 26 | leg *_legLF; 27 | leg *_legRF; 28 | leg *_legLH; 29 | leg *_legRH; 30 | figure _predictedBody; 31 | point _predictedLegLFfoot; 32 | point _predictedLegRFfoot; 33 | point _predictedLegLHfoot; 34 | point _predictedLegRHfoot; 35 | double moveInc = 30; 36 | double rotateInc = -0.3; 37 | }; 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/transition/transition.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Transitions for leg movement 3 | * 4 | * MIT Licence 5 | * Developed by Gleb Devyatkin (SovGVD) in 2020 6 | */ 7 | 8 | #include "math.h" 9 | #include "transition.h" 10 | 11 | transition::transition() 12 | { 13 | } 14 | 15 | void transition::set(transitionParameters param) 16 | { 17 | _param = param; 18 | 19 | dPx = _param.targetValue.position.x - _param.initialValue.position.x; 20 | dPy = _param.targetValue.position.y - _param.initialValue.position.y; 21 | dPz = _param.targetValue.position.z - _param.initialValue.position.z; 22 | 23 | dOp = _param.targetValue.orientation.pitch - _param.initialValue.orientation.pitch; 24 | dOr = _param.targetValue.orientation.roll - _param.initialValue.orientation.roll; 25 | dOy = _param.targetValue.orientation.yaw - _param.initialValue.orientation.yaw; 26 | 27 | P_z1 = _param.initialValue.position.z + _param.offTheGround; 28 | P_z2 = _param.targetValue.position.z + _param.offTheGround; 29 | } 30 | 31 | 32 | /** 33 | * progress [0,1]; 34 | */ 35 | figure transition::swing(double progress) 36 | { 37 | f.position.x = progress*dPx + _param.initialValue.position.x; 38 | f.position.y = progress*dPy + _param.initialValue.position.y; 39 | 40 | if (progress <= TRANSITION_PROGRESS_STEP1) { 41 | stepProgress = progress/TRANSITION_PROGRESS_STEP1; 42 | dPz = P_z1 - _param.initialValue.position.z; 43 | z = _param.initialValue.position.z; 44 | 45 | } else if (progress <= TRANSITION_PROGRESS_STEP2) { 46 | stepProgress = (progress - TRANSITION_PROGRESS_STEP1)/(TRANSITION_PROGRESS_STEP2 - TRANSITION_PROGRESS_STEP1); 47 | dPz = P_z2 - P_z1; 48 | z = P_z1; 49 | 50 | } else { 51 | stepProgress = (progress - TRANSITION_PROGRESS_STEP2)/(1 - TRANSITION_PROGRESS_STEP2); 52 | dPz = _param.targetValue.position.z - P_z2; 53 | z = P_z2; 54 | 55 | } 56 | f.position.z = stepProgress*dPz + z; 57 | 58 | return f; 59 | } 60 | 61 | figure transition::linear(double progress) 62 | { 63 | f.position.x = progress*dPx + _param.initialValue.position.x; 64 | f.position.y = progress*dPy + _param.initialValue.position.y; 65 | f.position.z = progress*dPz + _param.initialValue.position.z; 66 | 67 | f.orientation.pitch = progress*dOp + _param.initialValue.orientation.pitch; 68 | f.orientation.roll = progress*dOr + _param.initialValue.orientation.roll; 69 | f.orientation.yaw = progress*dOy + _param.initialValue.orientation.yaw; 70 | 71 | return f; 72 | } 73 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/libs/transition/transition.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Transitions for leg movement 3 | * 4 | * MIT Licence 5 | * Developed by Gleb Devyatkin (SovGVD) in 2020 6 | */ 7 | 8 | // WARNING: this is WIP and this library was not tested in any software or hardware!!! 9 | 10 | /** 11 | * transition of the leg is the set on two parabolic (TODO) functions and line on progress=[0,1]; 12 | * at that moment I will use just a line 13 | * 14 | * Z 15 | * ^ 16 | * | 17 | * | 18 | * | TODO parabolic func 19 | * 1| .----------. 20 | * | /. .\ 21 | * | / . . \ 22 | * | / . . \ 23 | * | / . . \ 24 | * |/ . . \ 25 | * 0+-------*----------*-------*----> XY (progress) 26 | * 0 0.2 0.8 1 27 | * 28 | * 29 | * 30 | * 31 | * x - x1 y - y1 z - z1 32 | * ------- = ------- = ------- 33 | * x2 - x1 y2 - y1 z2 - z1 34 | * 35 | * { 36 | * x = t*ax + x1; 37 | * ax = x2 - x1; 38 | * 39 | * y = t*(y2 - y1) + y1; 40 | * ay = y2 - y1; 41 | * 42 | * z = t*(z2 - z1) + z1; 43 | * az = z2 - z1; 44 | * 45 | * t = [0,1]; 46 | * } 47 | * 48 | * 49 | * Z 50 | * ^ 51 | * | 52 | * | 53 | * | point1 54 | * 1| *---___ 55 | * | /. ---*point2 56 | * | / . . \ 57 | * | / . . \ 58 | * | /k1 . k2 . k3\ 59 | * |/ . . \ targetValue 60 | * 0*-----|---------|-----*----> XY (progress) 61 | * 0 0.2 0.8 1 62 | * initialValue 63 | * 64 | * (yes, 0XY != 0Y, just as a sketch) 65 | * initialValue 66 | * *-----|---------|-----*--->Y 67 | * |\_ . . 68 | * | \ _ . 69 | * | *\ _ . 70 | * | 0.2 \ _ . progressLength 71 | * | \ _. . 72 | * | * _ . 73 | * | 0.8 \ _ 74 | * v * 75 | * X 1 76 | */ 77 | #ifndef transition_h 78 | #define transition_h 79 | 80 | #define TRANSITION_PROGRESS_STEP1 0.4 81 | #define TRANSITION_PROGRESS_STEP2 0.4 82 | 83 | typedef struct { 84 | figure initialValue; 85 | figure targetValue; 86 | double offTheGround; 87 | } transitionParameters; 88 | 89 | class transition 90 | { 91 | public: 92 | transition(); 93 | void set(transitionParameters param); 94 | figure swing(double progress); 95 | figure linear(double progress); 96 | private: 97 | transitionParameters _param; 98 | figure f; 99 | double stepProgress; 100 | double dPx; 101 | double dPy; 102 | double dPz; 103 | double dOp; 104 | double dOr; 105 | double dOy; 106 | double z; 107 | double P_z1; // height off the ground on step1 108 | double P_z2; // height off the ground on step2 109 | }; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/packagesProcess.ino: -------------------------------------------------------------------------------- 1 | int bytesToInt16(uint8_t byte1, uint8_t byte2) { 2 | return (byte1 << 8) | byte2; 3 | } 4 | 5 | void pMove(uint8_t* data) { 6 | if (data[1] == 0) setFailsafe(); 7 | 8 | vector.move.x = float(bytesToInt16( data[2], data[3]))/10000-1; 9 | vector.move.y = float(bytesToInt16( data[4], data[5]))/10000-1; 10 | vector.move.z = float(bytesToInt16( data[6], data[7]))/10000-1; 11 | vector.rotate.pitch = float(bytesToInt16( data[8], data[9]))/10000-1; 12 | vector.rotate.roll = float(bytesToInt16(data[10], data[11]))/10000-1; 13 | vector.rotate.yaw = float(bytesToInt16(data[12], data[13]))/10000-1; 14 | } 15 | 16 | void pTelemetry() { 17 | /** 18 | * Telemetry package map 19 | * offset | type | length | description 20 | * -------|------|--------|------------- 21 | * 0 | char | 1 | 'T' (P_TELEMETRY) 22 | * 1 | | 1 | add something 23 | * 2 | bin | 2 | 16 statuses 24 | * 4 | int | 2 | voltage*1000 25 | * 6 | int | 2 | current*1000 26 | * 8 | int | 2 | loop time in microseconds 27 | */ 28 | telemetryPackage[0] = P_TELEMETRY; 29 | // TODO statuses 30 | int16ToBytes(getPowerSensorVoltage()*1000, 4); 31 | int16ToBytes(getPowerSensorCurrent()*1000, 6); 32 | int16ToBytes(loopTime, 8); 33 | } 34 | 35 | void int16ToBytes(int value, uint8_t _offset) 36 | { 37 | telemetryPackage[_offset] = (value >> 8) & 0xFF; 38 | telemetryPackage[_offset+1] = value & 0xFF; 39 | } 40 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/powerSensor.ino: -------------------------------------------------------------------------------- 1 | bool powerSensorReady = false; 2 | void initPowerSensor() { 3 | Serial.print("Power sensor "); 4 | #if POWER_SENSOR == INA219 5 | Serial.print("INA219"); 6 | #else 7 | Serial.print(""); 8 | #endif 9 | setupPowerSensor(); 10 | Serial.println(); 11 | } 12 | 13 | void updatePower() { 14 | voltage_V = powerSensorReady ? ina219.getBusVoltage_V() : 0.0; 15 | current_A = powerSensorReady ? ina219.getCurrent_mA() / 1000 : 0.0; 16 | } 17 | 18 | void setupPowerSensor() 19 | { 20 | #if POWER_SENSOR == INA219 21 | ina219 = INA219_WE(); 22 | 23 | if (ina219.init()) { 24 | powerSensorReady = true; 25 | } else { 26 | Serial.println("Failed to find INA219 chip"); 27 | } 28 | 29 | ina219.setADCMode(BIT_MODE_9); // just to make it fast 30 | ina219.setPGain(PG_320); 31 | ina219.setBusRange(BRNG_16); 32 | #endif 33 | } 34 | 35 | // return V 36 | double getPowerSensorVoltage() { 37 | return voltage_V; 38 | } 39 | 40 | 41 | // return A 42 | double getPowerSensorCurrent() { 43 | return current_A; 44 | } 45 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/robot_dog_esp32.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "libs/IK/geometry.h" 4 | #include "libs/IK/leg.h" 5 | #include "def.h" 6 | #include "config.h" 7 | #include "config_small.h" 8 | #include "config_wifi.h" 9 | #include "libs/IK/IK_simple.h" // TODO this is for small dog only!!! 10 | #include "libs/planner/planner.h" 11 | #include "libs/balance/balance.h" 12 | #include "libs/gait/gait.h" 13 | #include "libs/HAL_body/HAL_body.h" 14 | 15 | #include 16 | #include "WiFi.h" 17 | #include "ESPAsyncWebServer.h" 18 | #include "web/index.html.gz.h" 19 | #include "cli.h" 20 | #include "subscription.h" 21 | 22 | #include 23 | #include 24 | 25 | #include "libs/IK/IK_simple.cpp" // TODO this is for small dog only!!! 26 | #include "libs/planner/planner.cpp" 27 | #include "libs/balance/balance.cpp" 28 | #include "libs/gait/gait.cpp" 29 | #include "libs/HAL_body/HAL_body.cpp" 30 | 31 | /** 32 | * Hardware libraries 33 | */ 34 | #if PWM_CONTROLLER_TYPE == PCA9685 35 | #include 36 | #endif 37 | 38 | #if PWM_CONTROLLER_TYPE == ESP32PWM 39 | #define USE_ESP32_TIMER_NO 3 40 | #include "ESP32_ISR_Servo.h" 41 | #endif 42 | 43 | #ifdef POWER_SENSOR 44 | float voltage_V = 0.0; 45 | float current_A = 0.0; 46 | 47 | #if POWER_SENSOR == INA219 48 | #include 49 | INA219_WE ina219; 50 | #endif 51 | #endif 52 | 53 | float IMU_DATA[3] = {0, 0, 0}; 54 | MPU9250_WE IMU; 55 | 56 | // run commands on diferent cores (FAST for main, SLOW for services) 57 | bool runCommandFASTCore = false; 58 | bool runCommandSLOWCore = false; 59 | cliFunction cliFunctionFAST; 60 | cliFunction cliFunctionSLOW; 61 | double cliFunctionFASTVar = 0.0; 62 | double cliFunctionSLOWVar = 0.0; 63 | 64 | TaskHandle_t ServicesTask; 65 | 66 | #if PWM_CONTROLLER_TYPE == PCA9685 67 | Adafruit_PWMServoDriver pwm; 68 | #endif 69 | 70 | unsigned long currentTime; 71 | unsigned long previousTime; 72 | unsigned long loopTime; 73 | 74 | unsigned long serviceCurrentTime; 75 | 76 | unsigned long servicePreviousTime; 77 | unsigned long serviceLoopTime; 78 | 79 | unsigned long serviceFastPreviousTime; 80 | unsigned long serviceFastLoopTime; 81 | 82 | bool HALEnabled = true; 83 | 84 | // Gait 85 | uint16_t ticksPerGaitItem = 0; 86 | uint16_t ticksToNextGaitItem = 0; 87 | uint8_t currentGait = 0; 88 | uint8_t nextGait = 0; 89 | double gaitItemProgress = 0; 90 | double gaitProgress[] = {0, 0, 0, 0}; 91 | 92 | transition bodyTransition; 93 | transitionParameters bodyTransitionParams = {{0,0,0}, {0,0,0}, 0}; 94 | 95 | //Move 96 | moveVector vector = { 97 | {0, 0, 0}, 98 | {0, 0, 0} 99 | }; 100 | 101 | //Failsafe 102 | bool FS_FAIL = false; 103 | uint8_t FS_WS_count = 0; 104 | 105 | // HAL 106 | figure body = { 107 | { 0, 0, 0}, 108 | { 0, 0, 0} 109 | }; 110 | 111 | IK ikLegLF(legs[LEGLF], body); 112 | IK ikLegRF(legs[LEGRF], body); 113 | IK ikLegLH(legs[LEGLH], body); 114 | IK ikLegRH(legs[LEGRH], body); 115 | 116 | /* We need predict future position of legs/body */ 117 | planner walkPlanner( 118 | vector, 119 | body, 120 | legs[LEGLF], 121 | legs[LEGRF], 122 | legs[LEGLH], 123 | legs[LEGRH] 124 | ); 125 | 126 | /* and balance it someway */ 127 | balance bodyBalance( 128 | balanceOffset, 129 | body, 130 | legs[LEGLF], 131 | legs[LEGRF], 132 | legs[LEGLH], 133 | legs[LEGRH] 134 | ); 135 | 136 | HAL_body bodyUpdate(vector, body, legs); 137 | 138 | // WebServer 139 | bool clientOnline = false; 140 | int WiFiMode = AP_MODE; 141 | IPAddress WiFiIP; 142 | AsyncWebServer server(80); 143 | AsyncWebSocket ws("/ws"); 144 | uint8_t telemetryPackage[P_TELEMETRY_LEN]; 145 | 146 | // CLI 147 | Stream *cliSerial; 148 | 149 | // Subscriptions 150 | bool subscriptionEnabled = false; 151 | bool subscriptionBinary = false; 152 | 153 | bool mainLoopReady = false; 154 | bool serviceLoopReady = false; 155 | 156 | 157 | void setup() 158 | { 159 | Serial.begin(SERIAL_BAUD); 160 | delay(100); 161 | 162 | initSettings(); 163 | delay(100); 164 | 165 | initHAL(); 166 | delay(100); 167 | 168 | initGait(); 169 | delay(100); 170 | 171 | xTaskCreatePinnedToCore( 172 | servicesLoop, /* Task function. */ 173 | "Services", /* name of task. */ 174 | 100000, /* Stack size of task */ 175 | NULL, /* parameter of the task */ 176 | 1, /* priority of the task */ 177 | &ServicesTask, /* Task handle to keep track of created task */ 178 | 0); /* pin task to core 0 */ 179 | 180 | mainLoopReady = true; 181 | } 182 | 183 | /** 184 | Main loop for all major things 185 | Core 1 186 | */ 187 | void loop() 188 | { 189 | currentTime = micros(); 190 | if (mainLoopReady && serviceLoopReady && currentTime - previousTime >= LOOP_TIME) { 191 | previousTime = currentTime; 192 | 193 | updateFailsafe(); 194 | updateGait(); 195 | updateHAL(); 196 | doHAL(); 197 | 198 | FS_WS_count++; 199 | 200 | loopTime = micros() - currentTime; 201 | if (loopTime > LOOP_TIME) { 202 | Serial.print("WARNING! Increase LOOP_TIME: "); 203 | Serial.println(loopTime); 204 | } 205 | } 206 | } 207 | 208 | /** 209 | Loop for service things, like CLI 210 | Core 0 211 | */ 212 | void servicesSetup() { 213 | cliSerial = &Serial; 214 | initCLI(); 215 | initSubscription(); 216 | Wire.begin(); 217 | Wire.setClock(400000); 218 | delay(100); 219 | 220 | initIMU(); 221 | delay(100); 222 | 223 | initPowerSensor(); 224 | delay(100); 225 | 226 | initWiFi(); 227 | delay(100); 228 | 229 | initWebServer(); 230 | delay(100); 231 | 232 | serviceLoopReady = true; 233 | } 234 | 235 | void servicesLoop(void * pvParameters) { 236 | servicesSetup(); 237 | 238 | while(mainLoopReady && serviceLoopReady) { 239 | serviceCurrentTime = micros(); 240 | 241 | if (serviceCurrentTime - serviceFastPreviousTime >= SERVICE_FAST_LOOP_TIME) { 242 | serviceFastPreviousTime = serviceCurrentTime; 243 | 244 | updateIMU(); 245 | 246 | runFASTCommand(); 247 | doFASTSubscription(); 248 | 249 | serviceFastLoopTime = micros() - serviceCurrentTime; 250 | if (serviceFastLoopTime > LOOP_TIME) { 251 | Serial.print("WARNING! Increase SERVICE_FAST_LOOP_TIME: "); 252 | Serial.println(serviceFastLoopTime); 253 | } 254 | } 255 | 256 | if (serviceCurrentTime - servicePreviousTime >= SERVICE_LOOP_TIME) { 257 | servicePreviousTime = serviceCurrentTime; 258 | 259 | updateWiFi(); 260 | updatePower(); 261 | runSLOWCommand(); 262 | updateCLI(); 263 | doSLOWSubscription(); 264 | 265 | serviceLoopTime = micros() - serviceCurrentTime; // this loop + service fast loop 266 | if (serviceLoopTime > LOOP_TIME) { 267 | Serial.print("WARNING! Increase SERVICE_LOOP_TIME: "); 268 | Serial.println(serviceLoopTime); 269 | } 270 | 271 | } 272 | vTaskDelay(1); // https://github.com/espressif/arduino-esp32/issues/595 273 | } 274 | } 275 | 276 | void runFASTCommand() 277 | { 278 | if (runCommandFASTCore) { 279 | runCommandFASTCore = false; 280 | cliFunctionFAST(cliFunctionFASTVar); 281 | } 282 | } 283 | 284 | void runSLOWCommand() 285 | { 286 | if (runCommandSLOWCore) { 287 | runCommandSLOWCore = false; 288 | cliFunctionSLOW(cliFunctionSLOWVar); 289 | } 290 | } 291 | 292 | /** 293 | TODO 294 | - calculate center of mass and use it for balance 295 | - make the queue of tasks by core, e.g. not just cliFunctionFAST = runI2CScanFAST; but array/list with commands 296 | - i2c pwm broken, i2c in service loop 297 | 298 | */ 299 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/servo.ino: -------------------------------------------------------------------------------- 1 | float testAngle = 0; 2 | float testAngleInc = 0.5; 3 | bool testAngleWay = true; 4 | 5 | 6 | void initServo() 7 | { 8 | Serial.print("Servo "); 9 | 10 | initServoHAL(); 11 | 12 | Serial.println(); 13 | } 14 | 15 | void setServoToMiddle() 16 | { // remove it 17 | for (uint8_t i = 0; i < LEG_NUM; i++) { 18 | runServoCalibrate(legs[i]); 19 | } 20 | } 21 | 22 | /** 23 | * Set servo to init position 24 | * it is 90, 45, 90 25 | */ 26 | void setServoToInit() 27 | { 28 | for (uint8_t i = 0; i < LEG_NUM; i++) { 29 | legs[i].angle.alpha = M_PI_2; 30 | legs[i].angle.beta = M_PI_4; 31 | legs[i].angle.gamma = M_PI_2; 32 | setLegPWM(legs[i]); 33 | } 34 | } 35 | 36 | 37 | void servoSet() 38 | { 39 | for (uint8_t i = 0; i < LEG_NUM; i++) { 40 | setLegPWM(legs[i]); 41 | } 42 | } 43 | 44 | bool setAngleDeg(leg &_leg, int angleId, double deg) 45 | { 46 | // TODO limits? 47 | double rad = degToRad(deg); 48 | switch (angleId) { 49 | case ALPHA: 50 | _leg.angle.alpha = rad; 51 | break; 52 | case BETA: 53 | _leg.angle.beta = rad; 54 | break; 55 | case GAMMA: 56 | _leg.angle.gamma = rad; 57 | break; 58 | } 59 | 60 | return true; 61 | } 62 | 63 | double getAngleDeg(leg &_leg, int angleId) 64 | { 65 | switch (angleId) { 66 | case ALPHA: 67 | return radToDeg(_leg.angle.alpha); 68 | break; 69 | case BETA: 70 | return radToDeg(_leg.angle.beta); 71 | break; 72 | case GAMMA: 73 | return radToDeg(_leg.angle.gamma); 74 | break; 75 | } 76 | 77 | return 0.0; 78 | } 79 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/settings.ino: -------------------------------------------------------------------------------- 1 | /** 2 | * Save and restore settings using EEPROM 3 | * 4 | * EEPROM map 5 | * 0: version 6 | * 1: version revision 7 | * 2,3,4: LF alpha, beta, gamma 8 | * 5,6,7: RF alpha, beta, gamma 9 | * 8,9,10: LH alpha, beta, gamma 10 | * 11,12,13: RH alpha, beta, gamma 11 | * 14: WiFi mode (AP = 0 (default), 1...255 number of default WiFi AP to connect) 12 | */ 13 | 14 | #define EEPROM_VERSION 0x01 15 | #define EEPROM_REV 0x01 16 | #define EEPROM_OFFSET 2 // skip first value as it is version number 17 | #define EEPROM_SIZE 32 18 | 19 | // TODO 20 | // - check EEPROM data structure version 21 | // - print setting to Serial 22 | 23 | 24 | void initSettings() { 25 | Serial.print("Settings "); 26 | 27 | EEPROM.begin(EEPROM_SIZE); 28 | 29 | settingsCheck(); 30 | 31 | settingsLoad(); 32 | 33 | Serial.println(); 34 | } 35 | 36 | void settingsCheck() { 37 | if (EEPROM.read(0) == EEPROM_VERSION && EEPROM.read(1) == EEPROM_REV ) return; 38 | // TODO ask?? 39 | settingsInitEEPROM(); 40 | } 41 | 42 | void settingsInitEEPROM() { 43 | // In some cases that can be moving data from one version to another 44 | Serial.println("Re-init EEPROM"); 45 | delay(5000); 46 | EEPROM.write(0, EEPROM_VERSION); 47 | EEPROM.write(1, EEPROM_REV); 48 | for (int i = 2; i < 14; i++) { 49 | EEPROM.write(i,128); // Zeroes for trim 50 | } 51 | 52 | settingsSaveWiFi(AP_MODE); // Set AP mode 53 | 54 | for (int i = 15; i < EEPROM_SIZE; i++) { 55 | EEPROM.write(i,0); 56 | } 57 | settingsCommit(); 58 | } 59 | 60 | void settingsCommit() { 61 | // TODO confirm save? 62 | EEPROM.commit(); 63 | } 64 | 65 | double settingsUint8ToDouble(uint8_t value) { 66 | // This is for servo trimming, so the value of uint8_t is 0 to 255 67 | // can be converted to -0.25...+0.25 rad that should be enough for 68 | // servo trimming, in other cases you are doing hardware wrong 69 | // (256 values with sign: 0 to 255 -> -127...126 -> /500 and rad->deg -> -14.6...14.5 deg) 70 | return ((double)value-128)/500; 71 | } 72 | 73 | uint8_t settingsDoubleToUint8(double value) { 74 | // see settingsUint8ToDouble(); 75 | if (value >= LEG_TRIM_LIMIT) value = LEG_TRIM_LIMIT; 76 | if (value <= -LEG_TRIM_LIMIT) value = -LEG_TRIM_LIMIT; 77 | 78 | return value*500+128; 79 | } 80 | 81 | 82 | void settingsLoad() { 83 | settingsLoadTrim(); 84 | WiFiMode = settingsLoadWiFi(); 85 | } 86 | 87 | // leg trim values 88 | void settingsLoadTrim() { 89 | //0 leg trim {-0.05, -0.15, 0.20} 90 | //1 leg trim { 0.12, -0.10, -0.18} 91 | //2 leg trim {-0.02, -0.21, -0.06} 92 | //3 leg trim { 0.04, -0.12, -0.06} 93 | 94 | Serial.println(); 95 | for (int i = 0; i < LEG_NUM; i++) { 96 | legs[i].hal.trim = settingsLoadTrimLeg(legs[i]); 97 | Serial.print(i); 98 | Serial.print(" leg trim {"); 99 | Serial.print(radToDeg(legs[i].hal.trim.alpha), CLI_DP); 100 | Serial.print(", "); 101 | Serial.print(radToDeg(legs[i].hal.trim.beta), CLI_DP); 102 | Serial.print(", "); 103 | Serial.print(radToDeg(legs[i].hal.trim.gamma), CLI_DP); 104 | Serial.println("}"); 105 | } 106 | } 107 | 108 | legangle settingsLoadTrimLeg(leg &_leg){ 109 | int offset = _leg.id.id*3+EEPROM_OFFSET; 110 | return { 111 | settingsUint8ToDouble(EEPROM.read(offset)), 112 | settingsUint8ToDouble(EEPROM.read(offset+1)), 113 | settingsUint8ToDouble(EEPROM.read(offset+2)) 114 | }; 115 | } 116 | 117 | void settingsSaveTrimLeg(leg &_leg) { 118 | int offset = _leg.id.id*3+EEPROM_OFFSET; 119 | EEPROM.write(offset, settingsDoubleToUint8(_leg.hal.trim.alpha)); 120 | EEPROM.write(offset+1, settingsDoubleToUint8(_leg.hal.trim.beta)); 121 | EEPROM.write(offset+2, settingsDoubleToUint8(_leg.hal.trim.gamma)); 122 | settingsCommit(); 123 | } 124 | 125 | // WiFi 126 | int settingsLoadWiFi() { 127 | return (int)EEPROM.read(14); 128 | } 129 | 130 | int settingsSaveWiFi(int id) { 131 | EEPROM.write(14, id); 132 | settingsCommit(); 133 | return id; 134 | } 135 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/subscription.h: -------------------------------------------------------------------------------- 1 | 2 | const typedef struct subscription_t { 3 | mutable bool fast; 4 | mutable bool active; 5 | } subscription; 6 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/subscription.ino: -------------------------------------------------------------------------------- 1 | // TODO binarySubscription 2 | 3 | subscription subscriptions[CLI_MENU_COMMANDS_GET] = {}; 4 | 5 | void initSubscription() 6 | { 7 | // Copy all get methods to use is for subscriptions 8 | for (int i = 0; i < CLI_MENU_COMMANDS_GET; i++) { 9 | subscriptions[i].active = false; 10 | subscriptions[i].fast = false; 11 | } 12 | } 13 | 14 | double subscriptionState(double state) 15 | { 16 | subscriptionEnabled = state > 0; 17 | 18 | Serial.println(subscriptionEnabled); 19 | 20 | return subscriptionEnabled ? 1.0 : 0.0; 21 | } 22 | 23 | void subscribe(int commandId, bool subscribe, bool fast) 24 | { 25 | Serial.println(commandId); 26 | Serial.println(subscribe); 27 | Serial.println(fast); 28 | subscriptions[commandId].active = subscribe; 29 | subscriptions[commandId].fast = fast; 30 | } 31 | 32 | void doFASTSubscription() 33 | { 34 | doSubscription(true); 35 | } 36 | 37 | void doSLOWSubscription() 38 | { 39 | doSubscription(false); 40 | } 41 | 42 | void doSubscription(bool fast) 43 | { 44 | if (!subscriptionEnabled) return; 45 | 46 | for (int i = 0; i < CLI_MENU_COMMANDS_GET; i++) { 47 | if (subscriptions[i].active && subscriptions[i].fast) { 48 | double r = cliCommandsGet[i].func(0.0); 49 | cliSerial->print(cliCommandsGet[i].commandName); 50 | cliSerial->print(CLI_DELIMITER); 51 | cliSerial->println(r, CLI_DP); 52 | } 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/testHAL.ino: -------------------------------------------------------------------------------- 1 | double displayHALMovement = 1; // mm 2 | double displayHALRotation = M_PI/180; 3 | 4 | // tests 5 | double HALTEST1_dx = 0; 6 | double HALTEST1_dy = 0; 7 | double HALTEST1_dz = 0; 8 | double HALTEST1_d = 1; 9 | int HALTEST1_stage = 0; 10 | double HALTEST1_delta = 49.9; // +-50 mm (square 100x100) 11 | 12 | void testHAL(int test) { 13 | switch (HALTEST1_stage) { 14 | case 0: 15 | HALTEST1_dy = HALTEST1_dy + HALTEST1_d; 16 | if (HALTEST1_dy >= HALTEST1_delta) HALTEST1_stage++; 17 | break; 18 | case 1: 19 | HALTEST1_dx = HALTEST1_dx + HALTEST1_d; 20 | if (HALTEST1_dx >= HALTEST1_delta) HALTEST1_stage++; 21 | break; 22 | case 2: 23 | HALTEST1_dy = HALTEST1_dy - HALTEST1_d; 24 | if (HALTEST1_dy <= -HALTEST1_delta) HALTEST1_stage++; 25 | break; 26 | case 3: 27 | HALTEST1_dx = HALTEST1_dx - HALTEST1_d; 28 | if (HALTEST1_dx <= -HALTEST1_delta) HALTEST1_stage=0; 29 | break; 30 | } 31 | 32 | testHAL_LEGLF(legs[LEGLF]); 33 | //testHAL_LEGRF(legs[LEGRF]); 34 | //testHAL_LEGLH(legs[LEGLH]); 35 | //testHAL_LEGRH(legs[LEGRH]); 36 | } 37 | 38 | void testHAL_LEGLF(leg &_leg) { 39 | _leg.foot.x = -LEG_POINT_X + HALTEST1_dx; 40 | _leg.foot.y = LEG_POINT_Y_F + HALTEST1_dy; 41 | _leg.foot.z = -LEG_POINT_Z + HALTEST1_dz; 42 | } 43 | 44 | void testHAL_LEGRF(leg &_leg) { 45 | _leg.foot.x = LEG_POINT_X + HALTEST1_dx; 46 | _leg.foot.y = LEG_POINT_Y_F + HALTEST1_dy; 47 | _leg.foot.z = -LEG_POINT_Z + HALTEST1_dz; 48 | } 49 | 50 | 51 | void testHAL_LEGLH(leg &_leg) { 52 | _leg.foot.x = -LEG_POINT_X + HALTEST1_dx; 53 | _leg.foot.y = -LEG_POINT_Y_H + HALTEST1_dy; 54 | _leg.foot.z = -LEG_POINT_Z + HALTEST1_dz; 55 | } 56 | 57 | void testHAL_LEGRH(leg &_leg) { 58 | _leg.foot.x = LEG_POINT_X + HALTEST1_dx; 59 | _leg.foot.y = -LEG_POINT_Y_H + HALTEST1_dy; 60 | _leg.foot.z = -LEG_POINT_Z + HALTEST1_dz; 61 | } 62 | -------------------------------------------------------------------------------- /software/robot_dog_esp32/web/index.html.gz.h: -------------------------------------------------------------------------------- 1 | #define index_html_gz_len 2535 2 | static const uint8_t index_html_gz[] PROGMEM = {31,139,8,0,0,0,0,0,0,3,173,25,107,147,211,70,242,179,247,87,76,156,226,36,131,45,219,11,75,182,252,216,59,72,8,225,14,174,82,44,33,151,162,40,106,44,141,237,97,37,141,78,15,219,194,248,191,95,247,188,36,249,65,204,17,23,172,71,253,238,158,238,158,30,121,178,204,163,240,102,178,100,52,184,153,228,60,15,217,205,107,49,19,249,79,98,241,118,56,233,43,200,36,203,75,248,154,137,160,220,246,214,108,118,199,243,94,46,10,127,217,243,105,24,138,34,31,197,34,102,99,131,42,50,150,246,50,22,50,223,32,238,80,203,17,120,36,62,29,131,102,135,192,3,128,88,177,52,243,83,17,134,189,25,91,210,21,23,169,66,36,52,8,120,188,24,13,198,17,77,23,60,134,197,146,241,197,50,31,13,7,131,123,227,53,15,242,165,92,238,60,95,196,57,229,49,75,183,117,138,128,103,73,72,203,209,60,100,155,49,254,233,5,60,5,189,92,196,35,95,132,69,20,239,190,207,114,154,23,217,118,14,2,122,115,26,241,176,28,69,34,22,89,66,125,182,251,30,229,130,101,91,100,30,13,119,255,136,88,192,41,1,107,25,139,9,141,3,226,138,148,51,208,45,101,38,34,205,83,202,243,206,214,251,40,202,44,231,254,221,135,69,42,138,100,219,176,164,110,226,113,171,254,76,81,8,144,204,167,9,59,95,211,110,87,81,86,209,146,126,93,129,29,117,74,75,184,173,66,220,48,122,70,253,59,212,22,7,61,30,209,5,27,21,105,232,182,3,154,211,145,124,238,103,171,197,131,77,20,118,239,61,252,17,150,4,150,113,54,117,150,121,158,140,250,253,245,122,237,173,31,122,34,93,244,47,7,131,1,18,59,68,137,159,58,32,223,33,82,173,90,223,123,248,12,132,248,60,245,67,70,252,205,212,185,2,188,95,170,239,116,234,60,186,114,72,6,91,116,199,166,206,44,4,187,204,99,79,11,121,232,144,57,15,195,169,19,162,130,5,198,211,33,125,37,22,85,195,170,221,169,123,148,178,132,81,76,77,189,170,227,18,145,113,185,1,16,50,130,97,59,153,187,243,80,128,144,144,205,243,177,128,84,226,121,57,242,134,99,200,142,88,75,208,80,226,93,101,85,192,61,10,105,176,98,91,195,50,220,77,250,170,98,39,17,203,41,137,105,196,166,237,21,103,107,76,181,54,193,141,132,164,152,182,149,179,1,91,113,95,123,222,229,49,40,162,97,15,242,36,100,211,97,55,162,27,30,21,145,125,86,117,8,15,116,6,207,177,104,223,76,250,170,121,96,119,184,153,4,124,69,252,144,102,217,180,109,243,165,173,192,60,152,182,85,221,180,111,110,229,55,9,1,61,233,3,178,162,136,68,192,128,96,194,227,164,200,73,94,38,96,186,191,100,254,221,76,108,218,146,2,21,125,72,5,72,96,64,7,134,176,144,204,69,186,135,120,45,191,73,206,178,124,210,151,68,55,123,154,116,149,182,27,70,55,235,226,56,78,153,129,187,244,207,118,93,232,81,178,20,19,168,162,59,252,11,137,192,147,156,100,169,15,38,121,31,209,247,190,130,25,220,205,188,136,101,161,147,231,46,15,58,100,123,209,74,89,94,164,49,9,132,95,68,176,149,222,130,229,207,66,134,203,167,229,139,0,169,198,23,187,139,139,21,77,201,10,154,132,72,201,20,217,34,200,188,17,46,90,155,17,25,120,131,46,172,74,187,250,100,86,59,248,175,226,168,136,19,158,251,75,75,134,121,59,34,150,157,174,225,161,98,220,141,149,218,156,161,61,121,90,42,205,106,227,145,114,139,210,87,34,204,177,5,24,70,191,72,83,48,222,62,135,66,36,111,120,4,4,131,74,228,162,224,100,10,178,138,4,154,6,123,1,73,156,174,40,152,18,23,97,8,60,98,246,81,153,107,116,105,120,171,150,23,22,8,70,92,180,48,217,71,196,70,215,149,161,109,217,160,194,65,242,108,5,139,151,60,131,122,97,169,11,53,148,241,25,15,161,200,252,37,141,23,172,221,69,163,60,17,191,181,136,31,37,2,194,223,106,73,212,236,163,167,236,129,56,60,119,29,181,118,26,248,154,125,138,168,6,64,74,77,218,116,27,40,51,150,155,39,183,34,232,18,232,128,40,95,186,168,96,135,78,86,244,170,20,93,203,113,232,204,33,55,159,19,215,134,169,10,202,173,114,97,74,218,75,30,4,44,110,43,234,86,191,79,94,204,201,44,21,107,232,30,80,38,34,99,1,194,231,148,135,25,157,51,15,60,249,249,86,154,208,218,105,51,244,137,244,239,34,154,177,180,110,65,92,153,16,163,174,129,86,162,75,162,13,9,52,24,180,149,168,11,11,6,218,7,128,147,159,118,199,203,66,104,121,238,160,75,174,246,34,117,171,115,167,230,241,246,96,43,61,30,67,58,252,242,230,213,75,216,5,68,53,140,117,109,234,123,58,205,59,160,219,121,75,62,19,7,22,95,162,215,101,32,233,159,0,253,75,91,6,200,89,209,153,242,24,219,146,11,89,78,214,153,170,181,181,172,179,198,71,103,125,85,133,246,51,167,97,198,0,197,210,20,78,33,114,12,181,95,111,149,60,107,209,65,49,158,42,46,56,144,105,106,179,118,157,237,101,181,76,1,108,26,114,75,1,45,125,138,217,154,252,206,102,183,194,191,99,185,235,123,107,73,166,208,80,124,34,129,105,103,106,85,105,77,18,111,75,47,79,11,38,153,118,13,86,233,245,159,241,202,56,28,97,142,88,150,193,238,214,216,137,203,86,114,255,148,16,249,224,225,132,227,209,52,165,229,211,98,62,135,253,238,120,249,146,197,238,76,62,145,233,13,217,130,184,132,166,25,147,237,198,69,119,127,227,113,126,253,4,153,52,93,167,179,235,24,27,180,17,123,13,1,62,205,158,96,73,32,205,7,54,100,7,123,118,200,101,73,94,179,255,22,112,128,202,150,162,36,236,136,79,225,48,112,153,246,241,156,253,60,17,76,128,154,240,219,221,129,51,57,19,33,131,4,95,184,170,137,238,78,117,50,23,227,90,245,2,171,163,83,203,29,104,44,113,224,194,88,4,121,227,225,225,231,118,26,77,166,10,122,61,83,103,60,166,105,249,147,21,159,173,241,252,171,195,223,13,222,27,255,105,198,200,245,163,145,73,154,90,232,178,4,156,97,117,105,210,197,214,44,101,244,174,110,197,126,184,235,182,124,173,147,86,214,158,167,7,118,125,201,225,125,89,191,98,152,246,28,217,217,182,99,186,184,62,232,177,149,31,214,189,154,66,228,30,120,27,221,95,160,119,143,247,80,229,105,212,167,163,40,117,74,122,56,147,156,64,201,225,229,56,10,102,23,43,80,249,35,29,82,238,43,119,142,119,49,29,160,228,21,24,166,251,211,147,90,129,15,31,201,216,107,170,85,141,170,86,214,117,25,250,16,106,125,136,69,26,13,235,250,160,134,10,93,107,230,36,147,160,7,195,206,125,44,202,129,225,44,64,242,240,113,131,21,6,254,46,244,227,168,75,196,124,14,251,162,119,2,192,239,20,224,189,244,222,5,146,155,155,235,206,223,46,175,174,198,77,130,7,195,247,104,120,17,105,156,212,4,179,230,111,7,202,48,79,155,122,180,185,178,196,160,29,72,10,163,119,50,185,134,211,173,6,1,69,38,6,106,70,61,17,113,25,75,168,62,176,234,135,31,246,99,252,78,90,59,172,129,117,84,220,58,85,151,24,164,12,182,219,200,204,78,215,28,126,151,157,111,144,83,86,114,30,125,139,156,79,149,156,199,255,167,156,122,21,160,180,235,111,147,131,133,134,98,134,131,111,147,3,165,39,125,27,94,170,1,215,102,75,85,20,58,31,108,23,58,39,41,174,31,157,76,138,243,148,200,86,119,216,27,223,24,188,82,124,48,225,97,29,25,119,109,125,236,243,118,33,25,250,67,85,179,173,131,161,239,76,17,143,143,137,48,243,224,121,34,174,117,255,190,184,144,55,86,34,226,91,249,198,232,57,141,88,66,3,244,16,207,97,56,148,113,199,92,152,123,187,36,128,59,254,140,198,65,151,224,43,63,124,185,161,35,177,228,25,78,198,160,25,233,12,196,144,3,216,46,63,127,198,203,221,149,165,49,130,128,198,44,209,43,131,230,153,60,152,235,35,131,132,215,239,179,173,250,69,182,126,149,85,99,131,25,67,77,190,24,99,15,111,118,78,36,10,184,148,136,117,236,96,86,154,49,14,102,51,201,35,31,225,106,144,230,26,211,25,127,89,156,124,59,154,33,3,202,59,79,220,25,6,22,137,163,26,194,81,137,63,131,175,217,242,76,11,165,60,81,228,74,224,95,36,15,134,193,21,59,233,241,87,201,147,17,132,193,230,175,178,79,202,243,105,236,179,16,68,158,41,239,12,151,177,79,159,206,25,108,49,174,186,71,145,175,176,243,12,161,56,50,215,101,202,100,175,37,86,45,227,171,74,50,115,182,213,47,27,0,42,71,75,92,71,189,72,116,154,242,116,88,142,10,60,86,154,48,223,77,177,14,247,193,229,30,88,95,130,241,22,62,232,156,176,42,149,241,109,24,214,218,239,31,110,77,73,211,114,25,40,158,189,193,144,154,88,73,47,112,202,195,169,249,192,13,68,160,249,174,97,35,127,87,124,30,4,21,90,170,132,177,12,14,27,48,18,223,172,255,135,140,52,129,126,238,144,94,131,195,83,3,206,75,54,7,221,125,98,125,84,224,223,241,165,235,253,203,222,208,40,47,191,70,249,31,123,202,255,56,161,252,141,72,142,232,254,69,190,60,55,202,241,122,241,138,230,75,143,206,50,119,211,33,147,41,105,116,242,142,140,203,224,128,180,60,70,90,214,73,55,228,134,12,21,251,176,130,77,72,79,1,43,245,165,34,44,235,132,165,38,44,235,132,223,213,179,80,95,133,172,117,45,171,124,167,201,155,185,249,221,116,10,106,225,48,106,230,38,130,245,233,222,170,167,134,41,153,221,145,36,39,155,99,57,222,43,117,150,162,110,43,74,139,110,100,254,166,11,42,199,22,124,42,161,229,37,14,147,186,193,215,60,209,228,251,126,175,250,209,225,87,253,155,3,166,211,6,46,11,253,75,188,46,224,123,165,123,242,93,146,91,54,129,78,227,74,167,95,141,87,87,32,183,170,27,249,222,155,232,251,204,222,232,224,62,119,29,137,119,112,184,147,167,189,10,179,4,218,201,193,36,187,122,55,126,90,148,194,163,172,154,40,5,220,147,5,210,61,101,39,62,43,146,10,128,19,94,195,2,119,213,184,149,214,174,131,83,178,242,54,154,165,169,201,240,244,239,223,135,191,228,62,121,42,130,146,72,86,136,179,50,142,192,191,156,69,137,72,97,226,34,185,32,140,102,37,129,11,27,199,119,23,228,197,191,36,103,95,231,198,145,247,191,158,252,169,131,5,58,91,142,95,114,87,158,204,176,19,247,92,101,191,76,64,61,240,154,244,109,222,193,45,101,243,254,109,196,171,249,80,229,129,141,36,90,108,214,122,208,52,143,235,204,44,171,159,47,250,234,55,161,190,252,141,249,127,179,210,139,222,106,30,0,0}; -------------------------------------------------------------------------------- /software/robot_dog_esp32/webServer.ino: -------------------------------------------------------------------------------- 1 | void initWebServer() { 2 | Serial.print("WebServer "); 3 | initWebServerRoutes(); 4 | Serial.println(); 5 | } 6 | 7 | void initWebServerRoutes() { 8 | server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){ 9 | AsyncWebServerResponse *response = request->beginResponse_P(200, "text/html", index_html_gz, index_html_gz_len); 10 | response->addHeader("Content-Encoding", "gzip"); 11 | request->send(response); 12 | }); 13 | 14 | // Dinamic config 15 | server.on("/c.js", HTTP_GET, [](AsyncWebServerRequest *request){ 16 | request->send(200, "application/x-javascript", "var c={w:'ws://" + WiFiIP.toString() + "/ws'}"); 17 | }); 18 | 19 | ws.onEvent(onWsEvent); 20 | server.addHandler(&ws); 21 | 22 | server.begin(); 23 | } 24 | 25 | void onWsEvent(AsyncWebSocket * server, AsyncWebSocketClient * client, AwsEventType type, void * arg, uint8_t *data, size_t len){ 26 | 27 | if(type == WS_EVT_CONNECT){ 28 | client->text("Ok"); 29 | clientOnline = true; 30 | } else if (clientOnline && type == WS_EVT_DATA) { 31 | FS_WS_count = 0; // zero FS counter 32 | 33 | switch(data[0]) { 34 | case P_MOVE: 35 | pMove(data); 36 | break; 37 | case P_TELEMETRY: 38 | pTelemetry(); 39 | client->binary(telemetryPackage, P_TELEMETRY_LEN); 40 | break; 41 | default: 42 | Serial.print("UNKNOWN PACKAGE ID: "); 43 | Serial.println(data[0], DEC); 44 | } 45 | 46 | } else if(type == WS_EVT_DISCONNECT){ 47 | clientOnline = false; 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /software/web/README.md: -------------------------------------------------------------------------------- 1 | # Robot dog web interface 2 | 3 | ## Build 4 | - `npm install` - install node.js packages 5 | - `npm run build` - build web interface 6 | -------------------------------------------------------------------------------- /software/web/gulpfile.js: -------------------------------------------------------------------------------- 1 | // Based on sidoh/esp8266_milight_hub 2 | 3 | const fs = require('fs'); 4 | const gulp = require('gulp'); 5 | const htmlmin = require('gulp-htmlmin'); 6 | const cleancss = require('gulp-clean-css'); 7 | const uglify = require('gulp-uglify'); 8 | const gzip = require('gulp-gzip'); 9 | const del = require('del'); 10 | const inline = require('gulp-inline'); 11 | const inlineImages = require('gulp-css-base64'); 12 | const favicon = require('gulp-base64-favicon'); 13 | 14 | const dataFolder = '../robot_dog_esp32/web/'; 15 | 16 | gulp.task('clean', function() { 17 | del([ dataFolder + '*'], { force: true }); 18 | return true; 19 | }); 20 | 21 | gulp.task('buildfs_embeded', ['buildfs_inline'], function() { 22 | 23 | var source = dataFolder + 'index.html.gz'; 24 | var destination = dataFolder + 'index.html.gz.h'; 25 | 26 | var wstream = fs.createWriteStream(destination); 27 | wstream.on('error', function (err) { 28 | console.log('WRITE ERROR', err); 29 | }); 30 | 31 | var data = fs.readFileSync(source); 32 | 33 | wstream.write('#define index_html_gz_len ' + data.length + '\n'); 34 | wstream.write('static const uint8_t index_html_gz[] PROGMEM = {') 35 | 36 | for (i=0; i 2 | 3 | RobotDogV1 4 | 62 | 63 | 64 | 65 |
66 |
67 | Status line 68 |
69 |
70 | 71 |
72 |
73 |
74 |
75 |
76 |
77 |
78 |
79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /software/web/src/s.js: -------------------------------------------------------------------------------- 1 | function G(id) { 2 | return document.getElementById(id); 3 | } 4 | 5 | var vector = { 6 | move: { 7 | x: 0.0, 8 | y: 0.0, 9 | z: 0.0, 10 | }, 11 | rotate: { 12 | pitch: 0.0, 13 | roll: 0.0, 14 | yaw: 0.0, 15 | }, 16 | }; 17 | 18 | var telemetry = { 19 | status: {}, 20 | voltage: 0.0, 21 | current: 0.0, 22 | loopTime: 0, 23 | }; 24 | 25 | var gui ={ 26 | updateInterval: null, 27 | obj: { 28 | status: null, 29 | body_rotate: null, 30 | }, 31 | 32 | init: function () { 33 | document.addEventListener("visibilitychange", gui.onVisibilityChange); 34 | gui.obj.status = G('status'); 35 | gui.obj.body_rotate = G('body_rotate'); 36 | 37 | gui.updateInterval = setInterval(gui.update, 100); 38 | }, 39 | 40 | update: function () { 41 | gui.updateStatus(); 42 | }, 43 | 44 | onVisibilityChange: function () { 45 | if (document.visibilityState == "hidden") { 46 | // If browser closed 47 | failsafe.setFS(); 48 | } 49 | }, 50 | 51 | displayNumber: function (n) { 52 | if (n == 0) { 53 | return "0.000"; 54 | } 55 | 56 | return (n + "000000").slice(0, 5); 57 | }, 58 | 59 | updateStatus: function (){ 60 | gui.obj.status.innerHTML = gui.displayNumber(telemetry.voltage) + 'V | ' + gui.displayNumber(telemetry.current) + 'A | LoopTime: ' + telemetry.loopTime; 61 | }, 62 | }; 63 | 64 | let ws = { 65 | ws: null, 66 | status: false, 67 | error: false, 68 | updateInterval: null, 69 | telemetryInterval: null, 70 | 71 | init: function () { 72 | clearInterval(ws.updateInterval); 73 | try { 74 | ws.ws = new WebSocket(c.w); 75 | ws.ws.onopen = function() { 76 | ws.status = true; 77 | }; 78 | ws.ws.onerror = function() { 79 | ws.status = false; 80 | }; 81 | ws.ws.onmessage = function (event) { 82 | event.data.arrayBuffer().then(buffer => {ws.parseEvent(new Uint8Array(buffer))}); 83 | }; 84 | 85 | ws.updateInterval = setInterval(ws.update, 50); 86 | ws.telemetryInterval = setInterval(ws.telemetryRequest, 1000); 87 | } catch(e) { 88 | clearInterval(ws.updateInterval); 89 | ws.status = false; 90 | ws.error = true; 91 | console.log(e); 92 | }; 93 | }, 94 | 95 | update: function(data) { 96 | if (ws.status) { 97 | ws.ws.send(packet.move()); 98 | } 99 | }, 100 | 101 | parseEvent: function (binaryData) { 102 | switch (binaryData[0]) { 103 | case 84: 104 | ws.telemetryResponse(binaryData); 105 | break; 106 | } 107 | }, 108 | 109 | telemetryRequest: function (data) { 110 | if (ws.status) { 111 | ws.ws.send(packet.telemetry()); 112 | } 113 | }, 114 | 115 | telemetryResponse: function (binaryData) { 116 | packet.telemetryParse(binaryData); 117 | } 118 | }; 119 | 120 | let failsafe = { 121 | setFS: function () { 122 | vector.move.x = 0; 123 | vector.move.y = 0; 124 | vector.move.z = 0; 125 | vector.rotate.roll = 0; 126 | vector.rotate.pitch = 0; 127 | vector.rotate.yaw = 0; 128 | } 129 | }; 130 | 131 | 132 | let packet = { 133 | init: function () { 134 | packet.pMove = new ArrayBuffer(14); 135 | packet.vMove = new Uint8Array(packet.pMove); 136 | }, 137 | _norm1: function (value) { 138 | return (value+1)*10000; 139 | }, 140 | _uint16: function (view, num, offset) { 141 | view[offset] = (num>>8)&255; 142 | view[offset+1] = num&255; 143 | }, 144 | _getUint16: function (data, offset) { 145 | return parseInt(data[offset]<<8 | data[offset+1]); 146 | }, 147 | move: function () { 148 | packet.vMove[0] = 77; 149 | packet.vMove[1] = 1; 150 | packet._uint16(packet.vMove, packet._norm1(vector.move.x), 2); 151 | packet._uint16(packet.vMove, packet._norm1(vector.move.y), 4); 152 | packet._uint16(packet.vMove, packet._norm1(vector.move.z), 6); 153 | packet._uint16(packet.vMove, packet._norm1(vector.rotate.pitch), 8); 154 | packet._uint16(packet.vMove, packet._norm1(vector.rotate.roll), 10); 155 | packet._uint16(packet.vMove, packet._norm1(vector.rotate.yaw), 12); 156 | 157 | return packet.pMove; 158 | }, 159 | telemetry: function () { 160 | packet.vMove[0] = 84; 161 | packet.vMove[1] = 1; 162 | 163 | return packet.pMove; 164 | }, 165 | telemetryParse: function (binaryTelemetry) { 166 | telemetry.voltage = packet._getUint16(binaryTelemetry, 4)/1000; 167 | telemetry.current = packet._getUint16(binaryTelemetry, 6)/1000; 168 | telemetry.loopTime = packet._getUint16(binaryTelemetry, 8); 169 | } 170 | } 171 | 172 | class onScreenGamepad { 173 | constructor(obj, deadband, callback) { 174 | this.obj = obj, 175 | this.deadband = deadband || 0.05, 176 | this.callback = callback; 177 | 178 | this.isEvent = false; 179 | this.vector = { 180 | x: 0.0, 181 | y: 0.0, 182 | }; 183 | } 184 | 185 | init () { 186 | this.obj.addEventListener('mousedown', (event) => this.eventStart(event)); 187 | this.obj.addEventListener('touchstart', (event) => this.eventStart(event)); 188 | 189 | this.obj.addEventListener('mouseup', (event) => this.eventFinish(event)); 190 | this.obj.addEventListener('mouseout', (event) => this.eventFinish(event)); 191 | this.obj.addEventListener('mouseleave', (event) => this.eventFinish(event)); 192 | this.obj.addEventListener('touchend', (event) => this.eventFinish(event)); 193 | this.obj.addEventListener('touchcancel', (event) => this.eventFinish(event)); 194 | 195 | this.obj.addEventListener('mousemove', (event) => this.eventMove(false, event)); 196 | this.obj.addEventListener('touchmove', (event) => this.eventMove(true, event)); 197 | } 198 | 199 | eventStart() { 200 | this.isEvent = true; 201 | this.obj.classList.add('active'); 202 | } 203 | 204 | eventFinish() { 205 | this.isEvent = false; 206 | this.vector.x = 0.0; 207 | this.vector.y = 0.0; 208 | this.display(0, 0); 209 | this.obj.classList.remove('active'); 210 | 211 | this.callback(this.vector); 212 | } 213 | 214 | eventMove(isTouch, event) { 215 | let sendEvent = false; 216 | let x = ((isTouch ? event.targetTouches[0].clientX : event.clientX) - event.target.offsetLeft) / this.obj.offsetWidth*2-1; 217 | let y = ((isTouch ? event.targetTouches[0].clientY : event.clientY) - event.target.offsetTop) / this.obj.offsetHeight*2-1; 218 | if (Math.abs(x) <= this.deadband) x = 0; 219 | if (Math.abs(y) <= this.deadband) y = 0; 220 | if (x > 1) x = 1; 221 | if (x < -1) x =-1; 222 | if (y > 1) y = 1; 223 | if (y < -1) y =-1; 224 | if (!this.isEvent) { 225 | x = 0; 226 | y = 0; 227 | } 228 | if (this.vector.x !== x || this.vector.y !== y) { 229 | sendEvent = true; 230 | } 231 | this.vector.x = x; 232 | this.vector.y = -y; 233 | 234 | if (sendEvent) { 235 | this.display(x, y); 236 | this.callback(this.vector); 237 | } 238 | } 239 | 240 | display(x, y) { 241 | this.obj.style.backgroundPosition = (x+1)/2*100 + '% ' + (y+1)/2*100 + '%'; 242 | } 243 | }; 244 | 245 | let control = { 246 | init() { 247 | let leftJ = new onScreenGamepad(G('leftJ'), 0.05, this.leftJcallback); 248 | let rightJ = new onScreenGamepad(G('rightJ'), 0.05, this.rightJcallback); 249 | leftJ.init(); 250 | rightJ.init(); 251 | }, 252 | leftJcallback(v) { 253 | vector.rotate.yaw = v.x; 254 | }, 255 | rightJcallback(v) { 256 | /** 257 | * Body rotation, this is temporary to easy validate IK 258 | */ 259 | if (gui.obj.body_rotate.checked) { 260 | vector.rotate.roll = v.y; 261 | vector.rotate.pitch = v.x; 262 | 263 | return; 264 | } 265 | vector.move.x = v.x; 266 | vector.move.y = v.y; 267 | } 268 | } 269 | 270 | control.init(); 271 | gui.init(); 272 | packet.init(); 273 | ws.init(); 274 | -------------------------------------------------------------------------------- /tools/servoCalib/servoCalib.ino: -------------------------------------------------------------------------------- 1 | 2 | // Select different ESP32 timer number (0-3) to avoid conflict 3 | #define USE_ESP32_TIMER_NO 3 4 | 5 | #include "ESP32_ISR_Servo.h" 6 | 7 | int servoIndex = -1; 8 | String inputString = ""; 9 | bool stringComplete = false; 10 | 11 | void setup() 12 | { 13 | Serial.begin(115200); 14 | while (!Serial); 15 | 16 | delay(200); 17 | 18 | Serial.print(F("\nStarting ESP32_ISR_MultiServos on ")); Serial.println(ARDUINO_BOARD); 19 | Serial.println(ESP32_ISR_SERVO_VERSION); 20 | 21 | //Select ESP32 timer USE_ESP32_TIMER_NO 22 | ESP32_ISR_Servos.useTimer(USE_ESP32_TIMER_NO); 23 | 24 | servoIndex = ESP32_ISR_Servos.setupServo(14, 0, 3000); 25 | 26 | ESP32_ISR_Servos.setPulseWidth(servoIndex, 1500); 27 | 28 | inputString.reserve(200); 29 | } 30 | 31 | void loop() 32 | { 33 | while (Serial.available()) { 34 | char inChar = (char)Serial.read(); 35 | inputString += inChar; 36 | if (inChar == '\n') { 37 | stringComplete = true; 38 | } 39 | } 40 | 41 | 42 | if (stringComplete) { 43 | ESP32_ISR_Servos.setPulseWidth(servoIndex, inputString.toInt()); 44 | Serial.println(inputString.toInt()); 45 | inputString = ""; 46 | stringComplete = false; 47 | } 48 | } 49 | --------------------------------------------------------------------------------