Martin Gyurkó / Mbed 2 deprecated stepper_motor_control

Powers on a Pololu Stepper Motor driver board equipped with an Allegro a4983 IC. First it searches for the home switch, then it moves endlessly back and forth with linear speed ramp At motor currents above 1A the driver board seriously needs extra cooling attached to the driver IC, like a simple aluminium piece with heat transfer grease Thanks to Atmel AVR446 Application Note!

Dependencies:   mbed

main.cpp@0:761688adad1d, 2011-03-01 (annotated)

Committer:
gyurma
Date:
Tue Mar 01 22:30:21 2011 +0000
Revision:
0:761688adad1d
First publication of the linear ramp stepper motor driver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gyurma 0:761688adad1d 1 /****************************************************/
gyurma 0:761688adad1d 2 /* */
gyurma 0:761688adad1d 3 /* Title: Linear speed profile implementation for */
gyurma 0:761688adad1d 4 /* Pololu Breakout board with A4983 */
gyurma 0:761688adad1d 5 /* with an mbed evaluation board */
gyurma 0:761688adad1d 6 /* Author: Martin Gyurkó */
gyurma 0:761688adad1d 7 /* email: nospam@gyurma.de */
gyurma 0:761688adad1d 8 /* */
gyurma 0:761688adad1d 9 /* $ID:$ */
gyurma 0:761688adad1d 10 /****************************************************/
gyurma 0:761688adad1d 11
gyurma 0:761688adad1d 12 #include "mbed.h"
gyurma 0:761688adad1d 13
gyurma 0:761688adad1d 14 //communication channel
gyurma 0:761688adad1d 15 Serial pc(USBTX, USBRX);
gyurma 0:761688adad1d 16
gyurma 0:761688adad1d 17 //LEDs port definition on mbed
gyurma 0:761688adad1d 18 DigitalOut myled1(LED1);
gyurma 0:761688adad1d 19 DigitalOut myled2(LED2);
gyurma 0:761688adad1d 20 DigitalOut myled3(LED3);
gyurma 0:761688adad1d 21 DigitalOut myled4(LED4);
gyurma 0:761688adad1d 22
gyurma 0:761688adad1d 23 //ports connected to pololu driver board = straightforward connection of one side of mbed
gyurma 0:761688adad1d 24 DigitalOut Dir(p5); //direction of movement
gyurma 0:761688adad1d 25 DigitalOut Step(p6); //make one step at rising edge
gyurma 0:761688adad1d 26 DigitalOut nSleep(p7); // be HIGH to make it work
gyurma 0:761688adad1d 27 DigitalOut nReset(p8); // be HIGH to make it work
gyurma 0:761688adad1d 28 DigitalOut MS3(p9); //microstep mode selectors
gyurma 0:761688adad1d 29 DigitalOut MS2(p10);
gyurma 0:761688adad1d 30 DigitalOut MS1(p11);
gyurma 0:761688adad1d 31 DigitalOut nEnable(p12);// be LOW to make it work
gyurma 0:761688adad1d 32
gyurma 0:761688adad1d 33 //Home sensor input
gyurma 0:761688adad1d 34 DigitalIn EndSwitchIn(p20); //normally HIGH when nothing inside optopath
gyurma 0:761688adad1d 35
gyurma 0:761688adad1d 36
gyurma 0:761688adad1d 37 #define HIGH 1
gyurma 0:761688adad1d 38 #define LOW 0
gyurma 0:761688adad1d 39 #define ON 1
gyurma 0:761688adad1d 40 #define OFF 0
gyurma 0:761688adad1d 41 #define Clockwise LOW
gyurma 0:761688adad1d 42 #define CounterClockwise HIGH
gyurma 0:761688adad1d 43
gyurma 0:761688adad1d 44 //which mode of driver to be used. This is now 1/16 stepping.
gyurma 0:761688adad1d 45 #define Factor 16
gyurma 0:761688adad1d 46
gyurma 0:761688adad1d 47 //Maximum distance to travel
gyurma 0:761688adad1d 48 //my linear motion sled has 3.25 rounds * 200 imp/round *16
gyurma 0:761688adad1d 49 #define SMax ( 650 * Factor ) //for sled
gyurma 0:761688adad1d 50
gyurma 0:761688adad1d 51 //for simple motor tests we set a real big distance:
gyurma 0:761688adad1d 52 //#define SMax ( 40000 * Factor )
gyurma 0:761688adad1d 53
gyurma 0:761688adad1d 54 //minimum speed
gyurma 0:761688adad1d 55 #define VMin 1
gyurma 0:761688adad1d 56
gyurma 0:761688adad1d 57 //maximum speed (runtime settable)
gyurma 0:761688adad1d 58 double VMax = ( 66.64 * Factor );
gyurma 0:761688adad1d 59
gyurma 0:761688adad1d 60 //homing speed
gyurma 0:761688adad1d 61 #define VHome ( 30 * Factor )
gyurma 0:761688adad1d 62
gyurma 0:761688adad1d 63 //maximum acceleration
gyurma 0:761688adad1d 64 #define AMax (800 * Factor )
gyurma 0:761688adad1d 65
gyurma 0:761688adad1d 66 //trace buffer for debugging, take care of size of ram!
gyurma 0:761688adad1d 67 //float TimeTrace[SMax];
gyurma 0:761688adad1d 68
gyurma 0:761688adad1d 69 // Make a step and wait some LowtTime time long
gyurma 0:761688adad1d 70 void MakeSpeedStep(float v = 1000) {
gyurma 0:761688adad1d 71 if (v >= VMin) { //avoid crash because of division
gyurma 0:761688adad1d 72 myled4 = 0;
gyurma 0:761688adad1d 73 Step = HIGH;
gyurma 0:761688adad1d 74
gyurma 0:761688adad1d 75 wait(0.000001); // minimum Hightime is 1us, this is ca 5us
gyurma 0:761688adad1d 76
gyurma 0:761688adad1d 77 myled4 = 1;
gyurma 0:761688adad1d 78 Step = LOW;
gyurma 0:761688adad1d 79
gyurma 0:761688adad1d 80 wait(1/v); // minimum Lowtime is 1us
gyurma 0:761688adad1d 81 }
gyurma 0:761688adad1d 82 }
gyurma 0:761688adad1d 83
gyurma 0:761688adad1d 84 // Make a step and wait some dt time long
gyurma 0:761688adad1d 85 int MakeTimedStep(float dt = 1000) {
gyurma 0:761688adad1d 86 int temptime;
gyurma 0:761688adad1d 87 int dummy = 0;
gyurma 0:761688adad1d 88 if ((dt >= 0.000000001) && (dt < 10)) { // avoid too long or no waiting
gyurma 0:761688adad1d 89 myled4 = 0;
gyurma 0:761688adad1d 90 Step = HIGH;
gyurma 0:761688adad1d 91
gyurma 0:761688adad1d 92 wait(0.000001); // minimum Hightime is 1us, this is ca 3us
gyurma 0:761688adad1d 93
gyurma 0:761688adad1d 94 myled4 = 1;
gyurma 0:761688adad1d 95 Step = LOW;
gyurma 0:761688adad1d 96 //timing more accurate with a dummy operation
gyurma 0:761688adad1d 97 temptime = (int) (dt * 10000000.0f);
gyurma 0:761688adad1d 98 for (;temptime > 0; temptime--)
gyurma 0:761688adad1d 99 dummy += temptime;//asm (" nop ");
gyurma 0:761688adad1d 100
gyurma 0:761688adad1d 101 //wait(dt); // minimum Lowtime is 1us
gyurma 0:761688adad1d 102 }
gyurma 0:761688adad1d 103 return dummy; //needed beacause if omitted, the optimizer removes the timing loop :(
gyurma 0:761688adad1d 104 }
gyurma 0:761688adad1d 105
gyurma 0:761688adad1d 106 int main() {
gyurma 0:761688adad1d 107 char OldEndSwitchIn = HIGH;
gyurma 0:761688adad1d 108 // char inchar;
gyurma 0:761688adad1d 109 int i;
gyurma 0:761688adad1d 110 double dummy = 0.0f;
gyurma 0:761688adad1d 111 int counter = 0;
gyurma 0:761688adad1d 112 //init max time base
gyurma 0:761688adad1d 113 double t0 = sqrt(2.0/AMax)*0.676;
gyurma 0:761688adad1d 114
gyurma 0:761688adad1d 115 double newt;
gyurma 0:761688adad1d 116
gyurma 0:761688adad1d 117 // ********** caculations for acceleration and deceleration **********
gyurma 0:761688adad1d 118 int countLimSMax = VMax*VMax/(2*AMax); //if we can reach max speed we use this many steps
gyurma 0:761688adad1d 119 int countAccelLim = SMax/2; //if we can not reach max speed, we need to decelerate from half the stepdistance
gyurma 0:761688adad1d 120 int countLim = (countLimSMax<countAccelLim)?countLimSMax:countAccelLim; //which do we use?
gyurma 0:761688adad1d 121
gyurma 0:761688adad1d 122 // double tmp;
gyurma 0:761688adad1d 123
gyurma 0:761688adad1d 124 //set communication speed
gyurma 0:761688adad1d 125 pc.baud(115200);
gyurma 0:761688adad1d 126 //test to see if comm is working
gyurma 0:761688adad1d 127 printf("Hello!\n");
gyurma 0:761688adad1d 128
gyurma 0:761688adad1d 129 // define filesystem
gyurma 0:761688adad1d 130 // LocalFileSystem local("local");
gyurma 0:761688adad1d 131
gyurma 0:761688adad1d 132 myled1 = 0;
gyurma 0:761688adad1d 133 myled2 = 0;
gyurma 0:761688adad1d 134 myled3 = 0;
gyurma 0:761688adad1d 135 myled4 = 0;
gyurma 0:761688adad1d 136
gyurma 0:761688adad1d 137 // select which factor mode is to be set on the a4983
gyurma 0:761688adad1d 138 // compile time selection depending on Factor
gyurma 0:761688adad1d 139 #if (Factor==1)
gyurma 0:761688adad1d 140 MS1 = LOW;
gyurma 0:761688adad1d 141 MS2 = LOW;
gyurma 0:761688adad1d 142 MS3 = LOW; // 1/1
gyurma 0:761688adad1d 143 #elif (Factor==2)
gyurma 0:761688adad1d 144 MS1 = HIGH;
gyurma 0:761688adad1d 145 MS2 = LOW;
gyurma 0:761688adad1d 146 MS3 = LOW; // 1/2
gyurma 0:761688adad1d 147 #elif (Factor==4)
gyurma 0:761688adad1d 148 MS1 = LOW;
gyurma 0:761688adad1d 149 MS2 = HIGH;
gyurma 0:761688adad1d 150 MS3 = LOW; // 1/4
gyurma 0:761688adad1d 151 #elif (Factor==8)
gyurma 0:761688adad1d 152 MS1 = HIGH;
gyurma 0:761688adad1d 153 MS2 = HIGH;
gyurma 0:761688adad1d 154 MS3 = LOW; // 1/8
gyurma 0:761688adad1d 155 #elif (Factor==16)
gyurma 0:761688adad1d 156 MS1 = HIGH;
gyurma 0:761688adad1d 157 MS2 = HIGH;
gyurma 0:761688adad1d 158 MS3 = HIGH; // 1/16
gyurma 0:761688adad1d 159 #endif
gyurma 0:761688adad1d 160
gyurma 0:761688adad1d 161 // ************ initialisation of power stage: ***********
gyurma 0:761688adad1d 162
gyurma 0:761688adad1d 163 Dir = CounterClockwise;
gyurma 0:761688adad1d 164 Step = LOW;
gyurma 0:761688adad1d 165 nSleep = HIGH;
gyurma 0:761688adad1d 166 nEnable = HIGH;
gyurma 0:761688adad1d 167 nReset = LOW;
gyurma 0:761688adad1d 168 myled1 = 1;
gyurma 0:761688adad1d 169 myled2 = 0;
gyurma 0:761688adad1d 170 myled3 = 0;
gyurma 0:761688adad1d 171 myled4 = 0;
gyurma 0:761688adad1d 172
gyurma 0:761688adad1d 173 wait(0.5);
gyurma 0:761688adad1d 174
gyurma 0:761688adad1d 175 nReset = HIGH;
gyurma 0:761688adad1d 176 myled1 = 0;
gyurma 0:761688adad1d 177 myled2 = 1;
gyurma 0:761688adad1d 178 myled3 = 0;
gyurma 0:761688adad1d 179 myled4 = 0;
gyurma 0:761688adad1d 180
gyurma 0:761688adad1d 181 wait(0.5);
gyurma 0:761688adad1d 182
gyurma 0:761688adad1d 183 nEnable = LOW;
gyurma 0:761688adad1d 184 myled1 = 1;
gyurma 0:761688adad1d 185 myled2 = 1;
gyurma 0:761688adad1d 186 myled3 = 0;
gyurma 0:761688adad1d 187 myled4 = 0;
gyurma 0:761688adad1d 188
gyurma 0:761688adad1d 189 wait(0.5);
gyurma 0:761688adad1d 190
gyurma 0:761688adad1d 191 Step = HIGH;
gyurma 0:761688adad1d 192 myled1 = 0;
gyurma 0:761688adad1d 193 myled2 = 0;
gyurma 0:761688adad1d 194 myled3 = 1;
gyurma 0:761688adad1d 195 myled4 = 0;
gyurma 0:761688adad1d 196
gyurma 0:761688adad1d 197 wait(0.5);
gyurma 0:761688adad1d 198
gyurma 0:761688adad1d 199 Step = LOW;
gyurma 0:761688adad1d 200 myled1 = 1;
gyurma 0:761688adad1d 201 myled2 = 0;
gyurma 0:761688adad1d 202 myled3 = 1;
gyurma 0:761688adad1d 203 myled4 = 0;
gyurma 0:761688adad1d 204
gyurma 0:761688adad1d 205 wait(0.5);
gyurma 0:761688adad1d 206
gyurma 0:761688adad1d 207 // test for setting up power stage
gyurma 0:761688adad1d 208 /* while( 1 )
gyurma 0:761688adad1d 209 {
gyurma 0:761688adad1d 210 //by using the endswitch we can make single steps forward and see how the micsosteps are behaving
gyurma 0:761688adad1d 211 Step = EndSwitchIn;
gyurma 0:761688adad1d 212 }
gyurma 0:761688adad1d 213 */
gyurma 0:761688adad1d 214
gyurma 0:761688adad1d 215 // ************* Homing routine *************
gyurma 0:761688adad1d 216 // moves stepper in one direction until the endswitch triggers
gyurma 0:761688adad1d 217 while ( 1 ) {
gyurma 0:761688adad1d 218 if (( EndSwitchIn == LOW) && (OldEndSwitchIn == HIGH )) break;
gyurma 0:761688adad1d 219 OldEndSwitchIn = EndSwitchIn;
gyurma 0:761688adad1d 220 MakeSpeedStep(VHome*Factor);
gyurma 0:761688adad1d 221 }
gyurma 0:761688adad1d 222
gyurma 0:761688adad1d 223 // wait for settling the sled
gyurma 0:761688adad1d 224 wait(0.2);
gyurma 0:761688adad1d 225
gyurma 0:761688adad1d 226 // ************* Main endless loop ******************
gyurma 0:761688adad1d 227 // reads speed from a terminal and makes a movement with this maximum speed.
gyurma 0:761688adad1d 228 // acceleration and distance limits maximum reachable speed
gyurma 0:761688adad1d 229 while ( 1 ) {
gyurma 0:761688adad1d 230 // before every run we read how big VMax should be
gyurma 0:761688adad1d 231 scanf("%lf",&VMax);
gyurma 0:761688adad1d 232 // just an echo to see that serial connection is working
gyurma 0:761688adad1d 233 printf("input was:%f\n\r", VMax);
gyurma 0:761688adad1d 234
gyurma 0:761688adad1d 235 //normally i set a positive speed and so the movement is an oscillation
gyurma 0:761688adad1d 236 //at negative values i want to let it go the same direction again and thus i have to set back Dir to original state
gyurma 0:761688adad1d 237 if (VMax < 0.0f)
gyurma 0:761688adad1d 238 Dir=!Dir; //let it go the
gyurma 0:761688adad1d 239
gyurma 0:761688adad1d 240 // special case for driver testing and current setting purposes
gyurma 0:761688adad1d 241 // for external current measurements make reset and set phases to 100% and 0%
gyurma 0:761688adad1d 242 if (VMax == 1.0f) {
gyurma 0:761688adad1d 243 nEnable = LOW;
gyurma 0:761688adad1d 244 nReset = LOW;
gyurma 0:761688adad1d 245 printf("reset low\n\r");
gyurma 0:761688adad1d 246 wait(0.05);
gyurma 0:761688adad1d 247 nReset = HIGH;
gyurma 0:761688adad1d 248 printf("reset high\n\r");
gyurma 0:761688adad1d 249 for (i = 0; i < (Factor/2); i++) //after so many steps the one of the phases has 100% and we can measure the max current
gyurma 0:761688adad1d 250 MakeSpeedStep(VHome*Factor);
gyurma 0:761688adad1d 251 printf("made %d steps\n\r", i);
gyurma 0:761688adad1d 252 }
gyurma 0:761688adad1d 253 // normal operation
gyurma 0:761688adad1d 254 // no 0 speed allowed
gyurma 0:761688adad1d 255 else if (VMax != 0.0f) {
gyurma 0:761688adad1d 256 nEnable = LOW;
gyurma 0:761688adad1d 257 VMax*=Factor;
gyurma 0:761688adad1d 258 countLimSMax = VMax*VMax/(2*AMax);
gyurma 0:761688adad1d 259 countLim = (countLimSMax<countAccelLim)?countLimSMax:countAccelLim; //depending on acceleration and max speed we decide which is to be used
gyurma 0:761688adad1d 260 Dir=!Dir; //change direction to opposite
gyurma 0:761688adad1d 261 counter = 1;
gyurma 0:761688adad1d 262 newt = t0; //in the beginning there was a steptime of t0...
gyurma 0:761688adad1d 263 printf("CountLimSMax: %d CountAccelLim: %d CountLim: %d \r\n", countLimSMax, countAccelLim, countLim);
gyurma 0:761688adad1d 264 //make linear speed ramp movement
gyurma 0:761688adad1d 265 //it is possible to stop it anytime during movement by hitting enter
gyurma 0:761688adad1d 266 while ( (counter < SMax ) && !pc.readable() ) {
gyurma 0:761688adad1d 267 // TimeTrace[counter]=newt;
gyurma 0:761688adad1d 268 // make the pulse and wait
gyurma 0:761688adad1d 269 dummy = MakeTimedStep(newt);
gyurma 0:761688adad1d 270 // at acceleration phase
gyurma 0:761688adad1d 271 if ( counter <= countLim ) {
gyurma 0:761688adad1d 272 // use only one of these methods!
gyurma 0:761688adad1d 273 // newtonian approximation method
gyurma 0:761688adad1d 274 newt = newt - 2.0 * newt / (4.0*counter + 1.0);
gyurma 0:761688adad1d 275 // accurate method with double precision
gyurma 0:761688adad1d 276 // newt = t0*((sqrt((long double)(counter+1))-sqrt((long double)counter)));
gyurma 0:761688adad1d 277 }
gyurma 0:761688adad1d 278 // at constant speed phase
gyurma 0:761688adad1d 279 else if ((countLim < counter) && (counter <= (SMax-countLim) )) {
gyurma 0:761688adad1d 280 //if (Dir == 1) newt = 1/VMax;
gyurma 0:761688adad1d 281 //to maintain execution time the processor is doing some really hard work ;)
gyurma 0:761688adad1d 282 dummy = dummy - 2.0 * newt / (4.0*counter + 1.0);
gyurma 0:761688adad1d 283 }
gyurma 0:761688adad1d 284 // at deceleration phase
gyurma 0:761688adad1d 285 else {
gyurma 0:761688adad1d 286 // newtonian approximation method
gyurma 0:761688adad1d 287 newt = newt + 2.0 * newt / (4.0*(SMax-counter) + 1.0) ;
gyurma 0:761688adad1d 288 // accurate method with double precision
gyurma 0:761688adad1d 289 // newt = t0*(sqrt((long double)(SMax-counter+1))-sqrt((long double)(SMax-counter)));
gyurma 0:761688adad1d 290 }
gyurma 0:761688adad1d 291 //increase step counter
gyurma 0:761688adad1d 292 counter++;
gyurma 0:761688adad1d 293 }
gyurma 0:761688adad1d 294 nEnable = HIGH; //when waiting shut down output to save power and avoid overheating
gyurma 0:761688adad1d 295 printf("dummy = %f\n\r",dummy);//to ensure the compiler doesnt optimize the countings away...
gyurma 0:761688adad1d 296 }
gyurma 0:761688adad1d 297
gyurma 0:761688adad1d 298 // ************ write trace to file ************
gyurma 0:761688adad1d 299 // File usage for tracing is buggy... fix it!
gyurma 0:761688adad1d 300 /*
gyurma 0:761688adad1d 301 FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
gyurma 0:761688adad1d 302 tmp = 0.0f;
gyurma 0:761688adad1d 303
gyurma 0:761688adad1d 304 for(int i=1; i<SMax; i++)
gyurma 0:761688adad1d 305 {
gyurma 0:761688adad1d 306 tmp+=TimeTrace[i];
gyurma 0:761688adad1d 307 if (TimeTrace[i] != 0.0f)
gyurma 0:761688adad1d 308 fprintf(fp,"%8d %lf %lf %lf\r\n", i, TimeTrace[i]*1000, tmp, 1/TimeTrace[i]);
gyurma 0:761688adad1d 309 }
gyurma 0:761688adad1d 310
gyurma 0:761688adad1d 311 // fprintf(fp,"Hi \r\n");
gyurma 0:761688adad1d 312 fclose(fp);
gyurma 0:761688adad1d 313 */
gyurma 0:761688adad1d 314 }
gyurma 0:761688adad1d 315 }