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