main.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 #include "cmsis_os.h" // https://mbed.org/handbook/CMSIS-RTOS
00004 // http://mbed.org/users/AjK/notebook/regarding-interrupts-use-and-blocking/
00005 
00006 DigitalOut StepLED(LED1);
00007 DigitalOut CmdLED(LED2);
00008 DigitalOut DirLED(LED3);
00009 DigitalOut TestLED(LED4);
00010 DigitalOut xDir(p22);
00011 DigitalOut xStep(p24);
00012 DigitalIn xHome(p8);
00013 
00014 typedef struct {
00015     float steps;  // number of steps to take
00016     float accel;  // accelleration in steps/sec^2
00017     float maxspeed; // maximum speed in steps/sec
00018     float minspeed; // minimum speed in steps/sec
00019     bool dir;    // direction
00020 } stepcmd;
00021 
00022 osPoolDef(cmdpool, 16, stepcmd);
00023 osPoolId cmdpool;
00024 
00025 osMessageQDef(queue, 16, stepcmd);
00026 osMessageQId  queue;
00027 unsigned int qcnt = 0;
00028 Timeout timer,steptimer;
00029 Timer counter;
00030 
00031 void decelmove();
00032 void accelmove();
00033 void flatmove();
00034 void stepper();
00035 void stepdown();
00036 
00037 const bool xStepHigh = 1;
00038 const float pulseTime = 5; // pulse time in microseconds
00039 bool N_ESTOP = 1;
00040 float asteps, lsteps, dsteps, speed, accel, maxspeed, minspeed, ttime, dtime, wtime , etime, rtime, tsteps;
00041 char output[256];
00042 
00043 Serial pc(USBTX, USBRX); // tx, rx
00044 
00045 void stepdown()
00046 {
00047     xStep = !xStepHigh;
00048     tsteps++;
00049 }
00050 
00051 void decelmove()
00052 {
00053     if (N_ESTOP && (dsteps-- > 0)) {
00054         wtime = 1000000 / (speed - accel * (dtime/1000000));
00055         timer.attach_us(decelmove, wtime);
00056         xStep = xStepHigh;
00057         steptimer.attach_us(stepdown, pulseTime);
00058         dtime += wtime;
00059     } else {
00060         ttime += dtime;
00061         etime = ttime;
00062         counter.stop();
00063         rtime = counter.read_us();
00064         timer.attach_us(stepper, wtime);
00065     }
00066 }
00067 
00068 void flatmove()
00069 {
00070     if (N_ESTOP && (lsteps-- > 0)) {
00071         timer.attach_us(flatmove, wtime);
00072         xStep = xStepHigh;
00073         steptimer.attach_us(stepdown, pulseTime);
00074         ttime += wtime;
00075         dtime = 0;
00076     } else {
00077         // ttime was only accelleration, not flatmove!
00078         // after calculating wtime, set to 0 for use in decelmove
00079         speed = 1000000.0 / wtime;
00080         timer.attach_us(decelmove, wtime);
00081     }
00082 }
00083 
00084 void accelmove()
00085 {
00086     TestLED = 1;
00087     if (N_ESTOP && (asteps-- > 0)) {
00088         wtime = 1000000 / (speed + accel * (ttime/1000000));
00089         if (wtime < pulseTime) wtime = pulseTime;
00090         timer.attach_us(accelmove, wtime);
00091         xStep = xStepHigh;
00092         steptimer.attach_us(stepdown, pulseTime);
00093         ttime += wtime;
00094 
00095 
00096     } else {
00097         timer.attach_us(flatmove, wtime);
00098     }
00099 }
00100 
00101 void stepper()
00102 {
00103     osEvent evt = osMessageGet(queue, 0);
00104     if (evt.status == osEventMessage) {
00105         stepcmd *cmd = (stepcmd*)evt.value.p;
00106         xDir = DirLED = cmd->dir;
00107         accel = cmd->accel;
00108         speed = cmd->minspeed;
00109         lsteps = cmd->steps;
00110         ttime = 0;
00111         tsteps = 0;
00112         wtime = 1000000 / speed;
00113         float atime = 0;
00114         if (accel == 0) { // linear move
00115             asteps = dsteps = 0;
00116         } else {
00117             // calculate time to reach max speed:
00118             // speed = startspeed + accel * time;
00119             atime = (cmd->maxspeed - cmd->minspeed) / cmd->accel;
00120             asteps = speed * atime + 0.5 * accel * pow(atime,2);
00121             if ((lsteps /2) > asteps) {
00122                 // Accellerate to max speed, then flat, then decellerate
00123                 dsteps = asteps;
00124                 lsteps -= 2 * asteps;
00125             } else {
00126                 // Accellerate to half of the space, then decellerate
00127                 asteps = lsteps/ 2;
00128                 dsteps = lsteps-asteps;
00129                 lsteps = 0;
00130             }
00131         }
00132         sprintf(output, "atime: %2.6f steps: %f, %f, %f wtime: %f\n\r", atime, asteps, lsteps, dsteps, wtime);
00133         counter.reset();
00134         counter.start();
00135         timer.attach_us(accelmove, wtime);
00136         osPoolFree(cmdpool, cmd);
00137         qcnt--;
00138     } else {
00139         timer.attach(stepper, 2);
00140     }
00141 }
00142 void home(float speed)
00143 {
00144     int wtime = int(1000000.0/speed);
00145     xDir = 1;
00146     while (! xHome) {
00147         xStep = 1;
00148         wait_us(5);
00149         xStep = 0;
00150         wait_us(wtime);
00151     }
00152     xDir = 0;
00153     for (int x=0; x<1000; x++) {
00154         xStep = 1;
00155         wait_us(5);
00156         xStep = 0;
00157         wait_us(wtime);
00158     }
00159 }
00160 
00161 int main (void)
00162 {
00163     printf("Keys:\n\r");
00164     printf("0-1: Adjust minimum speed (m/s)\n\r");
00165     printf("3-4: Adjust maximum speed (m/s)\n\r");
00166     printf("5-6: Adjust Accelleration (m/s^2)\n\r");
00167     printf("7-8: Adjust total distance (m)\n\r");
00168     printf("9-0: Steps per meter\n\r");
00169     printf("h = home, p = print result, q = E_STOP, t=toggle direction\n\r");
00170     printf("SPACE = move\n\r");
00171     printf("CTRL-Break = reset\n\r\n\r");
00172     TestLED = 0;
00173     cmdpool = osPoolCreate(osPool(cmdpool));
00174     queue = osMessageCreate(osMessageQ(queue), NULL);
00175     timer.attach(stepper, 2);
00176     bool mydir = false;
00177     etime = 0;
00178     // 902.51 met kleine motor: 0.25/0.8/1.1/80000 = 12.5 micron
00179     float startspeed = 0.25;  // speed in m/s
00180     float maxspeed = 0.8; // speed in m/s
00181     float accel = 1.1; // accelleration in m/s^2
00182     float distance = 1.26; // maximum distance on the rod in m
00183     float steps = 80000; // steps per meter
00184     float precision = 1000000 / steps;
00185     pc.printf("Settings: min: %3.2f, max %3.2f, accel %3.2f, dist %3.2f, steps %ld\n\r",
00186               startspeed, maxspeed, accel, distance, steps);
00187     while (true) {
00188         if (pc.readable()) {
00189             char c = pc.getc();
00190             switch (c) {
00191                 case 'h':
00192                     home(startspeed * steps);
00193                     break;
00194                 case '1':
00195                     if (startspeed > 0) startspeed -= 0.01;
00196                     break;
00197                 case '2':
00198                     startspeed += 0.01;
00199                     break;
00200                 case '3':
00201                     if (maxspeed > startspeed) maxspeed -=0.01;
00202                     break;
00203                 case '4':
00204                     maxspeed += 0.01;
00205                     break;
00206                 case '5':
00207                     if (accel > 0) accel -= 0.01;
00208                     break;
00209                 case '6':
00210                     accel += 0.01;
00211                     break;
00212                 case '7':
00213                     if (distance > 0) distance -= 0.01;
00214                     break;
00215                 case '8':
00216                     distance += 0.01;
00217                     break;
00218                 case '9':
00219                     if (steps > 0) steps -= 100;
00220                     break;
00221                 case '0':
00222                     steps += 100;
00223                     break;
00224                 case 't':
00225                     mydir = !mydir;
00226                     break;
00227                 case 'p':
00228                     pc.printf("Etime: %2.3f, Rtime: %2.3f a,l,d,steps: %f,%f,%f,%f\n\r", (float)(etime/1000000.0), rtime/1000000, asteps, lsteps, dsteps, tsteps);
00229                     break;
00230                 case 'q':
00231                     N_ESTOP = ! N_ESTOP;
00232                     break;
00233                 case ' ':
00234                     if (qcnt < 15) {
00235                         stepcmd *cmd = (stepcmd*)osPoolAlloc(cmdpool);
00236                         cmd -> minspeed = startspeed *steps;
00237                         cmd -> maxspeed = maxspeed * steps;
00238                         cmd -> accel = accel * steps;
00239                         cmd -> steps = distance * steps;
00240 
00241                         pc.printf("Stepsettings: min: %f, max %f, accel %f, distance: %f\n\r",
00242                                   cmd->minspeed, cmd->maxspeed, cmd->accel, cmd->steps, precision);
00243                         cmd -> dir = mydir;
00244                         osStatus res = osMessagePut(queue, (uint32_t)cmd, 0);
00245                         while (res != osOK) {
00246                             osDelay(500);
00247                             res = osMessagePut(queue, (uint32_t)cmd, 0);
00248                         }
00249                         pc.printf("Command sent\n\r", ++qcnt);
00250                         mydir = !mydir;
00251                         osDelay(100);
00252                     } else {
00253                         // pc.printf("Storage full\n\r");
00254                     }
00255                     break;
00256             }
00257             precision = 1000000 / steps;
00258             pc.printf("Settings: min: %3.2f, max %3.2f, accel %3.2f, dist %3.2f, steps %7.0f DIR: %d, precision: %2.3f\n\r",
00259                       startspeed, maxspeed, accel, distance, steps, mydir, precision);
00260         } else {
00261             osDelay(2000);
00262         }
00263         CmdLED = ! CmdLED;
00264         if (strlen(output)>0) {
00265             pc.printf("%s\n\r", output);
00266             sprintf(output, "");
00267         }
00268     }
00269 }