diff --git a/ATtiny85/ATtiny85.ino b/ATtiny85/ATtiny85.ino new file mode 100644 index 0000000..c8224bf --- /dev/null +++ b/ATtiny85/ATtiny85.ino @@ -0,0 +1,235 @@ +/* + * Final setting: + * 0 - motor PWM1 / RX during setup + * 1 - motor PWM2 / TX during setup + * 2 - step input + * 3 - encoder A + * 4 - encoder B + * 5 - direction + * + * To fix fuses after final program is uploaded: + * avrdude -v -v -v -v -pattiny85 -cusbasp -Pusb -U lfuse:w:0xc1:m -U hfuse:w:0x5c:m -U efuse:w:0xff:m + * + * For Tiny Debug Serial use boards definition from + * https://storage.googleapis.com/google-code-archive-downloads/v2/code.google.com/arduino-tiny/arduino-tiny-0150-0020.zip + * + * Should you need to get back to programming (like adjust motor parameters), + * use HVSP programming - I used Arduino Nano with sketch from here: + * https://arduinodiy.wordpress.com/2015/05/16/high-voltage-programmingunbricking-for-attiny/ +*/ +#include "avr/interrupt.h" +#include + +volatile long encoder0Pos = 0; // real position from encoder +volatile long target1 = 0; // destination location at any moment +volatile int moveCoef = 1; +static byte newPort; + +#define M1 0 // only PWM capable pins on Tiny is 0 and 1 +#define M2 1 + +// please adjust these 3 values for your motor!!! +// Y axis config: +// const double kp = 12, ki = 8, kd = 0.02; +// X axis config: +const double kp = 12, ki = 4, kd = 0.02; +// Z axis config +// const double kp = 12, ki = 8, kd = 0.02; +#undef stepKoef +// #define stepKoef 5 +//#undef minSearch +#define minSearch + +double input = 0, output = 0, setpoint = 0; +PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); + +boolean autoReport = true; +unsigned long previousMillis = 0; // will store last time LED was updated +long lastEncPos = 0; // will store the encoded value when last max output was measured +unsigned long curTime = 0UL; // will store current time to avoid multiple millis() calls +unsigned long lastSafeCheck = 0; // will store last value when 255 output was measured +unsigned long motorSafe = 4000UL; // will store the interval to protect the motor - 4seconds +int lastMax = 0; // have to also store the max to avoid +-255 values + + +// when using ATtiny85 on 16MHz, serial pin is 2, not 3!!! + +void setup() { + // real code + pinMode(2, INPUT); // used for step interrupt + pinMode(5, INPUT); // used for step direction + // Setting the PWM pins as output + pinMode(M1, OUTPUT); + pinMode(M2, OUTPUT); + // Set the PWM frequencies + TCCR0A = TCCR0A & 0b11111000 | 0x01; + TCCR0B = TCCR0B & 0b11111000 | 0x01; + + // interrupts + GIMSK = 0b00100000; // turns on pin change interrupts + PCMSK = 0b00011100; // turn on interrupts on pins PB3 and PB4 + sei(); + + getMinimal(); + + myPID.SetMode(AUTOMATIC); + myPID.SetSampleTime(1); + myPID.SetControllerDirection((moveCoef > 0)?DIRECT:REVERSE); + myPID.SetOutputLimits(-255, 255); +} + +void loop() { + curTime = millis(); + input = encoder0Pos; + setpoint = target1; + myPID.Compute(); + + pwmOut(output); + + /* Motor safe code */ + if (abs(output) == 255) { + if (lastSafeCheck == 0) { + lastSafeCheck = curTime; + lastEncPos = encoder0Pos; + } else { + if (lastEncPos != encoder0Pos) { + lastSafeCheck = 0; + } else { + if (curTime - motorSafe > lastSafeCheck) { + // we have to protect the motor - looks like even with max output we are not moving + // we will set the target1 to current position to stop output + target1 = encoder0Pos - (output / 255 * 5); + lastEncPos = 0; + lastSafeCheck = 0; + } + } + } + } else { + lastSafeCheck = 0; + lastEncPos = 0; + } +} + + +// interrupt handling - have to select which triggered!!! +const int QEM [16] = {0, -1, 1, 2, 1, 0, 2, -1, -1, 2, 0, 1, 2, 1, -1, 0}; // Quadrature Encoder Matrix + +ISR(PCINT0_vect) +{ + byte oldInput, newInput, oldPort; + // reading port status and storing it + oldPort = newPort; + newPort = PINB; + + // determining if encoder position changed + newInput = (newPort & 0b00011000) >> 3; + oldInput = (oldPort & 0b00011000) >> 3; + if (newInput != oldInput) { + // we have change in Encoder position! + encoder0Pos += QEM[oldInput * 4 + newInput]; + } + + // determining if step input occured + // let's check only the step pin, + // we'll care about direction later + newInput = newPort & 0b0100; + oldInput = oldPort & 0b0100; + + // we have to have change comparing to previous state + // and we need raising change on input value for step + // here I do 10 steps per impusle!!! + if ((newInput != 0) && (oldInput == 0)) { + // we have step! + #ifndef stepKoef + target1 += (newPort & 0b100000) ? -1 : 1; + #else + target1 += (newPort & 0b100000) ? -1 * stepKoef : stepKoef; + #endif + } +} + +void pwmOut(int out) { + if (out < 0) { + analogWrite(M2, 255); + analogWrite(M1, 255 - abs(out)); + } + else { + analogWrite(M2, 255 - abs(out)); + analogWrite(M1, 255); + } +} + + +void getMinimal() { + + // goal is to: + // 1. measure minimal speed + // 2.a if we have no encoder input, perhaps we are at the end + // of possible movement track and we need to reverse + // 2.b if we have no encoder input after reverse, we need to + // stop everything - either the motor is disconnected + // or the encoder is and whatever we send can cause damage + // 3. verify encoder direction - to ensure positive movement + // of encoder input is also positive direction of motor + // 4. if we did not have a movement, the motor is disconnected + // and we are probably in a setup mode - we need to turn on + // the serial on respective pins + + // first, let's find minimum moving speed + int minSpeed = minSpd(1); + + // did we moved? + // if we did not move, we have to try the other way + // perhaps we were at the end of the path + // and we need to go back + if (encoder0Pos == 0) minSpeed = minSpd(-1); + + // stop the motor + pwmOut(0); + + // did we moved now? + // if we did not, we have to end - probably we need to turn on the serial + // port to debug, so we do it and quit the setup routine + if (encoder0Pos == 0) { + // Fatal error + // no input from encoder + // will halt + while (1) { + pwmOut(125); + delay(200); + pwmOut(-125); + delay(200); + } + } + + // we moved, variable i has the minimum moving speed + // we need to verify if the encoder is aligned with + // the direction of the motor + moveCoef = (encoder0Pos / abs(encoder0Pos)) * (minSpeed / abs(minSpeed)); + + #ifdef minSearch + // now move to minimum position + minSpeed = abs(minSpeed) + (255 - abs(minSpeed)) / 3; + long encoderOld = 0; + pwmOut(-minSpeed * (moveCoef)); + while (encoderOld != encoder0Pos) { + encoderOld = encoder0Pos; + delay(50); + } + pwmOut(0); + encoder0Pos = -75; + #ifdef stepKoef + encoder0Pos = -75 * stepKoef; + #endif + #endif +} + +int minSpd(int dirSpd) { + int minSpeed = 0; + while (( abs(minSpeed) <= 255) && (abs(encoder0Pos) < 5)) { + pwmOut(minSpeed * dirSpd); + delay(10); + minSpeed += 5; + } + return (minSpeed * dirSpd); +} diff --git a/PIDtunningTool.pde b/PIDtunningTool.ino similarity index 100% rename from PIDtunningTool.pde rename to PIDtunningTool.ino diff --git a/README.md b/README.md index a68f3a3..e213711 100644 --- a/README.md +++ b/README.md @@ -1,39 +1,28 @@ # dcservo -by misan +For original documentation, please, visit misan/dcservo. -This project uses and Arduino (or similar) to create a closed-loop position control for a DC motor to act -as a replacement of a stepper motor and its drive electronics. In order to be compatible with stepper logic -controller accepts two inputs STEP and DIRECTION so an external trajectory controller can operate the motor -as it would do with a stepper. +#ATtiny85 version +I added some modifications. -This code depends on the excellent Arduino PID library by Brett Beauregard: https://github.com/br3ttb/Arduino-PID-Library/ +#1. Determining the direction of the motor +As I was often modifying my design, I tend to plug in the motor with different polarity causing madness every time I tried to do first movement. +Now, when the code starts, it will try to move the motor and find out in which direction does it move in correlation to the encoder input. +If it finds no encoder inputs, it will consider either disconnected motor or disconnected encoder - for both cases the "error behavior" is simply to move the motor back-and-forth to signalize this problem. -For easy configuration of PID parameters, serial port communication is used when adjusting them. PID values can -be stored in EEPROM. +#2. Determining minimum movement speed and minimal position +As soon as I determine the motor direction, I proceed forward and try to find minimum position of the current axis. What I do is I move the motor in reverse direction with a small multiplier of minimal movement speed until I reach a point, where the encoder no longer receives input - this means I've hit physical end of the path. +At that moment I will reset the position to -75 (and not to zero) - this will allow to set the "real life" zero slightly above the minimal position of the axis to avoid loud bang everytime user wants to move to 0 position. The -75 is hardcoded for now and works OK for my current usage, but feel free to change it either to 0 or any other number you find OK for your printer. -Servostrap project and Michael Ball's work sparked my curiosity to go create a simple solution for anyone to use. +#3. Motor protection +I am monitoring the encoder input within regular time interval. If there is a situation where I am sending full power to the motors and the encoder position did not change for more then the predefined interval, I am trying to stop the motion. This will prevent situations whete the motor hits some problem (e.g. maximum axis position, huge obstacle, ...) and is unable to move, while the firmware still feeds full power - the motor would overheat and/or cause other significant damages. -I want to thank Brook Drum for his support on this project that started in here: https://www.youmagine.com/designs/dc-motor-closed-loop-control-software +#4. Removal of serial debug +Due to lack of pins on ATtiny85 and lack of storage, too, it made very little sense to keep the serial communication code and EEPROM code. Therefore I removed it and hardcoded the values for PID. You can use original code from Misan and his PID tuning tool to tune your motor and then use the final values within this sketch. -Thanks to Mauro Manco for providing a NodeMCU. Now there is a version than can work with a ESP12E module, with support to accept commands over Wifi. Please note EEPROM storage code will not work in this platform :-( +#5. Converting values to LONG +In some cases the 32767 values of encoder position and target step may not be enough. This is especially the case with geared motors and high resolution encoders. Therefore I moved the code to LONG, providing enough room for all situations. -AMS provided free samples of magnetic encoder. Now the AS6500 encoder is supported for 12-bit/revolution resolution. - -# usage -
-Available serial commands: (lines end with CRLF or LF) 
-P123.34 sets proportional term to 123.34
-I123.34 sets integral term to 123.34
-D123.34 sets derivative term to 123.34
-? prints out current encoder, output and setpoint values
-X123 sets the target destination for the motor to 123 encoder pulses
-T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that
-Q will print out the current values of P, I and D parameters
-W will store current values of P, I and D parameters into EEPROM
-H will print this help message again
-A will toggle on/off showing regulator status every second
-
- -#tuning tool -Written in Processing it allows you to see graphically the PID response to a step input while you can tune it by pressing capital P, I and D keys to increase values or p, i and d to lower them. -![Screenshot](http://i.imgur.com/3c8WySu.png "Tuning tool") +#6. ATtiny85 specifics +You have to use the avrdude code to fix the reset pin - otherwise your driver will have a reset situation whenever there is a step input. +To restore the original status - e.g. when reprogramming is needed, I provided a link to HVSP fuse reset project done by Arduino, 1 NPN transistor and 6 resistors. +Also, note you SHOULD NOT use the attinycore as the core base for the tiny - for some reason (I did not investigate for now) it does not work and compiled code will not execute the way it should. Use this URL to get the needed tiny core into Arduino IDE: http://highlowtech.org/?p=1695 diff --git a/dcservo.ino b/dcservo.ino index 9a9a601..caa9eed 100644 --- a/dcservo.ino +++ b/dcservo.ino @@ -1,183 +1,336 @@ - -/* This one is not using any PinChangeInterrupt library */ - /* - This program uses an Arduino for a closed-loop control of a DC-motor. + This program uses an Arduino for a closed-loop control of a DC-motor. Motor motion is detected by a quadrature encoder. Two inputs named STEP and DIR allow changing the target position. Serial port prints current position and target position every second. Serial input can be used to feed a new location for the servo (no CR LF). - + Pins used: Digital inputs 2 & 8 are connected to the two encoder signals (AB). Digital input 3 is the STEP input. Analog input 0 is the DIR input. Digital outputs 9 & 10 control the PWM outputs for the motor (I am using half L298 here). - - - Please note PID gains kp, ki, kd need to be tuned to each different setup. + Please note PID gains kp, ki, kd need to be tuned to each different setup. */ #include #include + #define encoder0PinA 2 // PD2; #define encoder0PinB 8 // PC0; #define M1 9 #define M2 10 // motor's PWM outputs -byte pos[1000]; int p=0; +int pos[500]; int p = 0; +volatile int backlash = 0; // grabbed from SingularitySurfer's git + +double kp = 16, ki = 16, kd = 0.10; +double input = 0, output = 0, setpoint = 0; + +PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); -double kp=3,ki=0,kd=0.0; -double input=0, output=0, setpoint=0; -PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT); volatile long encoder0Pos = 0; -boolean auto1=false, auto2=false,counting=false; +volatile int moveCoef = 1; +boolean setupDone = false; + +boolean auto1 = false, auto2 = false, counting = false; long previousMillis = 0; // will store last time LED was updated +long lastEncPos = 0; // will store the target value when last max output was measured +unsigned long curTime = 0UL; // will store current time to avoid multiple millis() calls +unsigned long lastSafeCheck = 0; // will store last value when 255 output was measured +unsigned long motorSafe = 1000UL; // will store the interval to protect the motor - 2seconds +int lastMax = 0; // have to also store the max to avoid +-255 values -long target1=0; // destination location at any moment +long target1 = 0; // destination location at any moment //for motor control ramps 1.4 bool newStep = false; bool oldStep = false; bool dir = false; -byte skip=0; +int skip = 0; // Install Pin change interrupt for a pin, can be called multiple times -void pciSetup(byte pin) +void pciSetup(byte pin) { - *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin - PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt - PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group + *digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin + PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt + PCICR |= bit (digitalPinToPCICRbit(pin)); // enable interrupt for the group } -void setup() { - pinMode(encoder0PinA, INPUT); - pinMode(encoder0PinB, INPUT); +void setup() { + pinMode(encoder0PinA, INPUT); + pinMode(encoder0PinB, INPUT); pciSetup(encoder0PinB); attachInterrupt(0, encoderInt, CHANGE); // encoder pin on interrupt 0 - pin 2 - attachInterrupt(1, countStep , RISING); // step input on interrupt 1 - pin 3 + attachInterrupt(1, countStep , RISING); // step input on interrupt 1 - pin 3 TCCR1B = TCCR1B & 0b11111000 | 1; // set 31Kh PWM Serial.begin (115200); help(); recoverPIDfromEEPROM(); - //Setup the pid + + Serial.println("Measuring minimal speed"); + // first, let's find minimum moving speed + int minSpeed = 0; + while (( minSpeed <= 255) && (abs(encoder0Pos) < 5)) { + pwmOut(minSpeed); + delay(10); + minSpeed += 5; + } + + // did we moved? + if (encoder0Pos == 0) { + // we did not move, we have to try the other way + // perhaps we were at the end of the path + // and we need to go back + minSpeed = 0; + while (( minSpeed >= -255) && (abs(encoder0Pos) < 5)) { + pwmOut(minSpeed); + delay(10); + minSpeed -= 5; + } + } + + // stop the motor + pwmOut(0); + + // did we moved now? + // if we did not, we have to end - either the encoder is not working + // or we don't have enough power to move + // therefore we will freeze + if (encoder0Pos == 0) { + Serial.println("We have a problem - no movement!!!"); + while (1) delay(1); + } + + Serial.print("Minimal speed: "); + Serial.println(minSpeed); + Serial.print("Encoder position: "); + Serial.println(encoder0Pos); + + // we moved, variable i has the minimum moving speed + // we need to verify if the encoder is aligned with + // the direction of the motor + moveCoef = (encoder0Pos / abs(encoder0Pos)) * (minSpeed / abs(minSpeed)); + + Serial.print("Movement coeficient: "); + Serial.println(moveCoef); + + // to ensure we will move + // we will increase the minimal moving speed + // by 25% of the difference to maximum speed + minSpeed = abs(minSpeed) + (255 - abs(minSpeed)) / 5; + + Serial.print("Adjusted movement speed: "); + Serial.println(minSpeed); + + // now we have reasonable speed to move but not break things + // we will find minimum and maximum + // move in negative direction by minimum speed + // until encoder stops + long encoderOld = 0; + pwmOut(-minSpeed); + while (encoderOld != encoder0Pos) { + encoderOld = encoder0Pos; + delay(50); + } + pwmOut(0); + + Serial.print("Measured minimum before reset to 0: "); + Serial.println(encoder0Pos); + + encoder0Pos = 0; + pwmOut(minSpeed); + while (encoderOld != encoder0Pos) { + encoderOld = encoder0Pos; + delay(50); + } + pwmOut(0); + + Serial.print("Measured maximum: "); + Serial.println(encoder0Pos); + + //Setup the pid myPID.SetMode(AUTOMATIC); myPID.SetSampleTime(1); - myPID.SetOutputLimits(-255,255); -} - -void loop(){ - input = encoder0Pos; - setpoint=target1; - myPID.Compute(); - if(Serial.available()) process_line(); // it may induce a glitch to move motion, so use it sparingly - pwmOut(output); - if(auto1) if(millis() % 3000 == 0) target1=random(2000); // that was for self test with no input from main controller - if(auto2) if(millis() % 1000 == 0) printPos(); - //if(counting && abs(input-target1)<15) counting=false; - if(counting && (skip++ % 5)==0 ) {pos[p]=encoder0Pos; if(p<999) p++; else counting=false;} + myPID.SetOutputLimits(-255, 255); +} + +void loop() { + curTime = millis(); + input = encoder0Pos; + setpoint = target1; + myPID.Compute(); + if (Serial.available()) process_line(); // it may induce a glitch to move motion, so use it sparingly + pwmOut(output); + if (auto1) if (curTime % 3000 == 0) target1 = random(2000); // that was for self test with no input from main controller + if (auto2) if (curTime % 1000 == 0) printPos(); + //if(counting && abs(input-target1)<15) counting=false; + if (counting && (skip++ % 10) == 0 ) { + pos[p] = encoder0Pos; + if (p < 499) p++; + else counting = false; + } + /* Motor safe code */ + if ((output == 255) || (output == -255)) { + if (lastSafeCheck == 0) { + lastSafeCheck = curTime; + lastEncPos = encoder0Pos; + } else { + if (lastEncPos != encoder0Pos) { + lastSafeCheck = 0; + } else { + if (curTime - motorSafe > lastSafeCheck) { + // we have to protect the motor - looks like even with max output we are not moving + // we will set the target to current position to stop output + Serial.println(F(" Will decrease output!!!")); + target1 = encoder0Pos - (output / 255 * 5); + lastEncPos = 0; + lastSafeCheck = curTime; + } + } + } + } else { + lastSafeCheck = 0; + lastEncPos = 0; + } + } void pwmOut(int out) { - if(out<0) { analogWrite(M1,0); analogWrite(M2,abs(out)); } - else { analogWrite(M2,0); analogWrite(M1,abs(out)); } + out = out * moveCoef; + if (out < 0) { + analogWrite(M1, 0); + analogWrite(M2, abs(out)); + } + else { + analogWrite(M2, 0); + analogWrite(M1, abs(out)); } +} + +const int QEM [16] = {0, -1, 1, 2, 1, 0, 2, -1, -1, 2, 0, 1, 2, 1, -1, 0}; // Quadrature Encoder Matrix -const int QEM [16] = {0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0}; // Quadrature Encoder Matrix static unsigned char New, Old; ISR (PCINT0_vect) { // handle pin change interrupt for D8 Old = New; - New = (PINB & 1 )+ ((PIND & 4) >> 1); // - encoder0Pos+= QEM [Old * 4 + New]; + New = (PINB & 1 ) + ((PIND & 4) >> 1); // + encoder0Pos += QEM [Old * 4 + New]; } void encoderInt() { // handle pin change interrupt for D2 Old = New; - New = (PINB & 1 )+ ((PIND & 4) >> 1); // - encoder0Pos+= QEM [Old * 4 + New]; + New = (PINB & 1 ) + ((PIND & 4) >> 1); // + encoder0Pos += QEM [Old * 4 + New]; } -void countStep(){ if (PINC&B0000001) target1--;else target1++; } // pin A0 represents direction +void countStep() { + if (PINC & B0000001) { + target1--; + } + else { + target1++; + } +} // pin A0 represents direction + void process_line() { - char cmd = Serial.read(); - if(cmd>'Z') cmd-=32; - switch(cmd) { - case 'P': kp=Serial.parseFloat(); myPID.SetTunings(kp,ki,kd); break; - case 'D': kd=Serial.parseFloat(); myPID.SetTunings(kp,ki,kd); break; - case 'I': ki=Serial.parseFloat(); myPID.SetTunings(kp,ki,kd); break; - case '?': printPos(); break; - case 'X': target1=Serial.parseInt(); p=0; counting=true; for(int i=0; i<300; i++) pos[i]=0; break; - case 'T': auto1 = !auto1; break; - case 'A': auto2 = !auto2; break; - case 'Q': Serial.print("P="); Serial.print(kp); Serial.print(" I="); Serial.print(ki); Serial.print(" D="); Serial.println(kd); break; - case 'H': help(); break; - case 'W': writetoEEPROM(); break; - case 'K': eedump(); break; - case 'R': recoverPIDfromEEPROM() ; break; - case 'S': for(int i=0; i 'Z') cmd -= 32; + switch (cmd) { + case 'P': kp = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'D': kd = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'I': ki = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case '?': printPos(); break; + case 'X': target1 = Serial.parseInt(); p = 0; counting = true; for (int i = 0; i < 500; i++) pos[i] = 0; break; + case 'T': auto1 = !auto1; break; + case 'A': auto2 = !auto2; break; + case 'Q': Serial.print("P="); Serial.print(kp); Serial.print(" I="); Serial.print(ki); Serial.print(" D="); Serial.println(kd); break; + case 'H': help(); break; + case 'W': writetoEEPROM(); break; + case 'K': eedump(); break; + case 'R': recoverPIDfromEEPROM() ; break; + case 'S': for (int i = 0; i < p; i++) Serial.println(pos[i]); break; + case 'B': backlash = Serial.parseFloat(); + } + while (Serial.read() != 10); // dump extra characters till LF is seen (you can use CRLF or just LF) } void printPos() { - Serial.print(F("Position=")); Serial.print(encoder0Pos); Serial.print(F(" PID_output=")); Serial.print(output); Serial.print(F(" Target=")); Serial.println(setpoint); + Serial.print(F("Position=")); + Serial.print(encoder0Pos); + Serial.print(F(" PID_output=")); + Serial.print(output); + Serial.print(F(" Target=")); + Serial.print(setpoint); + Serial.print(F(" LastCheck=")); + Serial.print(lastSafeCheck); + Serial.print(F(" LastEnc=")); + Serial.print(lastEncPos); + Serial.print(F(" Coeficient=")); + Serial.println(moveCoef); + } + void help() { - Serial.println(F("\nPID DC motor controller and stepper interface emulator")); - Serial.println(F("by misan")); - Serial.println(F("Available serial commands: (lines end with CRLF or LF)")); - Serial.println(F("P123.34 sets proportional term to 123.34")); - Serial.println(F("I123.34 sets integral term to 123.34")); - Serial.println(F("D123.34 sets derivative term to 123.34")); - Serial.println(F("? prints out current encoder, output and setpoint values")); - Serial.println(F("X123 sets the target destination for the motor to 123 encoder pulses")); - Serial.println(F("T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that")); - Serial.println(F("Q will print out the current values of P, I and D parameters")); - Serial.println(F("W will store current values of P, I and D parameters into EEPROM")); - Serial.println(F("H will print this help message again")); - Serial.println(F("A will toggle on/off showing regulator status every second\n")); + Serial.println(F("\nPID DC motor controller and stepper interface emulator")); + Serial.println(F("by misan")); + Serial.println(F("Available serial commands: (lines end with CRLF or LF)")); + Serial.println(F("P123.34 sets proportional term to 123.34")); + Serial.println(F("I123.34 sets integral term to 123.34")); + Serial.println(F("D123.34 sets derivative term to 123.34")); + Serial.println(F("? prints out current encoder, output and setpoint values")); + Serial.println(F("X123 sets the target destination for the motor to 123 encoder pulses")); + Serial.println(F("T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that")); + Serial.println(F("Q will print out the current values of P, I and D parameters")); + Serial.println(F("W will store current values of P, I and D parameters into EEPROM")); + Serial.println(F("H will print this help message again")); + Serial.println(F("A will toggle on/off showing regulator status every second\n")); + Serial.println(F("B123 will set backlash compensation to 123 steps per change in direction")); + Serial.print(F(" Backlash Value: ")); Serial.println(backlash); } void writetoEEPROM() { // keep PID set values in EEPROM so they are kept when arduino goes off - eeput(kp,0); - eeput(ki,4); - eeput(kd,8); - double cks=0; - for(int i=0; i<12; i++) cks+=EEPROM.read(i); - eeput(cks,12); + eeput(kp, 0); + eeput(ki, 4); + eeput(kd, 8); + double cks = 0; + for (int i = 0; i < 12; i++) cks += EEPROM.read(i); + eeput(cks, 12); Serial.println("\nPID values stored to EEPROM"); //Serial.println(cks); } void recoverPIDfromEEPROM() { - double cks=0; + double cks = 0; double cksEE; - for(int i=0; i<12; i++) cks+=EEPROM.read(i); - cksEE=eeget(12); + for (int i = 0; i < 12; i++) cks += EEPROM.read(i); + cksEE = eeget(12); //Serial.println(cks); - if(cks==cksEE) { + if (cks == cksEE) { Serial.println(F("*** Found PID values on EEPROM")); - kp=eeget(0); - ki=eeget(4); - kd=eeget(8); - myPID.SetTunings(kp,ki,kd); + kp = eeget(0); + ki = eeget(4); + kd = eeget(8); + myPID.SetTunings(kp, ki, kd); } else Serial.println(F("*** Bad checksum")); } void eeput(double value, int dir) { // Snow Leopard keeps me grounded to 1.0.6 Arduino, so I have to do this :-( char * addr = (char * ) &value; - for(int i=dir; i +#include +#include // support for I2C encoder + + +const int Step = 14; +const int M1 = 9; +const int M2 = 10; +const int DIR = 4; +const int PWM_MOT = 15; + +byte pos[1000]; int p = 0; +byte maxPower = 255; +double kp = 12, ki = 4, kd = 0.01; +double input = 0, output = 0, setpoint = 0; +PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); +//volatile long encoder0Pos = 0; +long pos1; + +boolean auto1 = false, auto2 = true, counting = false; +long previousMillis = 0; // will store last time LED was updated + +long target1 = 0; // destination location at any moment + +//for motor control ramps 1.4 +byte skip = 0; +int angle, diff; +double before = 0; + + +void setup() { + Serial.begin (115200); + pinMode(13, OUTPUT); + pinMode(M1, OUTPUT); + pinMode(M2, OUTPUT); + + TCCR1B = TCCR1B & 0b11111000 | 1; // set 31Kh PWM + + recoverPIDfromEEPROM(); + //Setup the pid + myPID.SetMode(AUTOMATIC); + myPID.SetSampleTime(1); + myPID.SetOutputLimits((-1) * maxPower, maxPower); + + Wire.begin(); // start I2C driver code + help(); +} + +void loop() { + angle = readTwoBytes() >> 2; //analogRead(A0); // encoder0Pos; + // process encoder rollover + diff = angle - before; + if (diff < -768) pos1 += 1024; + else if (diff > 768) pos1 -= 1024; + before = angle; + input = pos1 + angle; + + //input = encoder0Pos; + setpoint = target1; + myPID.Compute(); + if (Serial.available()) process_line(); + pwmOut(output); + if (auto1) if (millis() % 3000 == 0) target1 = random(2000); // that was for self test with no input from main controller + if (auto2) if (millis() % 1000 == 0) printPos(); + if (counting && abs(input - target1) < 15) counting = false; + if (counting && (skip++ % 2) == 0 ) { + pos[p] = input; + if (p < 999) p++; + else counting = false; + } +} + +word readTwoBytes() +{ + word retVal = -1; + + /* Read Low Byte */ + Wire.beginTransmission(0x36); + Wire.write(0x0d); + Wire.endTransmission(); + Wire.requestFrom(0x36, 1); + while (Wire.available() == 0); + int low = Wire.read(); + + /* Read High Byte */ + Wire.beginTransmission(0x36); + Wire.write(0x0c); + Wire.endTransmission(); + Wire.requestFrom(0x36, 1); + + while (Wire.available() == 0); + + word high = Wire.read(); + + high = high << 8; + retVal = high | low; + + return retVal; +} + + +void pwmOut(int out) { + if (out > 0) { + digitalWrite(M1, 0); + analogWrite(M2, out); + } + else { + analogWrite(M1, -out); + digitalWrite(M2, 0); + } +} + +void printPos() { + Serial.print(F("Position=")); Serial.print(input); Serial.print(F(" PID_output=")); Serial.print(output); Serial.print(F(" Target=")); Serial.print(setpoint); Serial.print(F(" Before=")); Serial.print(before); Serial.print(F(" Angle=")); Serial.println(angle); +} + +void help() { + Serial.println(F("\nPID DC motor controller and stepper interface emulator")); + Serial.println(F("by misan - porting cured by Exilaus")); + Serial.println(F("Available serial commands: (lines end with CRLF or LF)")); + Serial.println(F("P123.34 sets proportional term to 123.34")); + Serial.println(F("I123.34 sets integral term to 123.34")); + Serial.println(F("D123.34 sets derivative term to 123.34")); + Serial.println(F("M123 sets maximum motor power")); + Serial.println(F("? prints out current encoder, output and setpoint values")); + Serial.println(F("X123 sets the target destination for the motor to 123 encoder pulses")); + Serial.println(F("T starts a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that")); + Serial.println(F("Q prints out the current values of P, I and D parameters")); + Serial.println(F("W stores current values of P, I and D parameters into EEPROM")); + Serial.println(F("H prints this help message again")); + Serial.println(F("A toggles on/off showing regulator status every second\n")); +} + + +void eeput(double value, int dir) { // Snow Leopard keeps me grounded to 1.0.6 Arduino, so I have to do this :-( + char * addr = (char * ) &value; + for (int i = dir; i < dir + 4; i++) EEPROM.write(i, addr[i - dir]); +} + + +void writetoEEPROM() { // keep PID set values in EEPROM so they are kept when arduino goes off + eeput(kp, 0); + eeput(ki, 4); + eeput(kd, 8); + double cks = 0; + for (int i = 0; i < 12; i++) cks += EEPROM.read(i); + eeput(cks, 12); + Serial.println("\nPID values stored to EEPROM"); + //Serial.println(cks); +} + + +double eeget(int dir) { // Snow Leopard keeps me grounded to 1.0.6 Arduino, so I have to do this :-( + double value; + char * addr = (char * ) &value; + for (int i = dir; i < dir + 4; i++) addr[i - dir] = EEPROM.read(i); + return value; +} + +void recoverPIDfromEEPROM() { + double cks = 0; + double cksEE; + for (int i = 0; i < 12; i++) cks += EEPROM.read(i); + cksEE = eeget(12); + //Serial.println(cks); + if (cks == cksEE) { + Serial.println(F("*** Found PID values on EEPROM")); + kp = eeget(0); + ki = eeget(4); + kd = eeget(8); + myPID.SetTunings(kp, ki, kd); + } + else Serial.println(F("*** Bad checksum")); +} + + +void eedump() { + for (int i = 0; i < 16; i++) { + Serial.print(EEPROM.read(i), HEX); + Serial.print(" "); + } Serial.println(); +} + +void process_line() { + char cmd = Serial.read(); + if (cmd > 'Z') cmd -= 32; + switch (cmd) { + case 'P': kp = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'D': kd = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'I': ki = Serial.parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'M': maxPower = Serial.parseInt(); myPID.SetOutputLimits((-1) * maxPower, maxPower); break; + case '?': printPos(); break; + case 'X': target1 = Serial.parseInt(); counting = true; for (int i = 0; i < p; i++) pos[i] = 0; p = 0; break; + case 'T': auto1 = !auto1; break; + case 'A': auto2 = !auto2; break; + case 'Q': Serial.print("P="); Serial.print(kp); Serial.print(" I="); Serial.print(ki); Serial.print(" D="); Serial.println(kd); break; + case 'H': help(); break; + case 'W': writetoEEPROM(); break; + case 'K': eedump(); break; + case 'R': recoverPIDfromEEPROM() ; break; + case 'S': for (int i = 0; i < p; i++) Serial.println(pos[i]); break; + } +} diff --git a/tiny85_v2/tiny85_v2.ino b/tiny85_v2/tiny85_v2.ino new file mode 100644 index 0000000..fde878e --- /dev/null +++ b/tiny85_v2/tiny85_v2.ino @@ -0,0 +1,286 @@ +/* + * Goal: Have DC motor with encoder + * driven by ATtiny85 and Step and Direction + * from RAMPS + * However, when starting, I want to determine a "DEBUG" + * condition and wnable serial port to set PID and parameters + * like enable (minimum and maximum search. + * + * Tiny setup: + * 1 - PB5 - Encoder A + * 2 - PB3 - Encoder B + * 3 - PB4 - DC PWM + * 4 - GND + * 5 - PB0 - Step Impulse / Serial RX + * 6 - PB1 - Step Direction / Serial TX + * 7 - PB2 - DC direction + * 8 - VCC + */ +#include "avr/interrupt.h" +#include + +// please adjust these 3 values for your motor!!! +double kp = 50, ki = 4, kd = 0.10; +double input = 0, output = 0, setpoint = 0; +PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); +// I'll use SetControllerDirection to set the direction later + +boolean auto2 = true; + +unsigned long previousMillis = 0; // will store last time LED was updated +long lastEncPos = 0; // will store the encoded value when last max output was measured +unsigned long curTime = 0UL; // will store current time to avoid multiple millis() calls +unsigned long lastSafeCheck = 0; // will store last value when 255 output was measured +unsigned long motorSafe = 4000UL; // will store the interval to protect the motor - 4seconds +int lastMax = 0; // have to also store the max to avoid +-255 values +boolean debug = false; + +volatile int encoder0Pos = 0; +volatile long target1 = 0; // destination location at any moment +volatile int moveCoef = 1; +static byte newPort; + +// Encoder table - get the result based on previous and current encoder output +const int QEM [16] = {0, -1, 1, 2, 1, 0, 2, -1, -1, 2, 0, 1, 2, 1, -1, 0}; // Quadrature Encoder Matrix + +void setup() { + setPWM(); + /* + pinMode(0, INPUT); // used for step interrupt + pinMode(1, INPUT); // used for step direction + if (digitalRead(0) && digitalRead(1)) { + */ + // we have debug condition (both step and dir = 1 during setup + // we will start serial on these pins instead of listening to interrupts + Serial.begin(115200); + Serial.println("Setup in debug mode!"); + //delay(5000); // wait 5s to disable the startup condition + debug = true; + //} + + // interrupts + GIMSK = 0b00100000; // turns on pin change interrupts + PCMSK = 0b00010100; // turn on interrupts on pins PB3 and PB4 + if (!debug) PCMSK |= 0b1; // if we are not debuging, we need step input, too + sei(); + + //getMinimal(); + + myPID.SetMode(AUTOMATIC); + myPID.SetSampleTime(1); + //myPID.SetControllerDirection((moveCoef > 0) ? DIRECT : REVERSE); + myPID.SetOutputLimits(-255, 255); + + if (debug) Serial.println("Setup done"); +} + +void loop() { + curTime = millis(); + input = encoder0Pos; + setpoint = target1; + myPID.Compute(); + + pwmOut(output); + /* Motor safe code */ + if (abs(output) == 255) { + if (lastSafeCheck == 0) { + lastSafeCheck = curTime; + lastEncPos = encoder0Pos; + } else { + if (lastEncPos != encoder0Pos) { + lastSafeCheck = 0; + } else { + if (curTime - motorSafe > lastSafeCheck) { + // we have to protect the motor - looks like even with max output we are not moving + // we will set the target1 to current position to stop output + target1 = encoder0Pos - (output / 255 * 5); + lastEncPos = 0; + lastSafeCheck = 0; + } + } + } + } else { + lastSafeCheck = 0; + lastEncPos = 0; + } + + if (auto2) if (curTime % 1000 == 0) printPos(); + if (Serial.available()) processLine(); +} + + +ISR(PCINT0_vect) +{ + byte oldInput, newInput, oldPort; + // reading port status and storing it + oldPort = newPort; + newPort = PINB; + + // determining if encoder position changed + // encoder connected to PB5 and PB3 + newInput = (newPort & 0b00101000) >> 3; + oldInput = (oldPort & 0b00101000) >> 3; + if (newInput != oldInput) { + // we have change in Encoder position! + // the math here is to fix the encoder position + // as I am interested in 3rd and 1st bit, I need to remove + // the second one + newInput = newInput >> 2 | (newInput & 1); + encoder0Pos += QEM[(oldInput >> 2 | (oldInput & 1)) * 4 + (newInput >> 2 | (newInput & 1))]; + } + + // determining if step input occured + // let's check only the step pin, + // we'll care about direction later + newInput = newPort & 0b01; + oldInput = oldPort & 0b01; + + // we have to have change comparing to previous state + // and we need raising change on input value for step + // the direction of step is determined by PB1 + if ((newInput != 0) && (oldInput == 0)) { + // we have step! + target1 += (newPort & 0b10) ? -1 : 1; + } +} + + +void pwmOut(int out) { + if (out < 0) { + digitalWrite(2, HIGH); + OCR1B = 255 + out; + } + else { + digitalWrite(2, LOW); + OCR1B = out; + } +} + +void setPWM() { + // PWM setup according + // http://matt16060936.blogspot.ch/2012/04/attiny-pwm.html + DDRB |= 1 << DDB4; // set the PB4 as output + DDRB |= 1 << DDB2; // set the PB2 as output (direction pin) + TCCR0A = 1 << COM0A0 | 2 << COM0B0 | 3 << WGM00; + // set the timers COM0A0 for enabling + // timers and the rest for timer B + // without COM0A0 the Timer1 will not work + TCCR1 = 0 << PWM1A | 0 << COM1A0 | 1 << CS10; + // this tells not to use pins OC0B and OC1A + // and use no prescaler + GTCCR = 1 << PWM1B | 2 << COM1B0; + // enable OC1B (PB4) + // and clears it to bottom + // I'll use OCR1B = x to set the PWM value +} + +void getMinimal() { + + // goal is to: + // 1. measure minimal speed + // 2.a if we have no encoder input, perhaps we are at the end + // of possible movement track and we need to reverse + // 2.b if we have no encoder input after reverse, we need to + // stop everything - either the motor is disconnected + // or the encoder is and whatever we send can cause damage + // 3. verify encoder direction - to ensure positive movement + // of encoder input is also positive direction of motor + // 4. if we did not have a movement, the motor is disconnected + // and we are probably in a setup mode - we need to turn on + // the serial on respective pins + + // first, let's find minimum moving speed + int minSpeed = minSpd(1); + + // did we moved? + // if we did not move, we have to try the other way + // perhaps we were at the end of the path + // and we need to go back + if (encoder0Pos == 0) minSpeed = minSpd(-1); + + // stop the motor + pwmOut(0); + + // did we moved now? + // if we did not, we have to end - probably we need to turn on the serial + // port to debug, so we do it and quit the setup routine + if (encoder0Pos == 0) { + Serial.println("Err"); + if (!debug) while (1) delay(1); + } + + // we moved, variable i has the minimum moving speed + // we need to verify if the encoder is aligned with + // the direction of the motor + moveCoef = (encoder0Pos / abs(encoder0Pos)) * (minSpeed / abs(minSpeed)); +} + +int minSpd(int dirSpd) { + int minSpeed = 0; + while (( abs(minSpeed) <= 255) & (abs(encoder0Pos) < 5)) { + pwmOut(minSpeed * dirSpd); + delay(10); + minSpeed += 5; + } + return (minSpeed * dirSpd); +} + + +void printPos() { + Serial.print("Pos="); + Serial.print(encoder0Pos); + Serial.print(" PID="); + Serial.print(output); + Serial.print(" Target="); + Serial.println(setpoint); +} + +void processLine() { + char cmd = Serial.read(); + if (cmd > 'Z') cmd -= 32; + switch (cmd) { + case 'P': kp = parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'D': kd = parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case 'I': ki = parseFloat(); myPID.SetTunings(kp, ki, kd); break; + case '?': printPos(); break; + //case 'H': help(); break; + // case 'X': target1 = parseInt(); p = 0; counting = true; for (int i = 0; i < 500; i++) pos[i] = 0; break; + case 'X': target1 = parseInt(); break; + case 'Q': Serial.print("P="); Serial.print(kp); Serial.print(" I="); Serial.print(ki); Serial.print(" D="); Serial.println(kd); break; + } + while (Serial.read() != 10); // dump extra characters till LF is seen (you can use CRLF or just LF) +} + +float parseFloat() { + float returnVal = 0; + int isDecimal = 0; + + while (Serial.available() > 0) { + char serialChar = Serial.read(); + if ((serialChar >= '0') & (serialChar <= '9')) { + returnVal *= 10; + if (isDecimal > 0) isDecimal *= 10; + returnVal += int(serialChar) - int('0'); + } + if (serialChar == '.') isDecimal = 1; + } + if (isDecimal > 0) returnVal /= isDecimal; + Serial.print("Fl:"); + Serial.println(returnVal); + return (returnVal); +} + +int parseInt() { + int returnVal = 0; + + while (Serial.available() > 0) { + char serialChar = Serial.read(); + if ((serialChar >= '0') & (serialChar <= '9')) { + returnVal *= 10; + returnVal += int(serialChar) - int('0'); + } + } + Serial.print("Int:"); + Serial.println(returnVal); + return (returnVal); +}