home *** CD-ROM | disk | FTP | other *** search
/ The Starbase One Astronomy & Space Collection / STARBASE_ONE.ISO / a96 / disk07 / altaz.exe / STEPPERS.CPP < prev    next >
Encoding:
C/C++ Source or Header  |  1995-08-09  |  12.3 KB  |  605 lines
  1. #include <time.h>
  2. #include <sys\timeb.h>
  3. #include <dos.h>
  4. #include <values.h>
  5. #include <iostream.h>
  6. #include <iomanip.h>
  7. #include <time.h>
  8. #include <conio.h>
  9. #include <alloc.h>
  10. #include <stdio.h>
  11. #include <except.h>
  12.  
  13. #include "common.h"
  14. #include "badexit.hpp"
  15. #include "video.hpp"
  16. #include "atimes.hpp"
  17. #include "pport.hpp"
  18. #include "cmosclk.hpp"
  19. #include "handpad.hpp"
  20. #include "steppers.hpp"
  21.  
  22. const int MaxHsIx=7;
  23. Byte AHsOut[MaxHsIx+1];
  24. Byte ZHsOut[MaxHsIx+1];
  25.  
  26. Steppers::Steppers
  27. (
  28.     const int InvertOutput,
  29.     const int MaxDelay,
  30.     const int MinDelay,
  31.     const int HsDelayX,
  32.     const int InterruptHs,
  33.     const int HoldReps,
  34.     const int MsRepsTick,
  35.     const int MsDelayX,
  36.     const int MsPause,
  37.     const int Ms,
  38.     // ptr to array of size Ms,containing PWM counts
  39.     int* PWM,
  40.     const unsigned PPortAddr
  41. ):
  42.     Handpad(PPortAddr),
  43.  
  44.     InvertOutput(InvertOutput),
  45.     MaxDelay(MaxDelay),
  46.     MinDelay(MinDelay),
  47.     HsDelayX(HsDelayX),
  48.     InterruptHs(InterruptHs),
  49.     HoldReps(HoldReps),
  50.     MsRepsTick(MsRepsTick),
  51.     MsDelayX(MsDelayX),
  52.     MsPause(MsPause),
  53.     Ms(Ms)
  54. {
  55.     int Winding;
  56.     int MicroStep;
  57.     int CosWinding;
  58.     int SinWinding;
  59.     int IxA;
  60.     int IxB;
  61.  
  62.     // A motor lower nibble, Z motor upper nibble
  63.     if(InvertOutput)
  64.     {
  65.         AOff=0xF;
  66.         ZOff=0xF0;
  67.     }
  68.     else
  69.         ZOff=AOff=0;
  70.  
  71.     MotorPort=PPortAddrOutByte;
  72.     SteppersOff();
  73.  
  74.     MsPerHs=Ms/2;
  75.  
  76.     ADir=CW;
  77.     ZDir=CW;
  78.  
  79.     ActualMsRepsTick=MsRepsTick;
  80.  
  81.     AHsIx=ZHsIx=0;
  82.     AMsIx=ZMsIx=0;
  83.     ASteps=ZSteps=0;
  84.     AccumAMs=AccumZMs=0;
  85.  
  86.     if(MinDelay>=MaxDelay)
  87.         BadExit E("MinDelay>=MaxDelay in Steppers module");
  88.     if(HsDelayX<1)
  89.         BadExit E("HsDelayX must be at least 1");
  90.     if(MsDelayX<1)
  91.         BadExit E("MsDelayX must be at least 1");
  92.  
  93.     // A motor uses bits 0-3 (1,2,4,8)
  94.     AHsOut[0]=1;
  95.     AHsOut[1]=3;
  96.     AHsOut[2]=2;
  97.     AHsOut[3]=6;
  98.     AHsOut[4]=4;
  99.     AHsOut[5]=12;
  100.     AHsOut[6]=8;
  101.     AHsOut[7]=9;
  102.     // Z motor uses bits 4-7 (16,32,64,128)
  103.     for(IxA=0;IxA<=MaxHsIx;IxA++)
  104.         ZHsOut[IxA]=AHsOut[IxA]*16;
  105.  
  106.     // set MaxHsRepsIx
  107.     MaxHsRepsIx=MaxDelay-MinDelay;
  108.     // build HsReps array
  109.     /*    HsReps=(long*)malloc((MaxHsRepsIx+1)*sizeof(long));
  110.         if(HsReps==NULL)
  111.             BadExit E("Problem with malloc of HsReps in steppers module"); */
  112.     try
  113.     {
  114.         HsReps=new long[MaxHsRepsIx+1];
  115.     }
  116.     catch(xalloc)
  117.     {
  118.         BadExit E("Bad call of HsReps=new in steppers module");
  119.     }
  120.     // since MoveHs() rounds up to nearest full step
  121.     MaxHs=MAXLONG-1;
  122.     HsReps[0]=MaxHs;
  123.     for(HsRepsIx=1;HsRepsIx<=MaxHsRepsIx;HsRepsIx++)
  124.         HsReps[HsRepsIx]=(long)MaxDelay/(long)(HsRepsIx+MinDelay);
  125.  
  126.     MaxPWM=PWM[0];
  127.     HsIxToMsIxConvFactor=MaxPWM*MsPerHs;
  128.     // set MaxMsIx;
  129.     // # of microstep waveforms = # of microsteps (Ms) * # of windings (4);
  130.     // # of elements in microstep array = # of microstep waveforms *
  131.     // # of pulse width modulations per microstep voltage (MaxPWM)
  132.     MaxMsIx=4*Ms*MaxPWM-1;
  133.  
  134.     // build microstep arrays
  135.     /* AMs=(Byte*)malloc((MaxMsIx+1)*sizeof(Byte));
  136.     if(AMs==NULL)
  137.         BadExit E("Problem with malloc of AMs in steppers module");
  138.     ZMs=(Byte*)malloc((MaxMsIx+1)*sizeof(Byte));
  139.     if(ZMs==NULL)
  140.         BadExit E("Problem with malloc of ZMs in steppers module"); */
  141.     try
  142.     {
  143.         AMs=new Byte[MaxMsIx+1];
  144.         ZMs=new Byte[MaxMsIx+1];
  145.     }
  146.     catch(xalloc)
  147.     {
  148.         BadExit E("Bad call of AMs=new and ZMs=new in steppers module");
  149.     }
  150.  
  151.     // zero-out arrays
  152.     for(IxA=0;IxA<=MaxMsIx;IxA++)
  153.         ZMs[IxA]=AMs[IxA]=0;
  154.  
  155.     // start cos winding at bit #1 and sin winding at bit #2,
  156.     // cos winding will start at the highest microstep voltage and ramp 
  157.     // down to zero volts while sin winding will start at zero volts and
  158.     // ramp up
  159.     CosWinding=1;
  160.     SinWinding=2;
  161.  
  162.     // start base index of microstep array at the beginning,
  163.     // index is incremented by MaxPWM (# of pulse width modulations
  164.     // per microstep voltage)
  165.     IxA=0;
  166.  
  167.     // microstep array has 4 windings to build
  168.     for(Winding=1;Winding<=4;Winding++)
  169.     {
  170.         // each winding has Ms # of microsteps to build
  171.         for(MicroStep=0;MicroStep<Ms;MicroStep++)
  172.         {
  173.             // turn on the cos bit for PWM[MicroStep] # of times
  174.             for(IxB=0;IxB<PWM[MicroStep];IxB++)
  175.                 AMs[IxA+IxB]+=CosWinding;
  176.             // Microstep==0 has no contribution from SinWinding
  177.             if(MicroStep>0)
  178.                 /* the sin microstep that corresponds to the cos
  179.                 microstep of Ms is Ms-MicroStep,
  180.                 ie, if the total # of microsteps = 10
  181.                 microstep     cos     sin
  182.                 0             PWM[0]  n/a
  183.                 1             PWM[1]  PWM[9]
  184.                 2             PWM[2]  PWM[8]
  185.                 3             PWM[3]  PWM[7]
  186.                 4             PWM[4]  PWM[6]
  187.                 5             PWM[5]  PWM[5]
  188.                 6             PWM[6]  PWM[4]
  189.                 7             PWM[7]  PWM[3]
  190.                 8             PWM[8]  PWM[2]
  191.                 9             PWM[9]  PWM[1] */
  192.                 for(IxB=0;IxB<PWM[Ms-MicroStep];IxB++)
  193.                     AMs[IxA+IxB]+=SinWinding;
  194.             // increment to start of next microstep voltage
  195.             IxA+=MaxPWM;
  196.         }
  197.         // goto next windings
  198.         CosWinding*=2;
  199.         SinWinding*=2;
  200.         if(SinWinding>8)
  201.             SinWinding=1;
  202.     }
  203.     // Z motor bits occupy upper nibble
  204.     for(IxA=0;IxA<=MaxMsIx;IxA++)
  205.         ZMs[IxA]=AMs[IxA]*16;
  206.  
  207.     if(InvertOutput)
  208.     {
  209.         for(IxA=0;IxA<=MaxHsIx;IxA++)
  210.         {
  211.             AHsOut[IxA]=AOff-AHsOut[IxA];
  212.             ZHsOut[IxA]=ZOff-ZHsOut[IxA];
  213.         }
  214.         for(IxA=0;IxA<=MaxMsIx;IxA++)
  215.         {
  216.         AMs[IxA]=AOff-AMs[IxA];
  217.         ZMs[IxA]=ZOff-ZMs[IxA];
  218.         }
  219.     }
  220. }
  221.  
  222. Steppers::~Steppers(void)
  223. {
  224.     delete(HsReps);
  225.     delete(AMs);
  226.     delete(ZMs);
  227. }
  228.  
  229. void Steppers::SteppersOff(void)
  230. {
  231.     outportb(MotorPort,AOff+ZOff);
  232. }
  233.  
  234. void Steppers::Hold(void)
  235. {
  236.     Byte AOut,ZOut;
  237.     int RepCount,HsDelayXCount,DelayCount;
  238.  
  239.     if(AHoldFlag)
  240.         AOut=AHsOut[AHsIx];
  241.     else
  242.         AOut=AOff;
  243.     if(ZHoldFlag)
  244.         ZOut=ZHsOut[ZHsIx];
  245.     else
  246.         ZOut=ZOff;
  247.     outportb(MotorPort,AOut+ZOut);
  248.     for(RepCount=0;RepCount<HoldReps;RepCount++)
  249.         for(HsDelayXCount=0;HsDelayXCount<HsDelayX;HsDelayXCount++)
  250.             for(DelayCount=0;DelayCount<HsRepsIx+MinDelay;DelayCount++)
  251.             ;
  252. }
  253.  
  254. int Steppers::ClockTick(void)
  255. {
  256.     static long HoldClock;
  257.  
  258.     long Clock=clock();
  259.     if(Clock==HoldClock)
  260.         return No;
  261.     else
  262.     {
  263.         HoldClock=Clock;
  264.         return Yes;
  265.     }
  266. }
  267.  
  268. void Steppers::PauseUntilClockTick(void)
  269. {
  270.     ClockTick();
  271.     while(!ClockTick())
  272.     ;
  273. }
  274.  
  275. void Steppers::DoOneHs(void)
  276. {
  277.     Byte AOut;
  278.     Byte ZOut;
  279.     int HsDelayXCount,DelayCount;
  280.  
  281.     if(ASteps)
  282.     {
  283.         if(ADir==CW)
  284.         {
  285.             AHsIx++;
  286.             if(AHsIx>MaxHsIx)
  287.                 AHsIx=0;
  288.             AccumAMs+=MsPerHs;
  289.         }
  290.         else
  291.         {
  292.             AHsIx--;
  293.             if(AHsIx<0)
  294.                 AHsIx=MaxHsIx;
  295.             AccumAMs-=MsPerHs;
  296.         }
  297.         ASteps--;
  298.         AOut=AHsOut[AHsIx];
  299.     }
  300.     else
  301.         AOut=AOff;
  302.     AMsIx=AHsIx*HsIxToMsIxConvFactor;
  303.  
  304.     if(ZSteps)
  305.     {
  306.         if(ZDir==CW)
  307.         {
  308.             ZHsIx++;
  309.             if(ZHsIx>MaxHsIx)
  310.                 ZHsIx=0;
  311.             AccumZMs+=MsPerHs;
  312.         }
  313.         else
  314.         {
  315.             ZHsIx--;
  316.             if(ZHsIx<0)
  317.                 ZHsIx=MaxHsIx;
  318.             AccumZMs-=MsPerHs;
  319.         }
  320.         ZSteps--;
  321.         ZOut=ZHsOut[ZHsIx];
  322.     }
  323.     else
  324.         ZOut=ZOff;
  325.     ZMsIx=ZHsIx*HsIxToMsIxConvFactor;
  326.  
  327.     outportb(MotorPort,AOut+ZOut);
  328.     // repetitions of delay counts
  329.     for(HsDelayXCount=0;HsDelayXCount<HsDelayX;HsDelayXCount++)
  330.         for(DelayCount=0;DelayCount<HsRepsIx+MinDelay;DelayCount++)
  331.         ;
  332.     SetHandPadOKFlag();
  333. }
  334.  
  335. void Steppers::MoveHs(void)
  336. {
  337.     Flag InterruptFlag=No;
  338.     long MvmtHs;
  339.     long RampHs;
  340.     long SameSpeedHs;
  341.  
  342.     InitHandPad=HandPad;
  343.     HandPadOKFlag=Yes;
  344.     // end on next fullstep
  345.     if(ASteps%2)
  346.         ASteps++;
  347.     if(ZSteps%2)
  348.         ZSteps++;
  349.     while(HandPadOKFlag && (ASteps || ZSteps))
  350.     {
  351.         // do smaller # of steps 1st unless a motor has 0 steps to do:
  352.         // flags set for Hold() depend on this mvmt algorithm
  353.         if(ASteps==0)
  354.             MvmtHs=ZSteps;
  355.         else if(ZSteps==0)
  356.             MvmtHs=ASteps;
  357.         else if(ASteps<ZSteps)
  358.             MvmtHs=ASteps;
  359.         else
  360.             MvmtHs=ZSteps;
  361.         RampHs=MvmtHs/2;
  362.         /* set for max delay giving slowest speed (index is decremented
  363.         in while loop before being used for 1st time) */
  364.         HsRepsIx=MaxHsRepsIx+1;
  365.         /* if steps to move exceed InterruptHs, then disable interrupts for
  366.         smoothest timing pulses to allow faster speeds */
  367.         if(RampHs>InterruptHs)
  368.         {
  369.             InterruptFlag=Yes;
  370.             disable();
  371.         }
  372.         // hold motor(s) position for brief time to align
  373.         if(ASteps)
  374.             AHoldFlag=On;
  375.         else
  376.             AHoldFlag=Off;
  377.         if(ZSteps)
  378.             ZHoldFlag=On;
  379.         else
  380.             ZHoldFlag=Off;
  381.         Hold();
  382.         // ramp up
  383.         while(HandPadOKFlag && RampHs)
  384.         {
  385.             HsRepsIx--;
  386.             SameSpeedHs=HsReps[HsRepsIx];
  387.             while(HandPadOKFlag && RampHs && SameSpeedHs)
  388.             {
  389.                 DoOneHs();
  390.                 RampHs--;
  391.                 SameSpeedHs--;
  392.             }
  393.         }
  394.         // ramp down
  395.         while(HsRepsIx<=MaxHsRepsIx)
  396.         {
  397.             while(SameSpeedHs<HsReps[HsRepsIx])
  398.             {
  399.                 /* if move cancelled while moving at max speed, then immediate
  400.                 ramp down */
  401.                 if(!HandPadOKFlag && HsRepsIx==0)
  402.                     SameSpeedHs=HsReps[HsRepsIx]-1;
  403.                 DoOneHs();
  404.                 RampHs--;
  405.                 SameSpeedHs++;
  406.             }
  407.             SameSpeedHs=0;
  408.             HsRepsIx++;
  409.         }
  410.         // hold motor(s) position for brief time to brake
  411.         Hold();
  412.         AHoldFlag=ZHoldFlag=Off;
  413.     }
  414.     SteppersOff();
  415.     if(InterruptFlag)
  416.     {
  417.         SetDOSToCMOS();
  418.         enable();
  419.     }
  420.     PauseUntilClockTick();
  421. }
  422.  
  423. void Steppers::MoveMs(void)
  424. {
  425.     int IxA,IxB,IxC;
  426.     int AMsToDo;
  427.     int ZMsToDo;
  428.     double AMsRunningTotal=0;
  429.     double ZMsRunningTotal=0;
  430.  
  431.     MsAlignFlag=No;
  432.     ActualMsRepsTick=0;
  433.  
  434.     // allow skipping of microsteps up to halfstepping
  435.     double IncrAMs=(double)ASteps/(double)(MsRepsTick);
  436.     if(IncrAMs>MsPerHs)
  437.         IncrAMs=MsPerHs;
  438.     double IncrZMs=(double)ZSteps/(double)(MsRepsTick);
  439.     if(IncrZMs>MsPerHs)
  440.         IncrZMs=MsPerHs;
  441.  
  442.     do
  443.     {
  444.         if(ASteps)
  445.         {
  446.             AMsRunningTotal+=IncrAMs;
  447.             AMsToDo=AMsRunningTotal;
  448.             AMsRunningTotal-=AMsToDo;
  449.             ASteps-=AMsToDo;
  450.             if(ADir==CW)
  451.             {
  452.                 AMsIx+=MaxPWM*AMsToDo;
  453.                 if(AMsIx>MaxMsIx)
  454.                     AMsIx-=MaxMsIx+1;
  455.                 AccumAMs+=AMsToDo;
  456.             }
  457.             else
  458.             {
  459.                 AMsIx-=MaxPWM*AMsToDo;
  460.                 if(AMsIx<0)
  461.                     AMsIx+=MaxMsIx+1;
  462.                 AccumAMs-=AMsToDo;
  463.             }
  464.         }
  465.         if(ZSteps)
  466.         {
  467.             ZMsRunningTotal+=IncrZMs;
  468.             ZMsToDo=ZMsRunningTotal;
  469.             ZMsRunningTotal-=ZMsToDo;
  470.             ZSteps-=ZMsToDo;
  471.             if(ZDir==CW)
  472.             {
  473.                 ZMsIx+=MaxPWM*ZMsToDo;
  474.                 if(ZMsIx>MaxMsIx)
  475.                     ZMsIx-=MaxMsIx+1;
  476.                 AccumZMs+=ZMsToDo;
  477.             }
  478.             else
  479.             {
  480.                 ZMsIx-=MaxPWM*ZMsToDo;
  481.                 if(ZMsIx<0)
  482.                     ZMsIx+=MaxMsIx+1;
  483.                 AccumZMs-=ZMsToDo;
  484.             }
  485.         }
  486.         // send voltages to motors
  487.         for(IxA=0;IxA<MaxPWM;IxA++)
  488.             // repeat outportb() MsDelayX times
  489.             for(IxB=0;IxB<MsDelayX;IxB++)
  490.                 outportb(MotorPort,AMs[AMsIx+IxA]+ZMs[ZMsIx+IxA]);
  491.         SteppersOff();
  492.         // pause for MsPause counts
  493.         for(IxC=0;IxC<MsPause;IxC++)
  494.             SteppersOff();
  495.         // one microstepping voltage waveform finished
  496.         ActualMsRepsTick++;
  497.     } while(!ClockTick());
  498.  
  499.     AHsIx=AMsIx/HsIxToMsIxConvFactor;
  500.     ZHsIx=ZMsIx/HsIxToMsIxConvFactor;
  501. }
  502.  
  503. void Steppers::AlignMs(void)
  504. {
  505.     if(MsAlignFlag)
  506.         PauseUntilClockTick();
  507.     else
  508.     {
  509.         // find microsteps to nearest fullstep
  510.         ASteps=(AMsIx/MaxPWM)%Ms;
  511.         if(ASteps && ADir==CW)
  512.             ASteps=Ms-ASteps;
  513.         ZSteps=(ZMsIx/MaxPWM)%Ms;
  514.         if(ZSteps && ZDir==CW)
  515.             ZSteps=Ms-ZSteps;
  516.         Steppers::MoveMs();
  517.         MsAlignFlag=Yes;
  518.     }
  519. }
  520.  
  521. void Steppers::Test(int TestMs,long TestHs)
  522. {
  523.     int Input=0;
  524.     int IxA,IxB;
  525.  
  526.     clrscr();
  527.     cout<<endl<<endl<<endl<<"Starting steppers module test:"<<endl;
  528.     if(TestMs>MsRepsTick*MsPerHs)
  529.         TestMs=MsRepsTick*MsPerHs;
  530.     cout<<endl<<"microstepping at "<<TestMs*18.2<<" microsteps/sec";
  531.     cout<<endl<<ContMsg<<endl;
  532.     ASteps=TestMs;
  533.     PauseUntilClockTick();
  534.     while(!kbhit())
  535.     {
  536.         Steppers::MoveMs();
  537.         ASteps+=TestMs;
  538.     }
  539.     Steppers::AlignMs();
  540.     cout<<endl<<"MsRepsTick="<<MsRepsTick<<endl;
  541.     getch();
  542.     cout<<endl<<"halfstepping test:";
  543.     Input=' ';
  544.     while(Input!='q')
  545.     {
  546.         cout<<endl<<"press 'm' to move, 'q' to quit";
  547.         Input=getch();
  548.         if(Input=='m')
  549.         {
  550.             ASteps=ZSteps=TestHs;
  551.             Steppers::MoveHs();
  552.         }
  553.     }
  554.     cout<<endl<<endl<<"individual microstep test:";
  555.     Input=' ';
  556.     while(Input!='q')
  557.     {
  558.         cout<<endl<<"press 'l' for left, 'r' for right, 'q' to quit"<<endl;
  559.         Input=getch();
  560.         if(Input=='l')
  561.             ADir=CW;
  562.         else if(Input=='r')
  563.             ADir=CCW;
  564.         else
  565.             continue;
  566.         PauseUntilClockTick();
  567.         for(IxA=0;IxA<Ms;IxA++)
  568.         {
  569.             cout<<"  "<<IxA;
  570.             ASteps=ZSteps=1;
  571.             Steppers::MoveMs();
  572.             for(IxB=0;IxB<(int)ClockTicksSec;IxB++)
  573.                 Steppers::MoveMs();
  574.         }
  575.     }
  576.     cout<<endl<<endl<<ContMsg;
  577.     getch();
  578. }
  579.  
  580. void Steppers::Test1(void)
  581. {
  582.     int Input=0;
  583.     int Ix=0;
  584.     long Fs=2;
  585.  
  586.     clrscr();
  587.     cout<<endl<<endl<<endl<<"Starting steppers module1 test:"<<endl;
  588.     ASteps=ZSteps=Fs;
  589.     Steppers::MoveHs();
  590.     while(Input!='q')
  591.     {
  592.         cout<<"press 'm' to move one full step, 'q' to quit"<<endl;
  593.         Input=getch();
  594.         if(Input=='m')
  595.         {
  596.             ASteps=ZSteps=Fs;
  597.             Steppers::MoveHs();
  598.             cout<<"full step = "<<++Ix<<"   ";
  599.         }
  600.     }
  601.     cout<<endl<<endl<<ContMsg;
  602.     getch();
  603. }
  604.  
  605.