emc2_tmc2130_nema17SERVO_balance.ino (16277B)
1 #define EN_PIN 13 2 #define DIR_PIN 32 // 32 for 30p esp32, 33 for 38p esp32 3 #define STEP_PIN 33 // 33 for 30p esp32, 25 for 38p esp32 4 #define CS_PIN 5 //chip select 5 #define R_SENSE 0.11f // Match to your driver 6 7 #define SDA_PIN 27 // 27 for 30p esp32, 15 for 38p esp32 8 #define SCL_PIN 26 // 26 for 30p esp32, 16 for 38p esp32 9 10 #define SWITCH_ONE_PIN 14 // 14 for 30p esp32, 17 for 38p esp32 11 #define ANALOG_PIN 34 // Can use pin 35 or 34, but let's keep it at 34 12 13 #define I2C_SLAVE_ADDR 0x60 // 0x62 for thighs 14 15 #include <TMCStepper.h> 16 #include <FastAccelStepper.h> // Make sure you have the latest FastAccelstepper.h 17 18 #include <Adafruit_MPU6050.h> 19 #include <Adafruit_Sensor.h> 20 21 #include <Wire.h> // Make sure you have the latest ESP32 dev package: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json 22 #include <PID_v1.h> 23 24 // Seeed Studio AS5600.h library used for Encoder safety and error-checking. 25 // Make sure your i2c master pins (22 SCL, 21 SDA) are connected to the AS5600, in addition to the analog pin. 26 #include <AS5600.h> 27 28 TMC2130Stepper driver = TMC2130Stepper(CS_PIN, R_SENSE); // Hardware SPI 29 30 FastAccelStepperEngine engine = FastAccelStepperEngine(); 31 FastAccelStepper *stepperA = NULL; 32 33 AMS_5600 ams5600; 34 Adafruit_MPU6050 emc2gyro; 35 36 // Mpu6050 37 long lvalue; 38 long solid_statepoint; 39 40 // Homing handlers 41 int limitSwitchOne_state = 0; 42 43 float homeOffset = 2.5; // Perfect alignment located at angle 115 (i.e. 6.5 for thighs). 2.5 is standard 44 bool switchPressed = false; 45 bool offsetTriggered = false; 46 47 bool isInitiatingHoming = false; 48 bool isContinuousRotation = false; 49 50 // Balance-related stuffs 51 int Raw_B = 0; 52 int Input_B = 0; 53 54 bool balanceModeOn = false; 55 bool balanceRunning = false; 56 double gyroOutput; 57 58 // Wire character read stuffs 59 float Raw_Input = 0; 60 float User_Input = 0; // This while convert input string into integer 61 62 boolean flagRx = false; 63 boolean containsComma = false; 64 boolean containsAt = false; 65 char readStringC[10]; //This while store the user input data 66 67 // PID stuffs 68 double kp = 6, ki = 5, kd = 0.1; 69 double kp2 = 24, ki2 = 8, kd2 = 0.01; 70 71 float newkp = 24, newki = 8, newkd = 0.01; 72 73 double input = 0, output = 0, setpoint = 0; 74 double input2 = 0, output2 = 0, setpoint2 = 0; 75 76 PID myPID( & input, & output, & setpoint, kp, ki, kd, DIRECT); 77 PID myPID2( & input2, &output2, &setpoint2, kp2, ki2, kd2, DIRECT); 78 79 // Motor and driver speed stuffs 80 long openSpeed = 40; // Less is more 81 long closedSpeed = openSpeed + 10; 82 83 // WARNING: USE SLOW VALUES WHEN FIRST TESTING AND SETTING UP! Make sure you have an on/off switch handy for main 36v 84 long balanceSpeed = 15; // 35 is a good speed for first test, then 8 85 long minBalanceSpeed = 45; 86 long balanceAccel = 100000; 87 long balanceCurrent = 700; 88 long cutoffCurrent = 450; 89 90 long openAccel = 100000; 91 long homingSpeed = 25; 92 long homingAccel = 1000000; 93 94 int enableThreshold = 1; 95 int gyroThreshold = 80; 96 int gyroCutoff = 20; 97 98 bool openLoopRunning = false; 99 bool closedLoopRunning = true; 100 bool closedLoopStart = true; 101 102 // Encoder stuffs 103 int quadrantNumber, previousQuadrantNumber; 104 bool quadrantClockwise = true; 105 long revolutions = 0; // number of revolutions the encoder has made 106 107 double totalAngle, startAngle; 108 double previousTotalAngle; 109 110 // Stepper driver settings and gearboxes (if any) 111 float bevelGear = (49.00 / 12.00); 112 float gearReduction = 53.575757576 * bevelGear; 113 114 float stepsPerRev = 200.00; 115 int microstepping = 32; 116 float formula = ((stepsPerRev * microstepping) / 360.00); 117 118 long driverCurrent = 1000; 119 120 void setup() { 121 Serial.begin(115200); 122 SPI.begin(); 123 124 Wire.begin(); 125 Wire.setClock(800000); 126 bool success = Wire1.begin(I2C_SLAVE_ADDR, SDA_PIN, SCL_PIN, 400000); 127 if (!success) { 128 Serial.println("Error: I2C slave init failed"); 129 while (1) delay(100); 130 } else { 131 Serial.println("Success: I2C slave init!"); 132 } 133 Wire1.onReceive(receiveEvent); 134 135 myPID.SetMode(AUTOMATIC); //set PID in Auto mode 136 myPID.SetSampleTime(2); // refresh rate of PID controller 137 myPID.SetOutputLimits(-500, 500); // this is the MAX PWM value to move motor, here change in value reflect change in speed of motor. 138 139 myPID2.SetMode(AUTOMATIC); //set PID in Auto mode 140 myPID2.SetSampleTime(2); // refresh rate of PID controller 141 myPID2.SetOutputLimits(-500, 500); // this is the MAX PWM value to move motor, here change in value reflect change in speed of motor. 142 143 emc2gyro.begin(); 144 emc2gyro.setAccelerometerRange(MPU6050_RANGE_2_G);//2_G,4_G,8_G,16_G 145 emc2gyro.setGyroRange(MPU6050_RANGE_250_DEG);//250,500,1000,2000 146 emc2gyro.setFilterBandwidth(MPU6050_BAND_5_HZ); 147 148 engine.init(); 149 stepperA = engine.stepperConnectToPin(STEP_PIN); 150 if (stepperA) { 151 stepperA -> setDirectionPin(DIR_PIN); 152 stepperA -> setEnablePin(EN_PIN); 153 stepperA -> setAutoEnable(true); 154 155 stepperA -> setSpeedInUs(openSpeed); // the parameter is us/step !!! 156 stepperA -> setAcceleration(openAccel); 157 } 158 159 pinMode(CS_PIN, OUTPUT); 160 digitalWrite(CS_PIN, HIGH); 161 driver.begin(); // Initiate pins and registeries 162 driver.rms_current(driverCurrent); // Set stepper current to 600mA. The command is the same as command TMC2130.setCurrent(600, 0.11, 0.5); 163 driver.microsteps(microstepping); 164 driver.shaft(true); // Set this to True to change shaft direction without flipping cable. To change AS5600 encoder direction, short DIR to VCC & GPO 165 166 startAngle = degAngle(); //update startAngle with degAngle - for taring 167 int magnetStrength = ams5600.getMagnetStrength(); 168 169 if (magnetStrength == 0){ 170 Serial.println("Error: No magnet detected!"); 171 } else if (magnetStrength == 1){ 172 Serial.println("Warning: Magnet is too weak!"); 173 } else if (magnetStrength == 2){ 174 Serial.println("Success: Magnet is OK."); 175 } else if (magnetStrength == 3){ 176 Serial.println("Warning: Magnet is too strong!"); 177 } 178 } 179 180 double degAngle() { 181 double rawAngle = ams5600.getRawAngle(); //analogRead(ANALOG_PIN); 182 return rawAngle * (360.00 / 4096.00); 183 } 184 185 double taredAngle() { 186 double correctedAngle = degAngle() - startAngle; //this tares the position 187 188 if (correctedAngle < 0) { //if the calculated angle is negative, we need to "normalize" it 189 correctedAngle = correctedAngle + 360.00; //correction for negative numbers (i.e. -15 becomes +345) 190 } 191 192 return correctedAngle; 193 } 194 195 void getEncoderReading() { 196 while (ams5600.getMagnetStrength() < 2){ // Stops stepper motor if encoder not detected for safety 197 if (stepperA -> isRunning() == true){ 198 stepperA -> forceStop(); 199 } 200 Serial.println("Error: Magnet is weak!"); 201 } 202 203 double correctAngle = taredAngle(); //tare the value 204 205 if (correctAngle >= 0 && correctAngle <= 90) { 206 quadrantNumber = 1; 207 } else if (correctAngle > 90 && correctAngle <= 180) { 208 quadrantNumber = 2; 209 } else if (correctAngle > 180 && correctAngle <= 270) { 210 quadrantNumber = 3; 211 } else if (correctAngle > 270 && correctAngle < 360) { 212 quadrantNumber = 4; 213 } 214 215 if (quadrantNumber != previousQuadrantNumber) { //if we changed quadrant 216 if (quadrantNumber == 1 && previousQuadrantNumber == 4) { 217 revolutions++; // 4 --> 1 transition: CW rotation 218 quadrantClockwise = true; 219 } else if (quadrantNumber == 4 && previousQuadrantNumber == 1) { 220 revolutions--; // 1 --> 4 transition: CCW rotation 221 quadrantClockwise = false; 222 } 223 previousQuadrantNumber = quadrantNumber; //update to the current quadrant 224 } 225 totalAngle = (revolutions * 360.00) + correctAngle; 226 //Serial.println(totalAngle); 227 228 // Check for missing rotations! And correct them accordingly... 229 if (abs(totalAngle - previousTotalAngle) > 350.00) { 230 Serial.println("Warning: Missed revolution. Details: "); 231 Serial.println(previousTotalAngle); 232 Serial.println(totalAngle); 233 if (quadrantClockwise == true) { 234 revolutions--; 235 } else { 236 revolutions++; 237 } 238 totalAngle = (revolutions * 360.00) + correctAngle; 239 } 240 previousTotalAngle = totalAngle; 241 } 242 243 void stepperClosedLoop(int out) { 244 if (closedLoopStart == true) { 245 //Serial.println(User_Input); 246 //Serial.println(totalAngle); 247 closedLoopStart = false; 248 } 249 250 if (abs(out) < enableThreshold) { 251 stepperA -> stopMove(); 252 closedLoopRunning = false; 253 } else { 254 if (out > 0) { 255 int newSpeed = map(abs(out), 0, 500, 1000, closedSpeed); 256 stepperA -> setSpeedInUs(newSpeed); 257 stepperA -> applySpeedAcceleration(); 258 stepperA -> runForward(); 259 } else if (out < 0) { 260 int newSpeed = map(abs(out), 0, 500, 1000, closedSpeed); 261 stepperA -> setSpeedInUs(newSpeed); 262 stepperA -> applySpeedAcceleration(); 263 stepperA -> runBackward(); 264 } 265 } 266 } 267 268 void stepperBalance(int gyroOut, int bSpeed, int mbSpeed) { 269 openLoopRunning = true; 270 closedLoopRunning = false; 271 closedLoopStart = false; 272 273 if (abs(gyroOut) > gyroCutoff) { 274 balanceRunning = true; 275 if (gyroOut > 0) { 276 int newSpeed = map(abs(gyroOut), 0, 500, mbSpeed, bSpeed); 277 stepperA -> setSpeedInUs(newSpeed); 278 stepperA -> setAcceleration(balanceAccel); 279 stepperA -> applySpeedAcceleration(); 280 stepperA -> runBackward(); 281 } else if (gyroOut < 0) { 282 int newSpeed = map(abs(gyroOut), 0, 500, mbSpeed, bSpeed); 283 stepperA -> setSpeedInUs(newSpeed); 284 stepperA -> setAcceleration(balanceAccel); 285 stepperA -> applySpeedAcceleration(); 286 stepperA -> runForward(); 287 } 288 }else{ 289 if (balanceRunning == true){ 290 stepperA -> stopMove(); 291 balanceRunning = false; 292 } 293 } 294 } 295 296 void stepperOpenLoop(float openSteps, float moveSpeed) { 297 int actualSteps_relative = openSteps * formula; 298 int actualAngle = totalAngle * formula; 299 300 openLoopRunning = true; 301 closedLoopRunning = true; 302 closedLoopStart = true; 303 304 stepperA -> setCurrentPosition(actualAngle); 305 306 // Open loop runs faster than closed loop error correction 307 stepperA -> setSpeedInUs(moveSpeed); 308 stepperA -> setAcceleration(openAccel); 309 stepperA -> applySpeedAcceleration(); 310 stepperA -> moveTo(actualSteps_relative); 311 } 312 313 void receiveEvent(int byteCount) { 314 int i; 315 containsComma = false; 316 containsAt = false; 317 318 for (i = 0; i < byteCount; i++) { 319 readStringC[i] = Wire1.read(); // receive byte as a character 320 if (readStringC[i] == ','){ 321 containsComma = true; 322 } 323 else if (readStringC[i] == '@'){ 324 containsAt = true; 325 } 326 } 327 readStringC[i] = '\0'; // assume it is a string, add zero-terminator 328 flagRx = true; // set flag, we received something. 329 } 330 331 void loop() { 332 limitSwitchOne_state = digitalRead(SWITCH_ONE_PIN); 333 334 if (isInitiatingHoming == true) { 335 if (limitSwitchOne_state == HIGH){ 336 switchPressed = true; 337 } 338 339 if (switchPressed == true) { 340 if (offsetTriggered == false){ 341 stepperA -> stopMove(); 342 stepperA -> setCurrentPosition(0); 343 stepperA -> moveTo((-homeOffset * gearReduction) * formula); 344 345 offsetTriggered = true; 346 }else{ 347 if (stepperA -> isRunning() == false){ 348 ESP.restart(); //Don't bother with fancy code. Restart the damn thing! 349 } 350 } 351 }else{ 352 stepperA -> setSpeedInUs(homingSpeed); // the parameter is us/step !!! 353 stepperA -> setAcceleration(homingAccel); 354 stepperA -> applySpeedAcceleration(); 355 stepperA -> runForward(); 356 } 357 } else { 358 getEncoderReading(); 359 360 sensors_event_t a, g, temp; 361 emc2gyro.getEvent(&a, &g, &temp); 362 363 float fvalue = a.acceleration.y; //a.acceleration.x for thighs, a.acceleration.y for waist 364 lvalue = (fvalue * 10); 365 lvalue = map(lvalue, -90, 90, 180, 0); 366 367 //Serial.println(lvalue); 368 369 if (flagRx == true) { //Verify that the variable contains information 370 if (readStringC[0] == 'b') { 371 balanceModeOn = true; 372 solid_statepoint = lvalue; 373 driver.rms_current(balanceCurrent); 374 } 375 376 else if (readStringC[0] == 'x') { 377 balanceModeOn = false; 378 stepperA -> stopMove(); 379 closedLoopRunning = true; 380 closedLoopStart = true; 381 driver.rms_current(driverCurrent); 382 User_Input = totalAngle; 383 } 384 385 // Manual user data feed 386 else if (readStringC[0] == 'm') { 387 if (containsAt == true){ 388 // Handle master requests for current encoder position, etc. 389 char reqMessage[64]; 390 float gearedAngle = totalAngle / gearReduction; 391 snprintf(reqMessage, 64, "%f", gearedAngle); 392 Wire1.slaveWrite((uint8_t *)reqMessage, strlen(reqMessage)); 393 }else{ 394 readStringC[0] = '0'; 395 bool isNegative = false; 396 float moveAngle; 397 float moveSpeed; 398 399 if (readStringC[1] == '-') { 400 readStringC[1] = '0'; 401 isNegative = true; 402 } 403 404 if (containsComma == true){ 405 char *moveStrings[6]; // an array of pointers to the pieces of the above array after strtok() 406 char *ptr = NULL; 407 int moveIndex = 0; 408 409 ptr = strtok(readStringC, ","); // delimiter 410 while (ptr != NULL) { 411 moveStrings[moveIndex] = ptr; 412 moveIndex++; 413 ptr = strtok(NULL, ","); 414 } 415 moveAngle = atof(moveStrings[0]); 416 moveSpeed = atof(moveStrings[1]); 417 }else{ 418 moveAngle = atof(readStringC); 419 moveSpeed = openSpeed; 420 } 421 422 //Check for negative numbers 423 if (isNegative == true){ 424 Raw_Input = moveAngle; // here input data is stored in float form 425 User_Input = -Raw_Input; 426 } else { 427 User_Input = moveAngle; // here input data is stored in float form 428 } 429 430 if (isContinuousRotation == false) { 431 float Abs_Input = abs(User_Input); 432 User_Input = -Abs_Input; 433 } 434 //Apply Gear Reduction and move angle with speed 435 User_Input = User_Input * gearReduction; 436 stepperOpenLoop(User_Input, moveSpeed); 437 } 438 } else if (readStringC[0] == 's') { 439 readStringC[0] = '0'; 440 openSpeed = atoi(readStringC); 441 } else if (readStringC[0] == 'l') { 442 readStringC[0] = '0'; 443 balanceSpeed = atoi(readStringC); 444 } else if (readStringC[0] == 'n') { 445 readStringC[0] = '0'; 446 minBalanceSpeed = atoi(readStringC); 447 } else if (readStringC[0] == 'c') { 448 readStringC[0] = '0'; 449 if (atoi(readStringC) <= 1200) { 450 driverCurrent = atoi(readStringC); 451 driver.rms_current(driverCurrent); 452 } 453 454 // For PID tuning on the fly 455 } else if (readStringC[0] == 'p') { 456 readStringC[0] = '0'; 457 newkp = atof(readStringC); 458 myPID2.SetTunings(newkp, newki, newkd); 459 } else if (readStringC[0] == 'i') { 460 readStringC[0] = '0'; 461 newki = atof(readStringC); 462 myPID2.SetTunings(newkp, newki, newkd); 463 } else if (readStringC[0] == 'd') { 464 readStringC[0] = '0'; 465 newkd = atof(readStringC); 466 myPID2.SetTunings(newkp, newki, newkd); 467 } else if (readStringC[0] == 'h') { 468 isInitiatingHoming = true; 469 } 470 flagRx = false; 471 } 472 473 if (stepperA -> isRunning() == false) { 474 openLoopRunning = false; 475 } 476 477 if (balanceModeOn == true) { 478 setpoint2 = solid_statepoint; //PID while work to achive this value consider as SET value 479 input2 = lvalue; // data from encoder consider as a Process value 480 myPID2.Compute(); // calculate new output 481 482 int gyro2out = output2; 483 if(abs(gyro2out) < gyroThreshold){ 484 int newBalanceSpeed = balanceSpeed * 10; 485 balanceAccel = 50000; 486 int newMinBalanceSpeed = minBalanceSpeed * 10; 487 driver.rms_current(cutoffCurrent); 488 stepperBalance(output2, newBalanceSpeed, newMinBalanceSpeed); 489 }else{ 490 balanceAccel = 600000; 491 driver.rms_current(balanceCurrent); 492 stepperBalance(output2, balanceSpeed, minBalanceSpeed); 493 } 494 } 495 496 if (openLoopRunning == false) { 497 if (closedLoopRunning == true) { 498 setpoint = User_Input; //PID while work to achive this value consider as SET value 499 //Serial.println(User_Input); 500 input = totalAngle; // data from encoder consider as a Process value 501 myPID.Compute(); // calculate new output 502 stepperClosedLoop(output); 503 } 504 } 505 } 506 }