emc2_drv8825_nema17SERVO_standalone.ino (11562B)
1 //This is code for the EMC2 Robot servo using DRV8825 driver. 2 //You don't have to change anything with the wiring. 3 //..Just make sure you use this code on your ESP32!! 4 // 5 //It is recommended to use the StepStick Protector with DRV8825 6 7 #define EN_PIN 13 8 #define DIR_PIN 32 // 33 for 38p esp32 9 #define STEP_PIN 33 // 25 for 38p esp32 10 11 // Pins used for SPI are now used to set microsteps to 32 res 12 #define MODE0_PIN 23 13 #define MODE1_PIN 18 14 #define MODE2_PIN 5 15 #define RESET_PIN 19 16 17 //#define CS_PIN 5 18 //#define R_SENSE 0.11f // Match to your driver 19 20 #define SDA_PIN 27 // 15 for 38p esp32 21 #define SCL_PIN 26 // 16 for 38p esp32 22 23 #define SWITCH_ONE_PIN 14 // 17 for 38p esp32 24 #define ANALOG_PIN 34 // Can use pin 35 or 34, but let's keep it at 34 25 26 #define I2C_SLAVE_ADDR 0x61 27 28 //#include <TMCStepper.h> 29 #include <FastAccelStepper.h> 30 31 #include <Wire.h> 32 #include <PID_v1.h> 33 34 // Seeed Studio AS5600.h library used for Encoder safety and error-checking. 35 // Make sure your i2c master pins (22 SCL, 21 SDA) are connected to the AS5600, in addition to the analog pin. 36 #include <AS5600.h> 37 38 //TMC2130Stepper driver = TMC2130Stepper(CS_PIN, R_SENSE); // Hardware SPI 39 40 FastAccelStepperEngine engine = FastAccelStepperEngine(); 41 FastAccelStepper *stepperA = NULL; 42 43 AMS_5600 ams5600; 44 45 // Homing handlers 46 int limitSwitchOne_state = 0; 47 48 float homeOffset = 2.5; 49 bool switchPressed = false; 50 bool offsetTriggered = false; 51 52 bool isInitiatingHoming = false; 53 54 bool isContinuousRotation = true; 55 56 // Wire character read stuffs 57 float Raw_Input = 0; 58 float User_Input = 0; // This while convert input string into integer 59 60 boolean flagRx = false; 61 boolean containsComma = false; 62 char readStringC[10]; //This while store the user input data 63 64 // PID stuffs 65 double kp = 6, ki = 5, kd = 0.1; 66 float newkp = 6, newki = 5, newkd = 0.1; 67 double input = 0, output = 0, setpoint = 0; 68 PID myPID( & input, & output, & setpoint, kp, ki, kd, DIRECT); 69 70 // Motor and driver speed stuffs 71 long openSpeed = 40; // Less is more 72 long closedSpeed = openSpeed + 10; 73 74 long openAccel = 100000; 75 long homingSpeed = 25; 76 long homingAccel = 1000000; 77 78 int enableThreshold = 1; 79 80 bool openLoopRunning = false; 81 bool closedLoopRunning = true; 82 bool closedLoopStart = true; 83 84 // Encoder stuffs 85 int quadrantNumber, previousQuadrantNumber; 86 bool quadrantClockwise = true; 87 long revolutions = 0; // number of revolutions the encoder has made 88 89 double totalAngle, startAngle; 90 double previousTotalAngle; 91 92 // Stepper driver settings and gearboxes (if any) 93 float bevelGear = (49.00 / 12.00); 94 float gearReduction = 53.575757576 * bevelGear; 95 96 float stepsPerRev = 200.00; 97 int microstepping = 32; 98 float formula = ((stepsPerRev * microstepping) / 360.00); 99 100 //long driverCurrent = 1000; 101 102 void setup() { 103 Serial.begin(115200); 104 //SPI.begin(); 105 106 Wire.begin(); 107 Wire.setClock(800000); 108 bool success = Wire1.begin(I2C_SLAVE_ADDR, SDA_PIN, SCL_PIN, 400000); 109 if (!success) { 110 Serial.println("I2C slave init failed"); 111 while (1) delay(100); 112 } else { 113 Serial.println("I2C slave init success!"); 114 } 115 Wire1.onReceive(receiveEvent); 116 117 myPID.SetMode(AUTOMATIC); //set PID in Auto mode 118 myPID.SetSampleTime(2); // refresh rate of PID controller 119 myPID.SetOutputLimits(-500, 500); // this is the MAX PWM value to move motor, here change in value reflect change in speed of motor. 120 121 engine.init(); 122 stepperA = engine.stepperConnectToPin(STEP_PIN); 123 if (stepperA) { 124 stepperA -> setDirectionPin(DIR_PIN); 125 stepperA -> setEnablePin(EN_PIN); 126 stepperA -> setAutoEnable(true); 127 128 stepperA -> setSpeedInUs(openSpeed); // the parameter is us/step !!! 129 stepperA -> setAcceleration(openAccel); 130 } 131 132 pinMode(MODE0_PIN, OUTPUT); 133 pinMode(MODE1_PIN, OUTPUT); 134 pinMode(MODE2_PIN, OUTPUT); 135 pinMode(RESET_PIN, OUTPUT); 136 137 digitalWrite(MODE0_PIN, HIGH); 138 digitalWrite(MODE1_PIN, HIGH); 139 digitalWrite(MODE2_PIN, HIGH); 140 digitalWrite(RESET_PIN, HIGH); 141 142 //driver.begin(); // Initiate pins and registeries 143 //driver.rms_current(driverCurrent); // Set stepper current to 600mA. The command is the same as command TMC2130.setCurrent(600, 0.11, 0.5); 144 //driver.microsteps(microstepping); 145 //driver.shaft(false); 146 147 startAngle = degAngle(); //update startAngle with degAngle - for taring 148 int magnetStrength = ams5600.getMagnetStrength(); 149 150 if (magnetStrength == 0){ 151 Serial.println("Error: No magnet detected!"); 152 } else if (magnetStrength == 1){ 153 Serial.println("Warning: Magnet is too weak!"); 154 } else if (magnetStrength == 2){ 155 Serial.println("Success: Magnet is OK."); 156 } else if (magnetStrength == 3){ 157 Serial.println("Warning: Magnet is too strong!"); 158 } 159 } 160 161 double degAngle() { 162 double rawAngle = ams5600.getRawAngle(); //analogRead(ANALOG_PIN); 163 return rawAngle * (360.00 / 4096.00); 164 } 165 166 double taredAngle() { 167 double correctedAngle = degAngle() - startAngle; //this tares the position 168 169 if (correctedAngle < 0) { //if the calculated angle is negative, we need to "normalize" it 170 correctedAngle = correctedAngle + 360.00; //correction for negative numbers (i.e. -15 becomes +345) 171 } 172 173 return correctedAngle; 174 } 175 176 void getEncoderReading() { 177 while (ams5600.getMagnetStrength() < 2){ // Stops stepper motor if encoder not detected for safety 178 if (stepperA -> isRunning() == true){ 179 stepperA -> forceStop(); 180 } 181 Serial.println("Error: Magnet is weak!"); 182 } 183 184 double correctAngle = taredAngle(); //tare the value 185 186 if (correctAngle >= 0 && correctAngle <= 90) { 187 quadrantNumber = 1; 188 } else if (correctAngle > 90 && correctAngle <= 180) { 189 quadrantNumber = 2; 190 } else if (correctAngle > 180 && correctAngle <= 270) { 191 quadrantNumber = 3; 192 } else if (correctAngle > 270 && correctAngle < 360) { 193 quadrantNumber = 4; 194 } 195 196 if (quadrantNumber != previousQuadrantNumber) { //if we changed quadrant 197 if (quadrantNumber == 1 && previousQuadrantNumber == 4) { 198 revolutions++; // 4 --> 1 transition: CW rotation 199 quadrantClockwise = true; 200 } else if (quadrantNumber == 4 && previousQuadrantNumber == 1) { 201 revolutions--; // 1 --> 4 transition: CCW rotation 202 quadrantClockwise = false; 203 } 204 previousQuadrantNumber = quadrantNumber; //update to the current quadrant 205 } 206 totalAngle = (revolutions * 360.00) + correctAngle; 207 //Serial.println(totalAngle); 208 209 // Check for missing rotations! And correct them accordingly... 210 if (abs(totalAngle - previousTotalAngle) > 350.00) { 211 Serial.println("Warning! Missed revolution! Details: "); 212 Serial.println(previousTotalAngle); 213 Serial.println(totalAngle); 214 if (quadrantClockwise == true) { 215 revolutions--; 216 } else { 217 revolutions++; 218 } 219 totalAngle = (revolutions * 360.00) + correctAngle; 220 } 221 previousTotalAngle = totalAngle; 222 } 223 224 void stepperClosedLoop(int out) { 225 if (closedLoopStart == true) { 226 Serial.println(User_Input); 227 Serial.println(totalAngle); 228 closedLoopStart = false; 229 } 230 231 if (abs(out) < enableThreshold) { 232 stepperA -> stopMove(); 233 closedLoopRunning = false; 234 } else { 235 if (out > 0) { 236 int newSpeed = map(abs(out), 0, 500, 1000, closedSpeed); 237 stepperA -> setSpeedInUs(newSpeed); 238 stepperA -> applySpeedAcceleration(); 239 stepperA -> runForward(); 240 } else if (out < 0) { 241 int newSpeed = map(abs(out), 0, 500, 1000, closedSpeed); 242 stepperA -> setSpeedInUs(newSpeed); 243 stepperA -> applySpeedAcceleration(); 244 stepperA -> runBackward(); 245 } 246 } 247 } 248 249 void stepperOpenLoop(float openSteps, float moveSpeed) { 250 int actualSteps_relative = openSteps * formula; 251 int actualAngle = totalAngle * formula; 252 253 openLoopRunning = true; 254 closedLoopRunning = true; 255 closedLoopStart = true; 256 257 stepperA -> setCurrentPosition(actualAngle); 258 259 // Open loop runs faster than closed loop error correction 260 stepperA -> setSpeedInUs(moveSpeed); 261 stepperA -> setAcceleration(openAccel); 262 stepperA -> applySpeedAcceleration(); 263 stepperA -> moveTo(actualSteps_relative); 264 } 265 266 void receiveEvent(int byteCount) { 267 int i; 268 containsComma = false; 269 270 for (i = 0; i < byteCount; i++) { 271 readStringC[i] = Wire1.read(); // receive byte as a character 272 if (readStringC[i] == ','){ 273 containsComma = true; 274 } 275 } 276 readStringC[i] = '\0'; // assume it is a string, add zero-terminator 277 flagRx = true; // set flag, we received something. 278 } 279 280 void loop() { 281 limitSwitchOne_state = digitalRead(SWITCH_ONE_PIN); 282 283 if (isInitiatingHoming == true) { 284 if (limitSwitchOne_state == HIGH){ 285 switchPressed = true; 286 } 287 288 if (switchPressed == true) { 289 if (offsetTriggered == false){ 290 stepperA -> stopMove(); 291 stepperA -> setCurrentPosition(0); 292 293 stepperA -> moveTo((-homeOffset * gearReduction) * formula); 294 offsetTriggered = true; 295 }else{ 296 if (stepperA -> isRunning() == false){ 297 ESP.restart(); //Don't bother with fancy code. Restart the damn thing! 298 } 299 } 300 }else{ 301 stepperA -> setSpeedInUs(homingSpeed); // the parameter is us/step !!! 302 stepperA -> setAcceleration(homingAccel); 303 stepperA -> applySpeedAcceleration(); 304 stepperA -> runForward(); 305 } 306 } else { 307 getEncoderReading(); 308 309 if (flagRx == true) { //Verify that the variable contains information 310 311 if (readStringC[0] == 'm') { // Manual user data feed 312 readStringC[0] = '0'; 313 bool isNegative = false; 314 float moveAngle; 315 float moveSpeed; 316 317 if (readStringC[1] == '-') { 318 readStringC[1] = '0'; 319 isNegative = true; 320 } 321 322 if (containsComma == true){ 323 char *moveStrings[6]; // an array of pointers to the pieces of the above array after strtok() 324 char *ptr = NULL; 325 int moveIndex = 0; 326 327 ptr = strtok(readStringC, ","); // delimiter 328 while (ptr != NULL) { 329 moveStrings[moveIndex] = ptr; 330 moveIndex++; 331 ptr = strtok(NULL, ","); 332 } 333 moveAngle = atof(moveStrings[0]); 334 moveSpeed = atof(moveStrings[1]); 335 }else{ 336 moveAngle = atof(readStringC); 337 moveSpeed = openSpeed; 338 } 339 340 //Check for negative numbers 341 if (isNegative == true){ 342 Raw_Input = moveAngle; // here input data is stored in float form 343 User_Input = -Raw_Input; 344 } else { 345 User_Input = moveAngle; // here input data is stored in float form 346 } 347 348 if (isContinuousRotation == false) { 349 float Abs_Input = abs(User_Input); 350 User_Input = -Abs_Input; 351 } 352 //Apply Gear Reduction and move angle with speed 353 User_Input = User_Input * gearReduction; 354 stepperOpenLoop(User_Input, moveSpeed); 355 356 } else if (readStringC[0] == 's') { 357 readStringC[0] = '0'; 358 openSpeed = atoi(readStringC); 359 360 //} else if (readStringC[0] == 'c') { 361 // readStringC[0] = '0'; 362 // if (atoi(readStringC) <= 1200) { 363 // driverCurrent = atoi(readStringC); 364 // driver.rms_current(driverCurrent); 365 // } 366 367 } else if (readStringC[0] == 'h') { 368 isInitiatingHoming = true; 369 } 370 flagRx = false; 371 } 372 373 if (stepperA -> isRunning() == false) { 374 openLoopRunning = false; 375 } 376 377 if (openLoopRunning == false) { 378 if (closedLoopRunning == true) { 379 setpoint = User_Input; //PID while work to achive this value consider as SET value 380 input = totalAngle; // data from encoder consider as a Process value 381 myPID.Compute(); // calculate new output 382 stepperClosedLoop(output); 383 } 384 } 385 } 386 }