├── BRobotEvo2ESP32 ├── BRobotEvo2ESP32.ino ├── Control.cpp ├── Control.h ├── ESP32SelfbalancingBot.cpp ├── MPU6050.cpp ├── MPU6050.h ├── Motors.cpp ├── Motors.h ├── OSC.cpp ├── OSC.h ├── Timers.cpp ├── defines.h ├── globals.cpp └── globals.h ├── LICENSE ├── README.md └── assets ├── CircuitBottom.png ├── CircuitTop.png └── Robot.png /BRobotEvo2ESP32/BRobotEvo2ESP32.ino: -------------------------------------------------------------------------------- 1 | /* 2 | The purpose of this file is to make BRobotEvo2ESP32 a valid Arduino sketch. 3 | It is intentionally left blank. 4 | See the other files for the project source code. 5 | */ -------------------------------------------------------------------------------- /BRobotEvo2ESP32/Control.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Control.cpp 3 | * 4 | * Created on: 25.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #include 9 | #include "defines.h" 10 | #include "globals.h" 11 | 12 | // PD controller implementation(Proportional, derivative). DT in seconds 13 | float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd) 14 | { 15 | float error; 16 | float output; 17 | 18 | error = setPoint - input; 19 | 20 | // Kd is implemented in two parts 21 | // The biggest one using only the input (sensor) part not the SetPoint input-input(t-1). 22 | // And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1) 23 | float Kd_setPoint = constrain((setPoint - setPointOld), -8, 8); // We limit the input part... 24 | output = Kp * error + (Kd * Kd_setPoint - Kd * (input - PID_errorOld)) / DT; 25 | //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t"); 26 | //PID_errorOld2 = PID_errorOld; 27 | PID_errorOld = input; // error for Kd is only the input component 28 | setPointOld = setPoint; 29 | return (output); 30 | } 31 | 32 | 33 | // PI controller implementation (Proportional, integral). DT in seconds 34 | float speedPIControl(float DT, int16_t input, int16_t setPoint, float Kp, float Ki) 35 | { 36 | int16_t error; 37 | float output; 38 | 39 | error = setPoint - input; 40 | PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR); 41 | PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX); 42 | 43 | //Serial.println(PID_errorSum); 44 | 45 | output = Kp * error + Ki * PID_errorSum * DT; // DT is in miliseconds... 46 | return (output); 47 | } 48 | 49 | 50 | float positionPDControl(long actualPos, long setPointPos, float Kpp, float Kdp, int16_t speedM) 51 | { 52 | float output; 53 | float P; 54 | 55 | P = constrain(Kpp * float(setPointPos - actualPos), -115, 115); // Limit command 56 | output = P + Kdp * float(speedM); 57 | return (output); 58 | } 59 | 60 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/Control.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Control.h 3 | * 4 | * Created on: 25.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #ifndef CONTROL_H_ 9 | #define CONTROL_H_ 10 | 11 | float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd); 12 | float speedPIControl(float DT, int16_t input, int16_t setPoint, float Kp, float Ki); 13 | float positionPDControl(long actualPos, long setPointPos, float Kpp, float Kdp, int16_t speedM); 14 | 15 | 16 | 17 | 18 | #endif /* CONTROL_H_ */ 19 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/ESP32SelfbalancingBot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "MPU6050.h" 7 | #include 8 | #include "esp_types.h" 9 | #include "soc/timer_group_struct.h" 10 | #include "driver/periph_ctrl.h" 11 | #include "driver/timer.h" 12 | #include "driver/ledc.h" 13 | #include "defines.h" 14 | #include "globals.h" 15 | #include "Motors.h" 16 | #include "Control.h" 17 | #include 18 | #include "OSC.h" 19 | #include "esp32-hal-ledc.h" 20 | 21 | void initTimers(); 22 | 23 | void initWifiAP() { 24 | //Serial.println("Setting up WiFi AP..."); 25 | if (WiFi.softAP("bbot", "12345678")) { 26 | Serial.println("Wifi AP set up successfully"); 27 | } 28 | WiFi.softAPConfig(IPAddress(192, 168, 4, 1), IPAddress(192, 168, 4, 1), 29 | IPAddress(255, 255, 255, 0)); 30 | } 31 | 32 | void initMPU6050() { 33 | MPU6050_setup(); 34 | delay(500); 35 | MPU6050_calibrate(); 36 | } 37 | 38 | void setup() { 39 | pinMode(PIN_ENABLE_MOTORS, OUTPUT); 40 | digitalWrite(PIN_ENABLE_MOTORS, HIGH); 41 | 42 | pinMode(PIN_MOTOR1_DIR, OUTPUT); 43 | pinMode(PIN_MOTOR1_STEP, OUTPUT); 44 | pinMode(PIN_MOTOR2_DIR, OUTPUT); 45 | pinMode(PIN_MOTOR2_STEP, OUTPUT); 46 | 47 | pinMode(PIN_SERVO, OUTPUT); 48 | 49 | ledcSetup(6, 50, 16); // channel 6, 50 Hz, 16-bit width 50 | ledcAttachPin(PIN_SERVO, 6); // GPIO 22 assigned to channel 1 51 | delay(50); 52 | ledcWrite(6, SERVO_AUX_NEUTRO); 53 | 54 | Serial.begin(115200); 55 | 56 | Wire.begin(); 57 | 58 | initWifiAP(); 59 | 60 | initMPU6050(); 61 | initTimers(); 62 | 63 | OSC_init(); 64 | 65 | digitalWrite(PIN_ENABLE_MOTORS, LOW); 66 | for (uint8_t k = 0; k < 5; k++) { 67 | setMotorSpeedM1(5); 68 | setMotorSpeedM2(5); 69 | ledcWrite(6, SERVO_AUX_NEUTRO + 250); 70 | delay(200); 71 | setMotorSpeedM1(-5); 72 | setMotorSpeedM2(-5); 73 | ledcWrite(6, SERVO_AUX_NEUTRO - 250); 74 | delay(200); 75 | } 76 | ledcWrite(6, SERVO_AUX_NEUTRO); 77 | 78 | digitalWrite(PIN_ENABLE_MOTORS, HIGH); 79 | } 80 | 81 | void processOSCMsg() { 82 | if (OSCpage == 1) { 83 | if (modifing_control_parameters) // We came from the settings screen 84 | { 85 | OSCfader[0] = 0.5; // default neutral values 86 | OSCfader[1] = 0.5; 87 | OSCtoggle[0] = 0; // Normal mode 88 | mode = 0; 89 | modifing_control_parameters = false; 90 | } 91 | 92 | if (OSCmove_mode) { 93 | //Serial.print("M "); 94 | //Serial.print(OSCmove_speed); 95 | //Serial.print(" "); 96 | //Serial.print(OSCmove_steps1); 97 | //Serial.print(","); 98 | //Serial.println(OSCmove_steps2); 99 | positionControlMode = true; 100 | OSCmove_mode = false; 101 | target_steps1 = steps1 + OSCmove_steps1; 102 | target_steps2 = steps2 + OSCmove_steps2; 103 | } else { 104 | positionControlMode = false; 105 | throttle = (OSCfader[0] - 0.5) * max_throttle; 106 | // We add some exponential on steering to smooth the center band 107 | steering = OSCfader[1] - 0.5; 108 | if (steering > 0) 109 | steering = (steering * steering + 0.5 * steering) 110 | * max_steering; 111 | else 112 | steering = (-steering * steering + 0.5 * steering) 113 | * max_steering; 114 | } 115 | 116 | if ((mode == 0) && (OSCtoggle[0])) { 117 | // Change to PRO mode 118 | max_throttle = MAX_THROTTLE_PRO; 119 | max_steering = MAX_STEERING_PRO; 120 | max_target_angle = MAX_TARGET_ANGLE_PRO; 121 | mode = 1; 122 | } 123 | if ((mode == 1) && (OSCtoggle[0] == 0)) { 124 | // Change to NORMAL mode 125 | max_throttle = MAX_THROTTLE; 126 | max_steering = MAX_STEERING; 127 | max_target_angle = MAX_TARGET_ANGLE; 128 | mode = 0; 129 | } 130 | } else if (OSCpage == 2) { // OSC page 2 131 | if (!modifing_control_parameters) { 132 | for (uint8_t i = 0; i < 4; i++) 133 | OSCfader[i] = 0.5; 134 | OSCtoggle[0] = 0; 135 | 136 | modifing_control_parameters = true; 137 | OSC_MsgSend("$P2", 4); 138 | } 139 | // User could adjust KP, KD, KP_THROTTLE and KI_THROTTLE (fadder1,2,3,4) 140 | // Now we need to adjust all the parameters all the times because we dont know what parameter has been moved 141 | Kp_user = KP * 2 * OSCfader[0]; 142 | Kd_user = KD * 2 * OSCfader[1]; 143 | Kp_thr_user = KP_THROTTLE * 2 * OSCfader[2]; 144 | Ki_thr_user = KI_THROTTLE * 2 * OSCfader[3]; 145 | // Send a special telemetry message with the new parameters 146 | char auxS[50]; 147 | sprintf(auxS, "$tP,%d,%d,%d,%d", int(Kp_user * 1000), 148 | int(Kd_user * 1000), int(Kp_thr_user * 1000), 149 | int(Ki_thr_user * 1000)); 150 | OSC_MsgSend(auxS, 50); 151 | 152 | #if DEBUG>0 153 | Serial.print("Par: "); 154 | Serial.print(Kp_user); 155 | Serial.print(" "); 156 | Serial.print(Kd_user); 157 | Serial.print(" "); 158 | Serial.print(Kp_thr_user); 159 | Serial.print(" "); 160 | Serial.println(Ki_thr_user); 161 | #endif 162 | 163 | // Calibration mode?? 164 | if (OSCpush[2] == 1) { 165 | Serial.print("Calibration MODE "); 166 | angle_offset = angle_adjusted_filtered; 167 | Serial.println(angle_offset); 168 | } 169 | 170 | // Kill robot => Sleep 171 | while (OSCtoggle[0] == 1) { 172 | //Reset external parameters 173 | PID_errorSum = 0; 174 | timer_old = millis(); 175 | setMotorSpeedM1(0); 176 | setMotorSpeedM2(0); 177 | digitalWrite(PIN_ENABLE_MOTORS, HIGH); // Disable motors 178 | OSC_MsgRead(); 179 | } 180 | } 181 | } 182 | 183 | void loop() { 184 | OSC_MsgRead(); 185 | 186 | if (OSCnewMessage) { 187 | OSCnewMessage = 0; 188 | processOSCMsg(); 189 | } 190 | 191 | timer_value = micros(); 192 | 193 | if (MPU6050_newData()) { 194 | MPU6050_read_3axis(); 195 | 196 | loop_counter++; 197 | slow_loop_counter++; 198 | dt = (timer_value - timer_old) * 0.000001; // dt in seconds 199 | timer_old = timer_value; 200 | 201 | angle_adjusted_Old = angle_adjusted; 202 | // Get new orientation angle from IMU (MPU6050) 203 | float MPU_sensor_angle = MPU6050_getAngle(dt); 204 | angle_adjusted = MPU_sensor_angle + angle_offset; 205 | if ((MPU_sensor_angle > -15) && (MPU_sensor_angle < 15)) 206 | angle_adjusted_filtered = angle_adjusted_filtered * 0.99 207 | + MPU_sensor_angle * 0.01; 208 | 209 | #if DEBUG==1 210 | Serial.print(dt); 211 | Serial.print(" "); 212 | Serial.print(angle_offset); 213 | Serial.print(" "); 214 | Serial.print(angle_adjusted); 215 | Serial.print(","); 216 | Serial.println(angle_adjusted_filtered); 217 | #endif 218 | //Serial.print("\t"); 219 | 220 | // We calculate the estimated robot speed: 221 | // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU) 222 | actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward 223 | 224 | int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 25.0; // 25 is an empirical extracted factor to adjust for real units 225 | int16_t estimated_speed = -actual_robot_speed + angular_velocity; 226 | estimated_speed_filtered = estimated_speed_filtered * 0.9 227 | + (float) estimated_speed * 0.1; // low pass filter on estimated speed 228 | 229 | #if DEBUG==2 230 | Serial.print(angle_adjusted); 231 | Serial.print(" "); 232 | Serial.println(estimated_speed_filtered); 233 | #endif 234 | 235 | if (positionControlMode) { 236 | // POSITION CONTROL. INPUT: Target steps for each motor. Output: motors speed 237 | motor1_control = positionPDControl(steps1, target_steps1, 238 | Kp_position, Kd_position, speed_M1); 239 | motor2_control = positionPDControl(steps2, target_steps2, 240 | Kp_position, Kd_position, speed_M2); 241 | 242 | // Convert from motor position control to throttle / steering commands 243 | throttle = (motor1_control + motor2_control) / 2; 244 | throttle = constrain(throttle, -190, 190); 245 | steering = motor2_control - motor1_control; 246 | steering = constrain(steering, -50, 50); 247 | } 248 | 249 | // ROBOT SPEED CONTROL: This is a PI controller. 250 | // input:user throttle(robot speed), variable: estimated robot speed, output: target robot angle to get the desired speed 251 | target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, 252 | Kp_thr, Ki_thr); 253 | target_angle = constrain(target_angle, -max_target_angle, 254 | max_target_angle); // limited output 255 | 256 | #if DEBUG==3 257 | Serial.print(angle_adjusted); 258 | Serial.print(" "); 259 | Serial.print(estimated_speed_filtered); 260 | Serial.print(" "); 261 | Serial.println(target_angle); 262 | #endif 263 | 264 | // Stability control (100Hz loop): This is a PD controller. 265 | // input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed 266 | // We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed. 267 | control_output += stabilityPDControl(dt, angle_adjusted, target_angle, 268 | Kp, Kd); 269 | control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, 270 | MAX_CONTROL_OUTPUT); // Limit max output from control 271 | 272 | // The steering part from the user is injected directly to the output 273 | motor1 = control_output + steering; 274 | motor2 = control_output - steering; 275 | 276 | // Limit max speed (control output) 277 | motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); 278 | motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); 279 | 280 | int angle_ready; 281 | if (OSCpush[0]) // If we press the SERVO button we start to move 282 | angle_ready = 82; 283 | else 284 | angle_ready = 74; // Default angle 285 | if ((angle_adjusted < angle_ready) && (angle_adjusted > -angle_ready)) // Is robot ready (upright?) 286 | { 287 | // NORMAL MODE 288 | digitalWrite(PIN_ENABLE_MOTORS, LOW); // Motors enable 289 | // NOW we send the commands to the motors 290 | setMotorSpeedM1(motor1); 291 | setMotorSpeedM2(motor2); 292 | } else // Robot not ready (flat), angle > angle_ready => ROBOT OFF 293 | { 294 | digitalWrite(PIN_ENABLE_MOTORS, HIGH); // Disable motors 295 | setMotorSpeedM1(0); 296 | setMotorSpeedM2(0); 297 | PID_errorSum = 0; // Reset PID I term 298 | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP 299 | Kd = KD_RAISEUP; 300 | Kp_thr = KP_THROTTLE_RAISEUP; 301 | Ki_thr = KI_THROTTLE_RAISEUP; 302 | // RESET steps 303 | steps1 = 0; 304 | steps2 = 0; 305 | positionControlMode = false; 306 | OSCmove_mode = false; 307 | throttle = 0; 308 | steering = 0; 309 | } 310 | // Push1 Move servo arm 311 | if (OSCpush[0]) // Move arm 312 | { 313 | if (angle_adjusted > -40) 314 | ledcWrite(6, SERVO_MIN_PULSEWIDTH); 315 | else 316 | ledcWrite(6, SERVO_MAX_PULSEWIDTH); 317 | } else 318 | ledcWrite(6, SERVO_AUX_NEUTRO); 319 | 320 | // Servo2 321 | //ledcWrite(6, SERVO2_NEUTRO + (OSCfader[2] - 0.5) * SERVO2_RANGE); 322 | 323 | // Normal condition? 324 | if ((angle_adjusted < 56) && (angle_adjusted > -56)) { 325 | Kp = Kp_user; // Default user control gains 326 | Kd = Kd_user; 327 | Kp_thr = Kp_thr_user; 328 | Ki_thr = Ki_thr_user; 329 | } else // We are in the raise up procedure => we use special control parameters 330 | { 331 | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP 332 | Kd = KD_RAISEUP; 333 | Kp_thr = KP_THROTTLE_RAISEUP; 334 | Ki_thr = KI_THROTTLE_RAISEUP; 335 | } 336 | 337 | } // End of new IMU data 338 | 339 | // Medium loop 7.5Hz 340 | if (loop_counter >= 15) { 341 | loop_counter = 0; 342 | // Telemetry here? 343 | #if TELEMETRY_ANGLE==1 344 | char auxS[25]; 345 | int ang_out = constrain(int(angle_adjusted * 10), -900, 900); 346 | sprintf(auxS, "$tA,%+04d", ang_out); 347 | OSC_MsgSend(auxS, 25); 348 | #endif 349 | #if TELEMETRY_DEBUG==1 350 | char auxS[50]; 351 | sprintf(auxS, "$tD,%d,%d,%ld", int(angle_adjusted * 10), int(estimated_speed_filtered), steps1); 352 | OSC_MsgSend(auxS, 50); 353 | #endif 354 | 355 | } // End of medium loop 356 | else if (slow_loop_counter >= 100) // 1Hz 357 | { 358 | slow_loop_counter = 0; 359 | // Read status 360 | #if TELEMETRY_BATTERY==1 361 | BatteryValue = (BatteryValue + BROBOT_readBattery(false)) / 2; 362 | sendBattery_counter++; 363 | if (sendBattery_counter >= 3) { //Every 3 seconds we send a message 364 | sendBattery_counter = 0; 365 | Serial.print("B"); 366 | Serial.println(BatteryValue); 367 | char auxS[25]; 368 | sprintf(auxS, "$tB,%04d", BatteryValue); 369 | OSC_MsgSend(auxS, 25); 370 | } 371 | #endif 372 | } // End of slow loop 373 | } 374 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/MPU6050.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MPU6050.cpp 3 | * 4 | * Created on: 22.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #include "MPU6050.h" 9 | #include "Wire.h" 10 | 11 | // BROBOT EVO 2 by JJROBOTS 12 | // SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS 13 | // License: GPL v2 14 | // MPU9250/6050 IMU code 15 | // Read the accel and gyro registers and calculate a complementary filter for sensor fusion between gyros and accel 16 | 17 | // Code based on arduino.cc MPU6050 sample 18 | // Open Source / Public Domain 19 | // 20 | // Documentation:"MPU-6000 and MPU-6050 Register Map and Descriptions": RM-MPU-6000A.pdf 21 | 22 | // MPU6050 Register map 23 | 24 | 25 | // Global MPU6050 IMU variables 26 | accel_t_gyro_union accel_t_gyro; 27 | float x_gyro_value; // in deg/seg units 28 | float x_gyro_offset = 0.0; 29 | float accel_angle; // in degree units 30 | float angle; 31 | 32 | // Util function to swap byte values 33 | uint8_t swap; 34 | #define SWAP(x,y) swap = x; x = y; y = swap 35 | 36 | 37 | // This function implements a complementary filter to fusion gyro and accel info 38 | float MPU6050_getAngle(float dt) 39 | { 40 | accel_angle = atan2f((float)accel_t_gyro.value.y_accel, (float)accel_t_gyro.value.z_accel) * RAD2GRAD; 41 | x_gyro_value = (accel_t_gyro.value.x_gyro - x_gyro_offset) / 65.5; // Accel scale at 500deg/seg => 65.5 LSB/deg/s 42 | 43 | // Complementary filter 44 | // We integrate the gyro rate value to obtain the angle in the short term and we take the accelerometer angle with a low pass filter in the long term... 45 | angle = 0.99 * (angle + x_gyro_value * dt) + 0.01 * accel_angle; // Time constant = 0.99*0.01(100hz)/(1-0.99) = 0.99, around 1 sec. 46 | 47 | // Gyro bias correction 48 | // We supose that the long term mean of the gyro_value should tend to zero (gyro_offset). This means that the robot is not continuosly rotating. 49 | int16_t correction = constrain(accel_t_gyro.value.x_gyro, x_gyro_offset - 10, x_gyro_offset + 10); // limit corrections... 50 | x_gyro_offset = x_gyro_offset * 0.9995 + correction * 0.0005; // Time constant of this correction is around 20 sec. 51 | 52 | //Serial.print(angle); 53 | //Serial.print(" "); 54 | //Serial.println(x_gyro_offset); 55 | 56 | return angle; 57 | } 58 | 59 | // Calibrate function. Take 100 readings (over 2 seconds) to calculate the gyro offset value. IMU should be steady in this process... 60 | void MPU6050_calibrate() 61 | { 62 | int i; 63 | long value = 0; 64 | float dev; 65 | int16_t values[100]; 66 | bool gyro_cal_ok = false; 67 | 68 | delay(500); 69 | while (!gyro_cal_ok){ 70 | Serial.println("Gyro calibration... DONT MOVE!"); 71 | // we take 100 measurements in 4 seconds 72 | for (i = 0; i < 100; i++) 73 | { 74 | MPU6050_read_3axis(); 75 | values[i] = accel_t_gyro.value.x_gyro; 76 | value += accel_t_gyro.value.x_gyro; 77 | delay(25); 78 | } 79 | // mean value 80 | value = value / 100; 81 | // calculate the standard deviation 82 | dev = 0; 83 | for (i = 0; i < 100; i++) 84 | dev += (values[i] - value) * (values[i] - value); 85 | dev = sqrt((1 / 100.0) * dev); 86 | Serial.print("offset: "); 87 | Serial.print(value); 88 | Serial.print(" stddev: "); 89 | Serial.println(dev); 90 | if (dev < 50.0) 91 | gyro_cal_ok = true; 92 | else 93 | Serial.println("Repeat, DONT MOVE!"); 94 | } 95 | x_gyro_offset = value; 96 | // Take the first reading of angle from accels 97 | angle = atan2f((float)accel_t_gyro.value.y_accel, (float)accel_t_gyro.value.z_accel) * RAD2GRAD; 98 | } 99 | 100 | void MPU6050_setup() 101 | { 102 | int error; 103 | uint8_t c; 104 | 105 | error = MPU6050_read(MPU6050_WHO_AM_I, &c, 1); 106 | Serial.print("WHO_AM_I : "); 107 | Serial.print(c, HEX); 108 | Serial.print(", error = "); 109 | Serial.println(error, DEC); 110 | 111 | // RESET chip 112 | MPU6050_write_reg(MPU6050_PWR_MGMT_1, bit(MPU6050_DEVICE_RESET)); 113 | delay(125); 114 | // Clear the 'sleep' bit to start the sensor and select clock source 115 | MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0x01); 116 | //MPU6050_write_reg(MPU6050_PWR_MGMT_1,MPU6050_CLKSEL_Z); 117 | 118 | // Config Gyro scale (500deg/seg) 119 | MPU6050_write_reg(MPU6050_GYRO_CONFIG, MPU6050_FS_SEL_500); 120 | // Config Accel scale (2g) 121 | MPU6050_write_reg(MPU6050_ACCEL_CONFIG, MPU6050_AFS_SEL_2G); 122 | // Config Digital Low Pass Filter 10Hz 123 | MPU6050_write_reg(MPU6050_CONFIG, MPU6050_DLPF_10HZ); 124 | // Set Sample Rate to 100Hz 125 | MPU6050_write_reg(MPU6050_SMPLRT_DIV, 9); // 100Hz : Sample Rate = 1000 / (1 + SMPLRT_DIV) Hz 126 | // Data ready interrupt enable 127 | MPU6050_write_reg(MPU6050_INT_ENABLE, MPU6050_DATA_RDY_EN); 128 | // Clear the 'sleep' bit to start the sensor (and select clock source). 129 | MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0x01); 130 | 131 | // Clear the 'sleep' bit to start the sensor. 132 | //MPU6050_write_reg(MPU6050_PWR_MGMT_1,MPU6050_CLKSEL_Z); 133 | //MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0); 134 | } 135 | 136 | void MPU6050_read_3axis() 137 | { 138 | int error; 139 | 140 | // read 14 bytes (gyros, temp and accels) 141 | error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro)); 142 | if (error != 0) { 143 | //Serial.print("MPU6050 Error:"); 144 | //Serial.println(error); 145 | } 146 | // swap bytes 147 | SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l); 148 | SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l); 149 | SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l); 150 | SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l); 151 | SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l); 152 | SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l); 153 | SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l); 154 | 155 | /* 156 | // Print the raw acceleration values 157 | Serial.print("ACC:"); 158 | Serial.print(accel_t_gyro.value.x_accel, DEC); 159 | Serial.print(","); 160 | Serial.print(accel_t_gyro.value.y_accel, DEC); 161 | Serial.print(","); 162 | Serial.print(accel_t_gyro.value.z_accel, DEC); 163 | Serial.println(); 164 | 165 | // Print the raw gyro values. 166 | Serial.print("GY:"); 167 | Serial.print(accel_t_gyro.value.x_gyro, DEC); 168 | Serial.print(","); 169 | Serial.print(accel_t_gyro.value.y_gyro, DEC); 170 | Serial.print(","); 171 | Serial.print(accel_t_gyro.value.z_gyro, DEC); 172 | Serial.print(","); 173 | Serial.println(); 174 | */ 175 | } 176 | 177 | void MPU6050_read_1axis() 178 | { 179 | int error; 180 | 181 | // read X accel 182 | error = MPU6050_read(MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro.reg.x_accel_h, 6); 183 | if (error != 0) { 184 | //Serial.print("MPU6050 Error:"); 185 | //Serial.println(error); 186 | } 187 | // read X gyro 188 | error = MPU6050_read(MPU6050_GYRO_XOUT_H, (uint8_t *) &accel_t_gyro.reg.x_gyro_h, 2); 189 | if (error != 0) { 190 | //Serial.print("MPU6050 Error:"); 191 | //Serial.println(error); 192 | } 193 | SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.y_accel_l); 194 | SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l); 195 | SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l); 196 | 197 | // Print values 198 | Serial.print("axis:"); 199 | Serial.print(accel_t_gyro.value.y_accel, DEC); 200 | Serial.print(","); 201 | Serial.println(accel_t_gyro.value.x_gyro, DEC); 202 | } 203 | 204 | // return true on new data available 205 | bool MPU6050_newData() 206 | { 207 | uint8_t status; 208 | int error; 209 | 210 | error = MPU6050_read(MPU6050_INT_STATUS, &status, 1); 211 | if (error != 0) { 212 | //Serial.print("MPU6050 Error:"); 213 | //Serial.println(error); 214 | } 215 | if (status & (0b00000001)) // Data ready? 216 | return true; 217 | else 218 | return false; 219 | } 220 | 221 | // MPU6050_read n bytes 222 | int MPU6050_read(int start, uint8_t *buffer, int size) 223 | { 224 | int i, n, error; 225 | 226 | Wire.beginTransmission(MPU6050_I2C_ADDRESS); 227 | n = Wire.write(start); 228 | if (n != 1) 229 | return (-10); 230 | 231 | n = Wire.endTransmission(false); // hold the I2C-bus 232 | if (n != 0) 233 | return (n); 234 | 235 | // Third parameter is true: relase I2C-bus after data is read. 236 | Wire.requestFrom((uint8_t) MPU6050_I2C_ADDRESS, (size_t) size, (bool) true); 237 | i = 0; 238 | while (Wire.available() && i < size) 239 | { 240 | buffer[i++] = Wire.read(); 241 | } 242 | if ( i != size) 243 | return (-11); 244 | 245 | return (0); // return : no error 246 | } 247 | 248 | 249 | // MPU6050_write n bytes 250 | int MPU6050_write(int start, const uint8_t *pData, int size) 251 | { 252 | int n, error; 253 | 254 | Wire.beginTransmission(MPU6050_I2C_ADDRESS); 255 | n = Wire.write(start); // write the start address 256 | if (n != 1) 257 | return (-20); 258 | 259 | n = Wire.write(pData, size); // write data bytes 260 | if (n != size) 261 | return (-21); 262 | 263 | error = Wire.endTransmission(true); // release the I2C-bus 264 | if (error != 0) 265 | return (error); 266 | 267 | return (0); // return : no error 268 | } 269 | 270 | // -------------------------------------------------------- 271 | // MPU6050_write_reg (only 1 byte) 272 | int MPU6050_write_reg(int reg, uint8_t data) 273 | { 274 | int error; 275 | 276 | error = MPU6050_write(reg, &data, 1); 277 | 278 | return (error); 279 | } 280 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/MPU6050.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MPU6050.h 3 | * 4 | * Created on: 22.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #include 9 | 10 | #ifndef MPU6050_H_ 11 | #define MPU6050_H_ 12 | 13 | #define RAD2GRAD 57.2957795 14 | #define GRAD2RAD 0.01745329251994329576923690768489 15 | 16 | #define MPU6050_AUX_VDDIO 0x01 // R/W 17 | #define MPU6050_SMPLRT_DIV 0x19 // R/W 18 | #define MPU6050_CONFIG 0x1A // R/W 19 | #define MPU6050_GYRO_CONFIG 0x1B // R/W 20 | #define MPU6050_ACCEL_CONFIG 0x1C // R/W 21 | #define MPU6050_FF_THR 0x1D // R/W 22 | #define MPU6050_FF_DUR 0x1E // R/W 23 | #define MPU6050_MOT_THR 0x1F // R/W 24 | #define MPU6050_MOT_DUR 0x20 // R/W 25 | #define MPU6050_ZRMOT_THR 0x21 // R/W 26 | #define MPU6050_ZRMOT_DUR 0x22 // R/W 27 | #define MPU6050_FIFO_EN 0x23 // R/W 28 | #define MPU6050_I2C_MST_CTRL 0x24 // R/W 29 | #define MPU6050_I2C_SLV0_ADDR 0x25 // R/W 30 | #define MPU6050_I2C_SLV0_REG 0x26 // R/W 31 | #define MPU6050_I2C_SLV0_CTRL 0x27 // R/W 32 | #define MPU6050_I2C_SLV1_ADDR 0x28 // R/W 33 | #define MPU6050_I2C_SLV1_REG 0x29 // R/W 34 | #define MPU6050_I2C_SLV1_CTRL 0x2A // R/W 35 | #define MPU6050_I2C_SLV2_ADDR 0x2B // R/W 36 | #define MPU6050_I2C_SLV2_REG 0x2C // R/W 37 | #define MPU6050_I2C_SLV2_CTRL 0x2D // R/W 38 | #define MPU6050_I2C_SLV3_ADDR 0x2E // R/W 39 | #define MPU6050_I2C_SLV3_REG 0x2F // R/W 40 | #define MPU6050_I2C_SLV3_CTRL 0x30 // R/W 41 | #define MPU6050_I2C_SLV4_ADDR 0x31 // R/W 42 | #define MPU6050_I2C_SLV4_REG 0x32 // R/W 43 | #define MPU6050_I2C_SLV4_DO 0x33 // R/W 44 | #define MPU6050_I2C_SLV4_CTRL 0x34 // R/W 45 | #define MPU6050_I2C_SLV4_DI 0x35 // R 46 | #define MPU6050_I2C_MST_STATUS 0x36 // R 47 | #define MPU6050_INT_PIN_CFG 0x37 // R/W 48 | #define MPU6050_INT_ENABLE 0x38 // R/W 49 | #define MPU6050_INT_STATUS 0x3A // R 50 | #define MPU6050_ACCEL_XOUT_H 0x3B // R 51 | #define MPU6050_ACCEL_XOUT_L 0x3C // R 52 | #define MPU6050_ACCEL_YOUT_H 0x3D // R 53 | #define MPU6050_ACCEL_YOUT_L 0x3E // R 54 | #define MPU6050_ACCEL_ZOUT_H 0x3F // R 55 | #define MPU6050_ACCEL_ZOUT_L 0x40 // R 56 | #define MPU6050_TEMP_OUT_H 0x41 // R 57 | #define MPU6050_TEMP_OUT_L 0x42 // R 58 | #define MPU6050_GYRO_XOUT_H 0x43 // R 59 | #define MPU6050_GYRO_XOUT_L 0x44 // R 60 | #define MPU6050_GYRO_YOUT_H 0x45 // R 61 | #define MPU6050_GYRO_YOUT_L 0x46 // R 62 | #define MPU6050_GYRO_ZOUT_H 0x47 // R 63 | #define MPU6050_GYRO_ZOUT_L 0x48 // R 64 | #define MPU6050_EXT_SENS_DATA_00 0x49 // R 65 | #define MPU6050_EXT_SENS_DATA_01 0x4A // R 66 | #define MPU6050_EXT_SENS_DATA_02 0x4B // R 67 | #define MPU6050_EXT_SENS_DATA_03 0x4C // R 68 | #define MPU6050_EXT_SENS_DATA_04 0x4D // R 69 | #define MPU6050_EXT_SENS_DATA_05 0x4E // R 70 | #define MPU6050_EXT_SENS_DATA_06 0x4F // R 71 | #define MPU6050_EXT_SENS_DATA_07 0x50 // R 72 | #define MPU6050_EXT_SENS_DATA_08 0x51 // R 73 | #define MPU6050_EXT_SENS_DATA_09 0x52 // R 74 | #define MPU6050_EXT_SENS_DATA_10 0x53 // R 75 | #define MPU6050_EXT_SENS_DATA_11 0x54 // R 76 | #define MPU6050_EXT_SENS_DATA_12 0x55 // R 77 | #define MPU6050_EXT_SENS_DATA_13 0x56 // R 78 | #define MPU6050_EXT_SENS_DATA_14 0x57 // R 79 | #define MPU6050_EXT_SENS_DATA_15 0x58 // R 80 | #define MPU6050_EXT_SENS_DATA_16 0x59 // R 81 | #define MPU6050_EXT_SENS_DATA_17 0x5A // R 82 | #define MPU6050_EXT_SENS_DATA_18 0x5B // R 83 | #define MPU6050_EXT_SENS_DATA_19 0x5C // R 84 | #define MPU6050_EXT_SENS_DATA_20 0x5D // R 85 | #define MPU6050_EXT_SENS_DATA_21 0x5E // R 86 | #define MPU6050_EXT_SENS_DATA_22 0x5F // R 87 | #define MPU6050_EXT_SENS_DATA_23 0x60 // R 88 | #define MPU6050_MOT_DETECT_STATUS 0x61 // R 89 | #define MPU6050_I2C_SLV0_DO 0x63 // R/W 90 | #define MPU6050_I2C_SLV1_DO 0x64 // R/W 91 | #define MPU6050_I2C_SLV2_DO 0x65 // R/W 92 | #define MPU6050_I2C_SLV3_DO 0x66 // R/W 93 | #define MPU6050_I2C_MST_DELAY_CTRL 0x67 // R/W 94 | #define MPU6050_SIGNAL_PATH_RESET 0x68 // R/W 95 | #define MPU6050_MOT_DETECT_CTRL 0x69 // R/W 96 | #define MPU6050_USER_CTRL 0x6A // R/W 97 | #define MPU6050_PWR_MGMT_1 0x6B // R/W 98 | #define MPU6050_PWR_MGMT_2 0x6C // R/W 99 | #define MPU6050_FIFO_COUNTH 0x72 // R/W 100 | #define MPU6050_FIFO_COUNTL 0x73 // R/W 101 | #define MPU6050_FIFO_R_W 0x74 // R/W 102 | #define MPU6050_WHO_AM_I 0x75 // R 103 | 104 | // Defines for the bits, to be able to change 105 | // between bit number and binary definition. 106 | // By using the bit number, programming the sensor 107 | // is like programming the AVR microcontroller. 108 | // But instead of using "(1< 9 | #include "defines.h" 10 | #include "globals.h" 11 | 12 | // Set speed of Stepper Motor1 13 | // tspeed could be positive or negative (reverse) 14 | void setMotorSpeedM1(int16_t tspeed) 15 | { 16 | long timer_period; 17 | int16_t speed; 18 | 19 | // Limit max speed? 20 | 21 | // WE LIMIT MAX ACCELERATION of the motors 22 | if ((speed_M1 - tspeed) > MAX_ACCEL) 23 | speed_M1 -= MAX_ACCEL; 24 | else if ((speed_M1 - tspeed) < -MAX_ACCEL) 25 | speed_M1 += MAX_ACCEL; 26 | else 27 | speed_M1 = tspeed; 28 | 29 | #if MICROSTEPPING==16 30 | speed = speed_M1 * 50; // Adjust factor from control output speed to real motor speed in steps/second 31 | #else 32 | speed = speed_M1 * 25; // 1/8 Microstepping 33 | #endif 34 | 35 | if (speed == 0) 36 | { 37 | timer_period = ZERO_SPEED; 38 | dir_M1 = 0; 39 | } 40 | else if (speed > 0) 41 | { 42 | timer_period = 2000000 / speed; // 2Mhz timer 43 | dir_M1 = 1; 44 | digitalWrite(PIN_MOTOR1_DIR, HIGH); 45 | } 46 | else 47 | { 48 | timer_period = 2000000 / -speed; 49 | dir_M1 = -1; 50 | digitalWrite(PIN_MOTOR1_DIR, LOW); 51 | } 52 | if (timer_period > ZERO_SPEED) // Check for minimun speed (maximun period without overflow) 53 | timer_period = ZERO_SPEED; 54 | 55 | timerAlarmWrite(timer1, timer_period, true); 56 | } 57 | 58 | // Set speed of Stepper Motor2 59 | // tspeed could be positive or negative (reverse) 60 | void setMotorSpeedM2(int16_t tspeed) 61 | { 62 | long timer_period; 63 | int16_t speed; 64 | 65 | // Limit max speed? 66 | 67 | // WE LIMIT MAX ACCELERATION of the motors 68 | if ((speed_M2 - tspeed) > MAX_ACCEL) 69 | speed_M2 -= MAX_ACCEL; 70 | else if ((speed_M2 - tspeed) < -MAX_ACCEL) 71 | speed_M2 += MAX_ACCEL; 72 | else 73 | speed_M2 = tspeed; 74 | 75 | #if MICROSTEPPING==16 76 | speed = speed_M2 * 50; // Adjust factor from control output speed to real motor speed in steps/second 77 | #else 78 | speed = speed_M2 * 25; // 1/8 Microstepping 79 | #endif 80 | 81 | if (speed == 0) 82 | { 83 | timer_period = ZERO_SPEED; 84 | dir_M2 = 0; 85 | } 86 | else if (speed > 0) 87 | { 88 | timer_period = 2000000 / speed; // 2Mhz timer 89 | dir_M2 = 1; 90 | digitalWrite(PIN_MOTOR2_DIR, LOW); 91 | } 92 | else 93 | { 94 | timer_period = 2000000 / -speed; 95 | dir_M2 = -1; 96 | digitalWrite(PIN_MOTOR2_DIR, HIGH); 97 | } 98 | if (timer_period > ZERO_SPEED) // Check for minimun speed (maximun period without overflow) 99 | timer_period = ZERO_SPEED; 100 | 101 | timerAlarmWrite(timer2, timer_period, true); 102 | } 103 | 104 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/Motors.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Motors.h 3 | * 4 | * Created on: 25.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #ifndef MOTORS_H_ 9 | #define MOTORS_H_ 10 | 11 | #include 12 | 13 | void setMotorSpeedM1(int16_t tspeed); 14 | void setMotorSpeedM2(int16_t tspeed); 15 | 16 | 17 | 18 | 19 | #endif /* MOTORS_H_ */ 20 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/OSC.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OSC.cpp 3 | * 4 | * Created on: 02.10.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #include "OSC.h" 9 | #include 10 | #include "globals.h" 11 | 12 | // for DEBUG uncomment this lines... 13 | //#define OSCDEBUG 0 14 | 15 | #define MAX_BUFFER 32 16 | unsigned char UDPBuffer[MAX_BUFFER]; // input message buffer 17 | 18 | // OSC message internal variables 19 | unsigned char OSCtouchMessage; 20 | WiFiUDP udp; 21 | IPAddress telemetryServer(192, 168, 4, 2); 22 | 23 | // ------- OSC functions ----------------------------------------- 24 | 25 | // Aux functions 26 | float OSC_extractParamFloat(uint8_t pos) { 27 | union { 28 | unsigned char Buff[4]; 29 | float d; 30 | } u; 31 | 32 | u.Buff[0] = UDPBuffer[pos + 3]; 33 | u.Buff[1] = UDPBuffer[pos + 2]; 34 | u.Buff[2] = UDPBuffer[pos + 1]; 35 | u.Buff[3] = UDPBuffer[pos]; 36 | 37 | return (u.d); 38 | } 39 | 40 | int16_t OSC_extractParamInt(uint8_t pos) { 41 | union { 42 | unsigned char Buff[2]; 43 | int16_t d; 44 | } u; 45 | 46 | u.Buff[1] = UDPBuffer[pos + 1]; 47 | u.Buff[0] = UDPBuffer[pos]; 48 | 49 | return (u.d); 50 | } 51 | 52 | void OSC_init() { 53 | udp.begin(2222); 54 | 55 | OSCfader[0] = 0.5; 56 | OSCfader[1] = 0.5; 57 | OSCfader[2] = 0.5; 58 | OSCfader[3] = 0.5; 59 | } 60 | 61 | void OSC_MsgSend(char *c, unsigned char msgSize, float p) { 62 | uint8_t i; 63 | union { 64 | unsigned char Buff[4]; 65 | float d; 66 | } u; 67 | 68 | // We copy the param in the last 4 bytes 69 | u.d = p; 70 | c[msgSize - 4] = u.Buff[3]; 71 | c[msgSize - 3] = u.Buff[2]; 72 | c[msgSize - 2] = u.Buff[1]; 73 | c[msgSize - 1] = u.Buff[0]; 74 | 75 | OSC_MsgSend(c, msgSize); 76 | } 77 | 78 | void OSC_MsgSend(char *c, unsigned char msgSize) { 79 | udp.beginPacket(telemetryServer, 2223); 80 | udp.write((uint8_t*) c, msgSize); 81 | udp.endPacket(); 82 | } 83 | 84 | void OSC_MsgRead() { 85 | int packetSize = udp.parsePacket(); 86 | 87 | if (packetSize <= MAX_BUFFER && packetSize > 0) { 88 | if (udp.read(UDPBuffer, MAX_BUFFER)) { 89 | #ifdef OSCDEBUG 90 | Serial.println(UDPBuffer); 91 | #endif 92 | // We look for an OSC message start like /x/ 93 | if ((UDPBuffer[0] == '/') && (UDPBuffer[2] == '/') 94 | && ((UDPBuffer[1] == '1') || (UDPBuffer[1] == '2'))) { 95 | OSCnewMessage = 1; 96 | OSCpage = UDPBuffer[1] - '0'; // Convert page to int 97 | OSCtouchMessage = 0; 98 | 99 | float value = OSC_extractParamFloat(16); 100 | 101 | if ((UDPBuffer[5] == 'd') && (UDPBuffer[6] == 'e') 102 | && (UDPBuffer[7] == 'r')) { 103 | // Fader /1/fader1 ,f xxxx 104 | 105 | OSCfader[UDPBuffer[8] - '0' - 1] = value; 106 | return; 107 | } else if ((UDPBuffer[4] == 'u') && (UDPBuffer[5] == 's') 108 | && (UDPBuffer[6] == 'h')) { 109 | // Push message 110 | 111 | if (value == 0) 112 | OSCpush[UDPBuffer[7] - '0' - 1] = 0; 113 | else 114 | OSCpush[UDPBuffer[7] - '0' - 1] = 1; 115 | 116 | return; 117 | } else if ((UDPBuffer[6] == 'g') && (UDPBuffer[7] == 'l') 118 | && (UDPBuffer[8] == 'e')) { 119 | // Toggle message 120 | if (value == 0) 121 | OSCtoggle[UDPBuffer[9] - '0' - 1] = 0; 122 | else 123 | OSCtoggle[UDPBuffer[9] - '0' - 1] = 1; 124 | return; 125 | } 126 | } 127 | } 128 | } 129 | } 130 | 131 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/OSC.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OSC.h 3 | * 4 | * Created on: 02.10.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #ifndef OSC_H_ 9 | #define OSC_H_ 10 | 11 | #include 12 | 13 | void OSC_init(); 14 | 15 | void OSC_MsgSend(char *c, unsigned char msgSize); 16 | void OSC_MsgSend(char *c, unsigned char msgSize, float p); 17 | 18 | void OSC_MsgRead(); 19 | 20 | 21 | 22 | 23 | #endif /* OSC_H_ */ 24 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/Timers.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Timers.c 3 | * 4 | * Created on: 24.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #include "globals.h" 9 | #include "defines.h" 10 | #include 11 | #include "freertos/FreeRTOS.h" 12 | #include "freertos/task.h" 13 | #include "freertos/queue.h" 14 | #include "esp_types.h" 15 | #include "soc/timer_group_struct.h" 16 | #include "driver/periph_ctrl.h" 17 | #include "driver/timer.h" 18 | #include "esp32-hal-timer.h" 19 | 20 | extern "C" { 21 | 22 | portMUX_TYPE muxer1 = portMUX_INITIALIZER_UNLOCKED; 23 | portMUX_TYPE muxer2 = portMUX_INITIALIZER_UNLOCKED; 24 | 25 | void IRAM_ATTR timer1ISR() { 26 | portENTER_CRITICAL_ISR(&muxer1); 27 | 28 | if (dir_M1 != 0) { 29 | // We generate 1us STEP pulse 30 | digitalWrite(PIN_MOTOR1_STEP, HIGH); 31 | 32 | if (dir_M1 > 0) 33 | steps1--; 34 | else 35 | steps1++; 36 | 37 | digitalWrite(PIN_MOTOR1_STEP, LOW); 38 | } 39 | 40 | portEXIT_CRITICAL_ISR(&muxer1); 41 | } 42 | void IRAM_ATTR timer2ISR() { 43 | portENTER_CRITICAL_ISR(&muxer2); 44 | 45 | if (dir_M2 != 0) { 46 | // We generate 1us STEP pulse 47 | digitalWrite(PIN_MOTOR2_STEP, HIGH); 48 | 49 | if (dir_M2 > 0) 50 | steps2--; 51 | else 52 | steps2++; 53 | 54 | digitalWrite(PIN_MOTOR2_STEP, LOW); 55 | } 56 | portEXIT_CRITICAL_ISR(&muxer2); 57 | } 58 | } 59 | 60 | void initTimers() { 61 | 62 | timer1 = timerBegin(0, 40, true); 63 | timerAttachInterrupt(timer1, &timer1ISR, true); 64 | timerAlarmWrite(timer1, ZERO_SPEED, true); 65 | 66 | timer2 = timerBegin(1, 40, true); 67 | timerAttachInterrupt(timer2, &timer2ISR, true); 68 | timerAlarmWrite(timer2, ZERO_SPEED, true); 69 | 70 | timerAlarmEnable(timer1); 71 | timerAlarmEnable(timer2); 72 | 73 | } 74 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/defines.h: -------------------------------------------------------------------------------- 1 | /* 2 | * defines.h 3 | * 4 | * Created on: 25.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #ifndef DEFINES_H_ 9 | #define DEFINES_H_ 10 | 11 | #define PIN_ENABLE_MOTORS 12 12 | 13 | #define PIN_SERVO 17 14 | 15 | #define PIN_MOTOR1_DIR 27 16 | #define PIN_MOTOR1_STEP 14 17 | 18 | #define PIN_MOTOR2_DIR 25 19 | #define PIN_MOTOR2_STEP 26 20 | 21 | #define TELEMETRY "192.168.4.1" // Default telemetry server (first client) port 2223 22 | 23 | // NORMAL MODE PARAMETERS (MAXIMUN SETTINGS) 24 | #define MAX_THROTTLE 550 25 | #define MAX_STEERING 140 26 | #define MAX_TARGET_ANGLE 14 27 | 28 | // PRO MODE = MORE AGGRESSIVE (MAXIMUN SETTINGS) 29 | #define MAX_THROTTLE_PRO 780 // Max recommended value: 860 30 | #define MAX_STEERING_PRO 260 // Max recommended value: 280 31 | #define MAX_TARGET_ANGLE_PRO 26 // Max recommended value: 32 32 | 33 | // Default control terms for EVO 2 34 | #define KP 0.32 35 | #define KD 0.050 36 | #define KP_THROTTLE 0.080 37 | #define KI_THROTTLE 0.1 38 | #define KP_POSITION 0.06 39 | #define KD_POSITION 0.45 40 | //#define KI_POSITION 0.02 41 | 42 | // Control gains for raiseup (the raiseup movement requiere special control parameters) 43 | #define KP_RAISEUP 0.1 44 | #define KD_RAISEUP 0.16 45 | #define KP_THROTTLE_RAISEUP 0 // No speed control on raiseup 46 | #define KI_THROTTLE_RAISEUP 0.0 47 | 48 | #define MAX_CONTROL_OUTPUT 500 49 | #define ITERM_MAX_ERROR 30 // Iterm windup constants for PI control 50 | #define ITERM_MAX 10000 51 | 52 | #define ANGLE_OFFSET 0.0 // Offset angle for balance (to compensate robot own weight distribution) 53 | 54 | // Servo definitions 55 | #define SERVO_AUX_NEUTRO 4444 // Servo neutral position 56 | #define SERVO_MIN_PULSEWIDTH SERVO_AUX_NEUTRO - 2700 57 | #define SERVO_MAX_PULSEWIDTH SERVO_AUX_NEUTRO + 2700 58 | 59 | #define SERVO2_NEUTRO 4444 60 | #define SERVO2_RANGE 8400 61 | 62 | // Telemetry 63 | #define TELEMETRY_BATTERY 0 64 | #define TELEMETRY_ANGLE 1 65 | //#define TELEMETRY_DEBUG 1 // Dont use TELEMETRY_ANGLE and TELEMETRY_DEBUG at the same time! 66 | 67 | #define ZERO_SPEED 0xffffff 68 | #define MAX_ACCEL 14 // Maximun motor acceleration (MAX RECOMMENDED VALUE: 20) (default:14) 69 | 70 | #define MICROSTEPPING 16 // 8 or 16 for 1/8 or 1/16 driver microstepping (default:16) 71 | 72 | #define DEBUG 0 // 0 = No debug info (default) DEBUG 1 for console output 73 | 74 | // AUX definitions 75 | #define CLR(x,y) (x&=(~(1< 9 | #include "defines.h" 10 | #include "esp32-hal-timer.h" 11 | 12 | 13 | String MAC; // MAC address of Wifi module 14 | 15 | volatile long counter1 = 0; 16 | volatile long counter2 = 0; 17 | hw_timer_t * timer1 = NULL; 18 | hw_timer_t * timer2 = NULL; 19 | 20 | uint8_t cascade_control_loop_counter = 0; 21 | uint8_t loop_counter; // To generate a medium loop 40Hz 22 | uint8_t slow_loop_counter; // slow loop 2Hz 23 | uint8_t sendBattery_counter; // To send battery status 24 | int16_t BatteryValue; 25 | 26 | long timer_old; 27 | long timer_value; 28 | float debugVariable; 29 | float dt; 30 | 31 | // Angle of the robot (used for stability control) 32 | float angle_adjusted; 33 | float angle_adjusted_Old; 34 | float angle_adjusted_filtered=0.0; 35 | 36 | // Default control values from constant definitions 37 | float Kp = KP; 38 | float Kd = KD; 39 | float Kp_thr = KP_THROTTLE; 40 | float Ki_thr = KI_THROTTLE; 41 | float Kp_user = KP; 42 | float Kd_user = KD; 43 | float Kp_thr_user = KP_THROTTLE; 44 | float Ki_thr_user = KI_THROTTLE; 45 | float Kp_position = KP_POSITION; 46 | float Kd_position = KD_POSITION; 47 | bool newControlParameters = false; 48 | bool modifing_control_parameters = false; 49 | int16_t position_error_sum_M1; 50 | int16_t position_error_sum_M2; 51 | float PID_errorSum; 52 | float PID_errorOld = 0; 53 | float PID_errorOld2 = 0; 54 | float setPointOld = 0; 55 | float target_angle; 56 | int16_t throttle; 57 | float steering; 58 | float max_throttle = MAX_THROTTLE; 59 | float max_steering = MAX_STEERING; 60 | float max_target_angle = MAX_TARGET_ANGLE; 61 | float control_output; 62 | float angle_offset = ANGLE_OFFSET; 63 | 64 | boolean positionControlMode = false; 65 | uint8_t mode; // mode = 0 Normal mode, mode = 1 Pro mode (More agressive) 66 | 67 | int16_t motor1; 68 | int16_t motor2; 69 | 70 | // position control 71 | volatile int32_t steps1; 72 | volatile int32_t steps2; 73 | int32_t target_steps1; 74 | int32_t target_steps2; 75 | int16_t motor1_control; 76 | int16_t motor2_control; 77 | 78 | int16_t speed_M1, speed_M2; // Actual speed of motors 79 | int8_t dir_M1, dir_M2; // Actual direction of steppers motors 80 | int16_t actual_robot_speed; // overall robot speed (measured from steppers speed) 81 | int16_t actual_robot_speed_Old; 82 | float estimated_speed_filtered; // Estimated robot speed 83 | 84 | // OSC output variables 85 | uint8_t OSCpage; 86 | uint8_t OSCnewMessage; 87 | float OSCfader[4]; 88 | float OSCxy1_x; 89 | float OSCxy1_y; 90 | float OSCxy2_x; 91 | float OSCxy2_y; 92 | uint8_t OSCpush[4]; 93 | uint8_t OSCtoggle[4]; 94 | uint8_t OSCmove_mode; 95 | int16_t OSCmove_speed; 96 | int16_t OSCmove_steps1; 97 | int16_t OSCmove_steps2; 98 | 99 | 100 | -------------------------------------------------------------------------------- /BRobotEvo2ESP32/globals.h: -------------------------------------------------------------------------------- 1 | /* 2 | * globals.h 3 | * 4 | * Created on: 25.09.2017 5 | * Author: anonymous 6 | */ 7 | 8 | #ifndef GLOBALS_H_ 9 | #define GLOBALS_H_ 10 | 11 | #include 12 | #include "esp32-hal-timer.h" 13 | 14 | extern volatile long counter1; 15 | extern volatile long counter2; 16 | extern hw_timer_t * timer1; 17 | extern hw_timer_t * timer2; 18 | 19 | extern String MAC; // MAC address of Wifi module 20 | 21 | extern uint8_t cascade_control_loop_counter; 22 | extern uint8_t loop_counter; // To generate a medium loop 40Hz 23 | extern uint8_t slow_loop_counter; // slow loop 2Hz 24 | extern uint8_t sendBattery_counter; // To send battery status 25 | extern int16_t BatteryValue; 26 | 27 | extern long timer_old; 28 | extern long timer_value; 29 | extern float debugVariable; 30 | extern float dt; 31 | 32 | // Angle of the robot (used for stability control) 33 | extern float angle_adjusted; 34 | extern float angle_adjusted_Old; 35 | extern float angle_adjusted_filtered; 36 | 37 | // Default control values from constant definitions 38 | extern float Kp; 39 | extern float Kd; 40 | extern float Kp_thr; 41 | extern float Ki_thr; 42 | extern float Kp_user; 43 | extern float Kd_user; 44 | extern float Kp_thr_user; 45 | extern float Ki_thr_user; 46 | extern float Kp_position; 47 | extern float Kd_position; 48 | extern bool newControlParameters; 49 | extern bool modifing_control_parameters; 50 | extern int16_t position_error_sum_M1; 51 | extern int16_t position_error_sum_M2; 52 | extern float PID_errorSum; 53 | extern float PID_errorOld; 54 | extern float PID_errorOld2; 55 | extern float setPointOld; 56 | extern float target_angle; 57 | extern int16_t throttle; 58 | extern float steering; 59 | extern float max_throttle; 60 | extern float max_steering; 61 | extern float max_target_angle; 62 | extern float control_output; 63 | extern float angle_offset; 64 | 65 | extern boolean positionControlMode; 66 | extern uint8_t mode; // mode = 0 Normal mode, mode = 1 Pro mode (More agressive) 67 | 68 | extern int16_t motor1; 69 | extern int16_t motor2; 70 | 71 | // position control 72 | extern volatile int32_t steps1; 73 | extern volatile int32_t steps2; 74 | extern int32_t target_steps1; 75 | extern int32_t target_steps2; 76 | extern int16_t motor1_control; 77 | extern int16_t motor2_control; 78 | 79 | extern int16_t speed_M1, speed_M2; // Actual speed of motors 80 | extern int8_t dir_M1, dir_M2; // Actual direction of steppers motors 81 | extern int16_t actual_robot_speed; // overall robot speed (measured from steppers speed) 82 | extern int16_t actual_robot_speed_Old; 83 | extern float estimated_speed_filtered; // Estimated robot speed 84 | 85 | // OSC output variables 86 | extern uint8_t OSCpage; 87 | extern uint8_t OSCnewMessage; 88 | extern float OSCfader[4]; 89 | extern float OSCxy1_x; 90 | extern float OSCxy1_y; 91 | extern float OSCxy2_x; 92 | extern float OSCxy2_y; 93 | extern uint8_t OSCpush[4]; 94 | extern uint8_t OSCtoggle[4]; 95 | extern uint8_t OSCmove_mode; 96 | extern int16_t OSCmove_speed; 97 | extern int16_t OSCmove_steps1; 98 | extern int16_t OSCmove_steps2; 99 | 100 | #endif /* GLOBALS_H_ */ 101 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # B-ROBOT_EVO2_ESP32 2 | ESP32 port of JJRobots brilliant B-ROBOT_EVO2 3 | 4 | The robot: 5 | ![Robot](assets/Robot.png) 6 | 7 | Circuit top: 8 | ![Circuit Top](assets/CircuitTop.png) 9 | 10 | Circuit bottom: 11 | ![Circuit Bottom](assets/CircuitBottom.png) 12 | 13 | 14 | Pin connections (refer to defines.h to change): 15 | * Enable motors: P12 16 | * Servo: P17 17 | * Motor1 Dir: P27 18 | * Motor1 Step: P14 19 | * Motor2 Dir: P25 20 | * Motor2 Step: P26 21 | * I2C pins for gyro: (defaults) SDA=P21, SCL=P22 22 | 23 | ESP-32 Board is a NodeMCU ESP32s, see [Pinout](http://esp32.net/images/Ai-Thinker/NodeMCU-32S/Ai-Thinker_NodeMCU-32S_DiagramPinout.png). 24 | 25 | **Important note:** At the time of this writing (January 2018) the recent version of arduino-esp32 contains changes that break the I2C communication with the IMU a few seconds after startup. 26 | This version is known to work reliably for the purpose of the robot: 27 | https://github.com/espressif/arduino-esp32/tree/32c028a27e6b3c3c3df769bb42f0d87917e0309c 28 | 29 | For more details on the I2C problem mentioned, please refer to these issues (haven't tried the forks mentioned there): 30 | * https://github.com/espressif/arduino-esp32/issues/834 31 | * https://github.com/espressif/arduino-esp32/issues/839 32 | 33 | Further discussions, information, circuit diagrams and more you can find in the corresponding thread in JJRobots forum at http://forums.jjrobots.com/showthread.php?tid=1399 34 | 35 | -------------------------------------------------------------------------------- /assets/CircuitBottom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ghmartin77/B-ROBOT_EVO2_ESP32/e1ec8001620391eddeb65eb4eca464b49927172a/assets/CircuitBottom.png -------------------------------------------------------------------------------- /assets/CircuitTop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ghmartin77/B-ROBOT_EVO2_ESP32/e1ec8001620391eddeb65eb4eca464b49927172a/assets/CircuitTop.png -------------------------------------------------------------------------------- /assets/Robot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ghmartin77/B-ROBOT_EVO2_ESP32/e1ec8001620391eddeb65eb4eca464b49927172a/assets/Robot.png --------------------------------------------------------------------------------