-
ISR Motion Control
12/16/2017 at 03:57 • 0 commentsISR Motion Control
Reworked the code to use an ISR (Interrupt Service Routine).
The ISR is pretty heavy duty but with the code optimisation it runs in less than 25 us. So it will run with a 16 MHz input clock (i.e. 31372.55 Hz interrupt rate). Still I slowed input clock down to 2 MHz for a 3921.57 Hz interrupt rate. As there is no ramp code, it is likely the stepper motors will miss steps if you try to go too fast.
Here is the updated code:
/* Simple 3 Axis Motion Controller =============================== Written by Alan Cooper (agp.cooper@gmail.com) This work is licensed under the Creative Commons Attribution-NonCommercial 2.5 License. This means you are free to copy and share the code (but not to sell it). */ // Motor controller (CNC board) pin mapping: #define DirX 2 #define DirY 3 #define DirZ 4 #define StepX 5 #define StepY 6 #define StepZ 7 #define Enable 8 // Active low #define Laser 12 // Turn laser on or off // Set motion (rate) steps per second unsigned long rate=1000; // Motor Controller direction settings bool xReverse=false; bool yReverse=true; bool zReverse=false; /* Motion ISR */ volatile long xNew=0; // The target X co-ordinate volatile long yNew=0; // The target Y co-ordinate volatile long zNew=0; // The target Z co-ordinate volatile long xCurrent=0; // The current X co-ordinate volatile long yCurrent=0; // The current Y co-ordinate volatile long zCurrent=0; // The current Z co-ordinate volatile long steps=-1; // Number of steps remaining in motion volatile unsigned int magic=0; // Magic number (used by ISR) volatile unsigned int phase=0; // Accumulator (used by ISR) ISR(TIMER2_OVF_vect) { static long stepX,stepY,stepZ; static long dx,dy,dz,ax,ay,az,sx,sy,sz,mx,my,mz; if (phase<0x8000) { phase+=magic; if (phase>=0x8000) { if (steps>0) { // Advance steppers stepX=0; stepY=0; stepZ=0; if ((ax>=ay)&&(ax>=az)) { if (my>=0) { my-=ax; stepY=sy; } if (mz>=0) { mz-=ax; stepZ=sz; } my+=ay; mz+=az; stepX=sx; } else if ((ay>=ax)&&(ay>=az)) { if (mx>=0) { mx-=ay; stepX=sx; } if (mz>=0) { mz-=ay; stepZ=sz; } mx+=ax; mz+=az; stepY=sy; } else { if (mx>=0) { mx-=az; stepX=sx; } if (my>=0) { my-=az; stepY=sy; } mx+=ax; my+=ay; stepZ=sz; } xCurrent+=stepX; yCurrent+=stepY; zCurrent+=stepZ; // Step HIGH if (stepX!=0) PORTD|=1<<StepX; if (stepY!=0) PORTD|=1<<StepY; if (stepZ!=0) PORTD|=1<<StepZ; steps--; // Disable ISR if (steps==0) steps=-1; } } if (steps==0) { // Determine next movement parameters dx=xNew-xCurrent; dy=yNew-yCurrent; dz=zNew-zCurrent; // Check that there is something to do if ((dx!=0)||(dy!=0)||(dz!=0)) { ax=abs(dx); ay=abs(dy); az=abs(dz); sx=xNew<xCurrent?-1:xNew>xCurrent?1:0; sy=yNew<yCurrent?-1:yNew>yCurrent?1:0; sz=zNew<zCurrent?-1:zNew>zCurrent?1:0; if ((ax>=ay)&&(ax>=az)) { mx=0; my=ay-(ax>>1); mz=az-(ax>>1); steps=ax; } else if ((ay>=ax)&&(ay>=az)) { mx=ax-(ay>>1); my=0; mz=az-(ay>>1); steps=ay; } else { mx=ax-(az>>1); my=ay-(az>>1); mz=0; steps=az; } // Set the stepper directions if (xReverse) { // digitalWrite(DirX,(1-sx)>>1); if (sx==1) PORTD&=~(1<<DirX); else PORTD|=1<<DirX; } else { // digitalWrite(DirX,(sx+1)>>1); if (sx==-1) PORTD&=~(1<<DirX); else PORTD|=1<<DirX; } if (yReverse) { // digitalWrite(DirY,(1-sy)>>1); if (sy==1) PORTD&=~(1<<DirY); else PORTD|=1<<DirY; } else { // digitalWrite(DirY,(sy+1)>>1); if (sy==-1) PORTD&=~(1<<DirY); else PORTD|=1<<DirY; } if (zReverse) { // digitalWrite(DirZ,(1-sz)>>1); if (sz==1) PORTD&=~(1<<DirZ); else PORTD|=1<<DirZ; } else { // digitalWrite(DirZ,(sz+1)>>1); if (sz==-1) PORTD&=~(1<<DirZ); else PORTD|=1<<DirZ; } } } } else { phase+=magic; // Reset step LOW PORTD&=~(1<<StepX); PORTD&=~(1<<StepY); PORTD&=~(1<<StepZ); } } #include <math.h> // For PI, radians(), sin() and cos() int iSin[360]; int iCos[360]; void setup() { // LED pinMode(LED_BUILTIN,OUTPUT); // Initialise Motor Controller pinMode(DirX,OUTPUT); pinMode(StepX,OUTPUT); pinMode(DirY,OUTPUT); pinMode(StepY,OUTPUT); pinMode(DirZ,OUTPUT); pinMode(StepZ,OUTPUT); pinMode(Enable,OUTPUT); pinMode(Laser,OUTPUT); digitalWrite(DirX,LOW); digitalWrite(StepX,LOW); digitalWrite(DirY,LOW); digitalWrite(StepY,LOW); digitalWrite(DirZ,LOW); digitalWrite(StepZ,LOW); digitalWrite(Enable,LOW); // Active low digitalWrite(Laser,LOW); // Active high // Disable interrupts cli(); // Use Timer 2 for ISR // Good for ATmega48A/PA/88A/PA/168A/PA/328/P TIMSK2=0; // Timer interrupts off TCCR2A=(0<<COM2A0)|(0<<COM2B0)|(1<<WGM20); // Phase correct PWM, no output TCCR2B=(0<<WGM22)|(2<<CS20); // 2 MHz clock (3921.57 Hz) OCR2A=128; // Set 50% duty TIMSK2=(1<<TOIE2); // Set interrupt on overflow (=BOTTOM) // Enable interrupts sei(); // Prepare circle array for (int i=0;i<360;i++) { iSin[i]=(int)(200*sin(radians(i))); iCos[i]=(int)(200*cos(radians(i))); } // Restart Timer phase=0; magic=rate*52224/3125; // 2 MHz clock } void loop() { // Used to keep track of motion static int angle=-1; // Update only when "steps==-1" if (steps==-1) { // Make a circle if (angle==-1) { digitalWrite(Laser,LOW); angle=0; xNew=iCos[angle]; yNew=iSin[angle]; zNew=0; } else { digitalWrite(Laser,HIGH); angle+=15; if (angle>=360) angle=0; xNew=iCos[angle]; yNew=iSin[angle]; zNew=0; } // Set ready for ISR steps=0; // Working indicator digitalWrite(LED_BUILTIN,!digitalRead(LED_BUILTIN)); } }
AlanX
-
Test 2
12/14/2017 at 01:55 • 3 commentsUpdate to Code
Spent the morning checking the code to determine why the X axis motor controller smoked. I don't know! The code seems to be doing what it should.
The code now scans a circles. The circle is pretty rough:
Set the driver to 1/8 micro-steps:
It has taken me a while to determine that what I am seeing is overshoot:
The overshoot is in the order of half a full step. Unfortunately there is not much I can do about it.
Here is the current code:
/* Simple 3 Axis Motion Controller =============================== Written by Alan Cooper (agp.cooper@gmail.com) This work is licensed under the Creative Commons Attribution-NonCommercial 2.5 License. This means you are free to copy and share the code (but not to sell it). */ #include <math.h> // For PI, radians(), sin() and cos() // Motor controller (CNC board) pin mapping: #define DirX 2 #define DirY 3 #define DirZ 4 #define StepX 5 #define StepY 6 #define StepZ 7 #define Enable 8 // Active low #define Laser 12 // Turn laser on or off // Motor Controller direction settings bool xReverse=false; bool yReverse=true; bool zReverse=false; // Movement control parameters long stepCountDown; // Movement steps remaining unsigned long movementRate; // Steps per second unsigned long movementInterval; // Step period (uS) unsigned long movementRampIncr; // Step period (uS) long xNew,yNew,zNew; // New co-ordinates long xCurrent,yCurrent,zCurrent; // Current co-ordinates void motionControl(void) { static unsigned long movementPreviousMicros=0; static unsigned long movementCurrentMicros=0; static unsigned long movementRamp; static long stepCountUp=0; static long stepX,stepY,stepZ; static long dx,dy,dz,ax,ay,az,sx,sy,sz,mx,my,mz; // Process motion command at end of current movement if (stepCountDown==-1) { stepCountDown=0; stepCountUp=0; // Determine movement parameters dx=xNew-xCurrent; dy=yNew-yCurrent; dz=zNew-zCurrent; ax=abs(dx); ay=abs(dy); az=abs(dz); sx=xNew<xCurrent?-1:xNew>xCurrent?1:0; sy=yNew<yCurrent?-1:yNew>yCurrent?1:0; sz=zNew<zCurrent?-1:zNew>zCurrent?1:0; if ((ax>=ay)&&(ax>=az)) { mx=0; my=ay-(ax>>1); mz=az-(ax>>1); stepCountDown=ax; } else if ((ay>=ax)&&(ay>=az)) { mx=ax-(ay>>1); my=0; mz=az-(ay>>1); stepCountDown=ay; } else { mx=ax-(az>>1); my=ay-(az>>1); mz=0; stepCountDown=az; } // Set counters if (stepCountDown>0) { stepCountUp=1; } else { stepCountDown=-1; stepCountUp=-1; } // Set the stepper directions if (xReverse) { digitalWrite(DirX,(1-sx)>>1); } else { digitalWrite(DirX,(sx+1)>>1); } if (yReverse) { digitalWrite(DirY,(1-sy)>>1); } else { digitalWrite(DirY,(sy+1)>>1); } if (zReverse) { digitalWrite(DirZ,(1-sz)>>1); } else { digitalWrite(DirZ,(sz+1)>>1); } } // Advance Steppers if (stepCountDown>0) { movementRamp=movementInterval; if ((stepCountUp==1)||(stepCountDown==1)) movementRamp+=movementRampIncr; movementCurrentMicros=micros(); if (movementCurrentMicros-movementPreviousMicros>movementRamp) { movementPreviousMicros=movementCurrentMicros; // Advance steppers stepX=0; stepY=0; stepZ=0; if ((ax>=ay)&&(ax>=az)) { if (my>=0) { my-=ax; stepY=sy; } if (mz>=0) { mz-=ax; stepZ=sz; } my+=ay; mz+=az; stepX=sx; } else if ((ay>=ax)&&(ay>=az)) { if (mx>=0) { mx-=ay; stepX=sx; } if (mz>=0) { mz-=ay; stepZ=sz; } mx+=ax; mz+=az; stepY=sy; } else { if (mx>=0) { mx-=az; stepX=sx; } if (my>=0) { my-=az; stepY=sy; } mx+=ax; my+=ay; stepZ=sz; } xCurrent+=stepX; yCurrent+=stepY; zCurrent+=stepZ; // Step pulse (high for at least 10 us) digitalWrite(StepX,abs(stepX)); digitalWrite(StepY,abs(stepY)); digitalWrite(StepZ,abs(stepZ)); delayMicroseconds(10); digitalWrite(StepX,LOW); digitalWrite(StepY,LOW); digitalWrite(StepZ,LOW); stepCountDown--; stepCountUp++; // Flag end of movement if (stepCountDown==0) { stepCountDown=-1; stepCountUp=-1; } } } else { movementPreviousMicros=micros(); // Reset movement rate clock to ensure near full cycle on start } } int iSin[360]; int iCos[360]; void setup() { // Initialise Motor Controller // Note: The motor controller is using 1/16 micro-stepping pinMode(DirX,OUTPUT); pinMode(StepX,OUTPUT); pinMode(DirY,OUTPUT); pinMode(StepY,OUTPUT); pinMode(DirZ,OUTPUT); pinMode(StepZ,OUTPUT); pinMode(Enable,OUTPUT); pinMode(Laser,OUTPUT); digitalWrite(DirX,LOW); digitalWrite(StepX,LOW); digitalWrite(DirY,LOW); digitalWrite(StepY,LOW); digitalWrite(DirZ,LOW); digitalWrite(StepZ,LOW); digitalWrite(Enable,LOW); // Active low digitalWrite(Laser,LOW); // Active high // Set Global Variables xNew=0; yNew=0; zNew=0; xCurrent=0; yCurrent=0; zCurrent=0; stepCountDown=-1; // Number of steps remaining in motion movementRate=2000; // Step per second movementInterval=1000000/movementRate; // Micro-seconds movementRampIncr=500; // Micro-seconds // LED pinMode(LED_BUILTIN,OUTPUT); digitalWrite(LED_BUILTIN,LOW); // Prepare circle array for (int i=0;i<360;i++) { iSin[i]=(int)(200*sin(radians(i))); iCos[i]=(int)(200*cos(radians(i))); } } void loop() { // LED timer static unsigned long intervalMillisLED=0; static unsigned long currentMillisLED=0; static unsigned long previousMillisLED=0; // Use LED to indicate working if (stepCountDown==0) { intervalMillisLED=1000; } else { intervalMillisLED=100; } currentMillisLED=millis(); if (currentMillisLED-previousMillisLED>intervalMillisLED) { previousMillisLED=currentMillisLED; digitalWrite(LED_BUILTIN,!digitalRead(LED_BUILTIN)); } /* Motion Command */ static int angle=-1; // Make a circle if (stepCountDown==-1) { if (angle==-1) { digitalWrite(Laser,LOW); angle=0; xNew=iCos[angle]; yNew=iSin[angle]; zNew=0; } else { digitalWrite(Laser,HIGH); angle+=15; if (angle>=360) angle=0; xNew=iCos[angle]; yNew=iSin[angle]; zNew=0; } } // Process Motion Command motionControl(); }
Well that just about wraps it up.
AlanX
-
Test Run
12/12/2017 at 06:32 • 0 commentsCode
I wrote some code (using Bresenham's algorithm) for the turret:
/* Simple 3 Axis Motion Controller =============================== Written by Alan Cooper (agp.cooper@gmail.com) This work is licensed under the Creative Commons Attribution-NonCommercial 2.5 License. This means you are free to copy and share the code (but not to sell it). */ // Motor controller (CNC board) pin mapping: #define DirX 2 #define DirY 3 #define DirZ 4 #define StepX 5 #define StepY 6 #define StepZ 7 #define Enable 8 // Active low #define Laser 12 // Turn laser on or off // Motor Controller direction settings bool xReverse=false; bool yReverse=true; bool zReverse=false; // Movement control parameters bool pause=false; // Pause movement long rampSteps; // start and stop ramp steps long stepCountDown; // Movement steps remaining long stepCountUp; // Movement steps completed long movementRate; // Steps per second long movementInterval; // Step period (uS) long ramp; // Ramp up/down period long xNew,yNew,zNew; // New co-ordinates long xCurrent,yCurrent,zCurrent; // Current co-ordinates void motionControl(void) { static unsigned long movementPreviousMicros=0; static unsigned long movementCurrentMicros=0; static long stepX,stepY,stepZ; static long dx,dy,dz,ax,ay,az,sx,sy,sz,mx,my,mz; // Process motion at end of current movement if (stepCountDown==-1) { stepCountDown=0; stepCountUp=0; // Determine movement parameters dx=xNew-xCurrent; dy=yNew-yCurrent; dz=zNew-zCurrent; ax=abs(dx); ay=abs(dy); az=abs(dz); sx=xNew<xCurrent?-1:xNew>xCurrent?1:0; sy=yNew<yCurrent?-1:yNew>yCurrent?1:0; sz=zNew<zCurrent?-1:zNew>zCurrent?1:0; if ((ax>=ay)&&(ax>=az)) { mx=0; my=ay-(ax>>1); mz=az-(ax>>1); stepCountDown=ax; } else if ((ay>=ax)&&(ay>=az)) { mx=ax-(ay>>1); my=0; mz=az-(ay>>1); stepCountDown=ay; } else { mx=ax-(az>>1); my=ay-(az>>1); mz=0; stepCountDown=az; } // Set counters if (stepCountDown>0) { stepCountUp=1; } else { stepCountDown=-1; stepCountUp=-1; } // Set the stepper directions if (xReverse) { digitalWrite(DirX,(1-sx)>>1); } else { digitalWrite(DirX,(sx+1)>>1); } if (yReverse) { digitalWrite(DirY,(1-sy)>>1); } else { digitalWrite(DirY,(sy+1)>>1); } if (zReverse) { digitalWrite(DirZ,(1-sz)>>1); } else { digitalWrite(DirZ,(sz+1)>>1); } } // Advance Steppers if ((pause==false)&&(stepCountDown>0)) { movementCurrentMicros=micros(); if (movementCurrentMicros-movementPreviousMicros>ramp) { movementPreviousMicros=movementCurrentMicros; // Start/stop ramp if (stepCountUp<=rampSteps) ramp-=movementInterval; if (stepCountDown<=rampSteps) ramp+=movementInterval; // Advance steppers stepX=0; stepY=0; stepZ=0; if ((ax>=ay)&&(ax>=az)) { if (my>=0) { my-=ax; stepY=sy; } if (mz>=0) { mz-=ax; stepZ=sz; } my+=ay; mz+=az; stepX=sx; } else if ((ay>=ax)&&(ay>=az)) { if (mx>=0) { mx-=ay; stepX=sx; } if (mz>=0) { mz-=ay; stepZ=sz; } mx+=ax; mz+=az; stepY=sy; } else { if (mx>=0) { mx-=az; stepX=sx; } if (my>=0) { my-=az; stepY=sy; } mx+=ax; my+=ay; stepZ=sz; } xCurrent+=stepX; yCurrent+=stepY; zCurrent+=stepZ; // Step pulse (high for at least 10 us) digitalWrite(StepX,abs(stepX)); digitalWrite(StepY,abs(stepY)); digitalWrite(StepZ,abs(stepZ)); delayMicroseconds(10); digitalWrite(StepX,LOW); digitalWrite(StepY,LOW); digitalWrite(StepZ,LOW); stepCountDown--; stepCountUp++; // Flag end of movement if (stepCountDown==0) { stepCountDown=-1; stepCountUp=-1; } } } else { movementPreviousMicros=micros(); // Reset movement rate clock to ensure near full cycle on start } } void setup() { // Initialise Motor Controller // Note: The motor controller is using 1/16 micro-stepping pinMode(DirX,OUTPUT); pinMode(StepX,OUTPUT); pinMode(DirY,OUTPUT); pinMode(StepY,OUTPUT); pinMode(DirZ,OUTPUT); pinMode(StepZ,OUTPUT); pinMode(Enable,OUTPUT); pinMode(Laser,OUTPUT); digitalWrite(DirX,LOW); digitalWrite(StepX,LOW); digitalWrite(DirY,LOW); digitalWrite(StepY,LOW); digitalWrite(DirZ,LOW); digitalWrite(StepZ,LOW); digitalWrite(Enable,LOW); // Active low digitalWrite(Laser,LOW); // Laser off // Reset Global Variables pause=false; xNew=0; yNew=0; zNew=0; xCurrent=0; yCurrent=0; zCurrent=0; stepCountDown=-1; stepCountUp=-1; movementRate=3200; // Step per second movementInterval=1000000/movementRate; // Micro-seconds rampSteps=6; // Number for steps for ramp up/down // Precalculate ramp constant ramp=movementInterval*(rampSteps+1); // LED pinMode(LED_BUILTIN,OUTPUT); digitalWrite(LED_BUILTIN,LOW); } void loop() { // Used to keep track of motion static int state=0; // LED timer static unsigned long intervalMillisLED=0; static unsigned long currentMillisLED=0; static unsigned long previousMillisLED=0; // Use LED to indicate working if (stepCountDown==0) { intervalMillisLED=1000; } else { intervalMillisLED=100; } currentMillisLED=millis(); if (currentMillisLED-previousMillisLED>intervalMillisLED) { previousMillisLED=currentMillisLED; digitalWrite(LED_BUILTIN,!digitalRead(LED_BUILTIN)); } /* Motion Command */ // Line scanner (-100 to 100 x -100 to 100 ) if (stepCountDown==-1) { if ((state&2)==0) { // Laser off digitalWrite(Laser,LOW); xNew=-100; yNew=-100+(state>>1); zNew=0; } else { // Laser on digitalWrite(Laser,HIGH); xNew=+100; yNew=-100+(state>>1); zNew=0; } state++; if (state>401) state=0; } /* Process Motion Command */ motionControl(); }
The code scans left to right, top to bottom, 201 lines high by 201 pixels wide.
I set the stepper drive up for 0.5A per phase (because the 12v wall plug power supply is a bit under rated for this application) and 1/16 micro stepping.
The CNC board has a design fault in that the micro-steeping shunts are to ground rather than to Vcc, so it is alway set to full step regardless of the shunt setting. I had to jump the shunts pins to Vcc.
The code still needs fine tuning to peak the maximum step rate. It is currently running fine at 3200 PPS (60 RPM).
Assembled and Operating
Here is the turret assembled and operating:
And here is part of the trace on the wall about 5m away:
It is only about 50% of the trace due to the camera shutter speed.
Notice the wiggle! It is about a one micro step (i.e. 1/16 of a full step) high and is quite consistent, probably vibration. If you watch each scan line, some scan lines appear to not move at 1/16 micro stepping. Suggesting the turret accuracy is about 1/8 of a step or about 0.1 degrees. For the next iteration I will drop back to 1/8 micro stepping.
Magic
-
Inspirational
12/07/2017 at 10:21 • 0 commentsInspirational
I found this doing an Internet search of "Animatronics":
Source: "See RoboBones and RoboHead" (http://www.animatronics.org)
Back to Work
Elected not to shunt the external Nano power supply to the motor power supply.
The external power supply input has a schottky to protected against reverse power supply connections. This has the unintended consequence of preventing back EMF from returning to the battery (usually desirable). Instead any back EMF has to be absorbed by the driver boards or the Nano board.
I had lots of problems with back EMF trying to use a common external battery in the past so this time around I will have two separate power supplies.
Added some cable sleeves:
Working on the Code
A bit more complicated then I expected. Do I want to move at an arbitary direction? I suppose I do, so I will gave to code a Bessenham's line drawing algorithm.
Set up an Interrupt Service Routine (ISR) for the step pulse generator. I have limited the maximum pulse rate to 500 PPS, which is safe from a standing start for full step operation for nearly all common stepper motors. Should be able to bump this up for micro-stepping.
Magic