Diff: main.cpp

Revision:

0:761688adad1d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 01 22:30:21 2011 +0000
@@ -0,0 +1,315 @@
+/****************************************************/
+/*                                                  */
+/* Title:   Linear speed profile implementation for */
+/*          Pololu Breakout board with A4983        */
+/*          with an mbed evaluation board           */
+/* Author:  Martin Gyurkó                           */
+/* email:   nospam@gyurma.de                        */
+/*                                                  */
+/* $ID:$ */
+/****************************************************/
+
+#include "mbed.h"
+
+//communication channel
+Serial pc(USBTX, USBRX);
+
+//LEDs port definition on mbed
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+//ports connected to pololu driver board = straightforward connection of one side of mbed
+DigitalOut Dir(p5); //direction of movement
+DigitalOut Step(p6); //make one step at rising edge
+DigitalOut nSleep(p7); // be HIGH to make it work
+DigitalOut nReset(p8); // be HIGH to make it work
+DigitalOut MS3(p9); //microstep mode selectors
+DigitalOut MS2(p10);
+DigitalOut MS1(p11);
+DigitalOut nEnable(p12);// be LOW to make it work
+
+//Home sensor input
+DigitalIn  EndSwitchIn(p20); //normally HIGH when nothing inside optopath
+
+
+#define HIGH 1
+#define LOW 0
+#define ON 1
+#define OFF 0
+#define Clockwise LOW
+#define CounterClockwise HIGH
+
+//which mode of driver to be used. This is now 1/16 stepping.
+#define Factor  16
+
+//Maximum distance to travel
+//my linear motion sled has 3.25 rounds * 200 imp/round  *16
+#define SMax   ( 650  * Factor ) //for sled
+
+//for simple motor tests we set a real big distance:
+//#define SMax   ( 40000  * Factor )
+
+//minimum speed
+#define VMin 1
+
+//maximum speed (runtime settable)
+double   VMax   = ( 66.64  * Factor );
+
+//homing speed
+#define VHome  (   30  * Factor )
+
+//maximum acceleration
+#define AMax   (800  * Factor )
+
+//trace buffer for debugging, take care of size of ram!
+//float TimeTrace[SMax];
+
+// Make a step and wait some LowtTime time long
+void MakeSpeedStep(float v = 1000) {
+    if (v >= VMin) { //avoid crash because of division
+        myled4 = 0;
+        Step = HIGH;
+
+        wait(0.000001); // minimum Hightime is 1us, this is ca 5us
+
+        myled4 = 1;
+        Step = LOW;
+
+        wait(1/v); // minimum Lowtime is 1us
+    }
+}
+
+// Make a step and wait some dt time long
+int MakeTimedStep(float dt = 1000) {
+    int temptime;
+    int dummy = 0;
+    if ((dt >= 0.000000001) && (dt < 10)) { // avoid too long or no waiting
+        myled4 = 0;
+        Step = HIGH;
+
+        wait(0.000001); // minimum Hightime is 1us, this is ca 3us
+
+        myled4 = 1;
+        Step = LOW;
+        //timing more accurate with a dummy operation
+        temptime = (int) (dt * 10000000.0f);
+        for (;temptime > 0; temptime--)
+            dummy += temptime;//asm (" nop ");
+
+        //wait(dt); // minimum Lowtime is 1us
+    }
+    return dummy; //needed beacause if omitted, the optimizer removes the timing loop :(
+}
+
+int main() {
+    char    OldEndSwitchIn = HIGH;
+//   char inchar;
+    int i;
+    double dummy = 0.0f;
+    int     counter = 0;
+//init max time base
+    double   t0 = sqrt(2.0/AMax)*0.676;
+
+    double   newt;
+
+// ********** caculations for acceleration and deceleration **********
+    int     countLimSMax = VMax*VMax/(2*AMax); //if we can reach max speed we use this many steps
+    int     countAccelLim = SMax/2; //if we can not reach max speed, we need to decelerate from half the stepdistance
+    int     countLim = (countLimSMax<countAccelLim)?countLimSMax:countAccelLim; //which do we use?
+
+//    double  tmp;
+
+//set communication speed
+    pc.baud(115200);
+//test to see if comm is working
+    printf("Hello!\n");
+
+//  define filesystem
+//    LocalFileSystem local("local");
+
+    myled1 = 0;
+    myled2 = 0;
+    myled3 = 0;
+    myled4 = 0;
+
+// select which factor mode is to be set on the a4983
+// compile time selection depending on Factor
+#if (Factor==1)
+    MS1 = LOW;
+    MS2 = LOW;
+    MS3 = LOW;  // 1/1
+#elif (Factor==2)
+    MS1 = HIGH;
+    MS2 = LOW;
+    MS3 = LOW;  // 1/2
+#elif (Factor==4)
+    MS1 = LOW;
+    MS2 = HIGH;
+    MS3 = LOW;  // 1/4
+#elif (Factor==8)
+    MS1 = HIGH;
+    MS2 = HIGH;
+    MS3 = LOW;  // 1/8
+#elif (Factor==16)
+    MS1 = HIGH;
+    MS2 = HIGH;
+    MS3 = HIGH; // 1/16
+#endif
+
+// ************ initialisation of power stage: ***********
+
+    Dir = CounterClockwise;
+    Step = LOW;
+    nSleep = HIGH;
+    nEnable = HIGH;
+    nReset = LOW;
+    myled1 = 1;
+    myled2 = 0;
+    myled3 = 0;
+    myled4 = 0;
+
+    wait(0.5);
+
+    nReset = HIGH;
+    myled1 = 0;
+    myled2 = 1;
+    myled3 = 0;
+    myled4 = 0;
+
+    wait(0.5);
+
+    nEnable = LOW;
+    myled1 = 1;
+    myled2 = 1;
+    myled3 = 0;
+    myled4 = 0;
+
+    wait(0.5);
+
+    Step = HIGH;
+    myled1 = 0;
+    myled2 = 0;
+    myled3 = 1;
+    myled4 = 0;
+
+    wait(0.5);
+
+    Step = LOW;
+    myled1 = 1;
+    myled2 = 0;
+    myled3 = 1;
+    myled4 = 0;
+
+    wait(0.5);
+
+// test for setting up power stage
+    /*  while( 1 )
+      {
+        //by using the endswitch we can make single steps forward and see how the micsosteps are behaving
+        Step = EndSwitchIn;
+      }
+    */
+
+// ************* Homing routine *************
+// moves stepper in one direction until the endswitch triggers
+    while ( 1 ) {
+        if (( EndSwitchIn == LOW) && (OldEndSwitchIn == HIGH )) break;
+        OldEndSwitchIn = EndSwitchIn;
+        MakeSpeedStep(VHome*Factor);
+    }
+
+// wait for settling the sled
+    wait(0.2);
+
+// ************* Main endless loop ******************
+// reads speed from a terminal and makes a movement with this maximum speed.
+// acceleration and distance limits maximum reachable speed
+    while ( 1 ) {
+        // before every run we read how big VMax should be
+        scanf("%lf",&VMax);
+        // just an echo to see that serial connection is working
+        printf("input was:%f\n\r", VMax);
+
+        //normally i set a positive speed and so the movement is an oscillation
+        //at negative values i want to let it go the same direction again and thus i have to set back Dir to original state
+        if (VMax < 0.0f)
+            Dir=!Dir; //let it go the
+
+        // special case for driver testing and current setting purposes
+        // for external current measurements make reset and set phases to 100% and 0%
+        if (VMax == 1.0f) {
+            nEnable = LOW;
+            nReset = LOW;
+            printf("reset low\n\r");
+            wait(0.05);
+            nReset = HIGH;
+            printf("reset high\n\r");
+            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
+                MakeSpeedStep(VHome*Factor);
+            printf("made %d steps\n\r", i);
+        }
+        // normal operation
+        // no 0 speed allowed
+        else if (VMax != 0.0f) {
+            nEnable = LOW;
+            VMax*=Factor;
+            countLimSMax = VMax*VMax/(2*AMax);
+            countLim = (countLimSMax<countAccelLim)?countLimSMax:countAccelLim; //depending on acceleration and max speed we decide which is to be used
+            Dir=!Dir; //change direction to opposite
+            counter = 1;
+            newt = t0; //in the beginning there was a steptime of t0...
+            printf("CountLimSMax: %d CountAccelLim: %d CountLim: %d \r\n", countLimSMax, countAccelLim, countLim);
+            //make linear speed ramp movement
+            //it is possible to stop it anytime during movement by hitting enter
+            while ( (counter < SMax ) && !pc.readable() ) {
+                //  TimeTrace[counter]=newt;
+                // make the pulse and wait
+                dummy = MakeTimedStep(newt);
+                // at acceleration phase
+                if (  counter <= countLim ) {
+                    // use only one of these methods!
+                    // newtonian approximation method
+                    newt = newt - 2.0 * newt / (4.0*counter + 1.0);
+                    // accurate method with double precision
+                    // newt = t0*((sqrt((long double)(counter+1))-sqrt((long double)counter)));
+                }
+                // at constant speed phase
+                else if ((countLim < counter) && (counter <= (SMax-countLim) )) {
+                    //if (Dir == 1) newt = 1/VMax;
+                    //to maintain execution time the processor is doing some really hard work ;)
+                    dummy = dummy - 2.0 * newt / (4.0*counter + 1.0);
+                }
+                // at deceleration phase
+                else {
+                    // newtonian approximation method
+                    newt = newt + 2.0 * newt / (4.0*(SMax-counter) + 1.0) ;
+                    // accurate method with double precision
+                    // newt = t0*(sqrt((long double)(SMax-counter+1))-sqrt((long double)(SMax-counter)));
+                }
+                //increase step counter
+                counter++;
+            }
+            nEnable = HIGH; //when waiting shut down output to save power and avoid overheating
+            printf("dummy = %f\n\r",dummy);//to ensure the compiler doesnt optimize the countings away...
+        }
+
+// ************ write trace to file ************
+// File usage for tracing is buggy... fix it!
+        /*
+               FILE *fp = fopen("/local/out.txt", "w");  // Open "out.txt" on the local file system for writing
+               tmp = 0.0f;
+
+               for(int i=1; i<SMax; i++)
+               {
+                   tmp+=TimeTrace[i];
+                   if (TimeTrace[i] != 0.0f)
+                       fprintf(fp,"%8d %lf %lf %lf\r\n", i, TimeTrace[i]*1000, tmp, 1/TimeTrace[i]);
+               }
+
+        //        fprintf(fp,"Hi \r\n");
+               fclose(fp);
+        */
+    }
+}