Servo and stepper easing example

stepper2small.pde

// This Arduino example demonstrates bidirectional operation of a 
// 28BYJ-48, which is readily available on eBay for $4.25 inc shipping,
// using a ULN2003 interface board to drive the stepper.
#include <Servo.h> 
#define dw digitalWrite
#define dm delayMicroseconds
#define wr32 Serial.write(32);
#define wr13 Serial.write(10);
const int mp1 = 4;	// Blue   - 28BYJ48 pin 1
const int mp2 = 5;	// Pink   - 28BYJ48 pin 2
const int mp3 = 6;	// Yellow - 28BYJ48 pin 3
const int mp4 = 7;	// Orange - 28BYJ48 pin 4
                        // Red    - 28BYJ48 pin 5 VCC
int motorSpeed=3000;    // set stepper speed, period actually
int stepnum=0;          // current microstep 0-7
int movecnt=0;          // running total of steps taken in move(), &=4095 in moveto
long ems,mst,m;         // millis()
int maxmove=0;          // ease()
float prev=45;          // eo_servo
Servo myservo;          // 22-155 deg

void setup() {
  pinMode(mp1, OUTPUT);
  pinMode(mp2, OUTPUT);
  pinMode(mp3, OUTPUT);
  pinMode(mp4, OUTPUT);
  pinMode(13, OUTPUT);  //onboard LED
  //Serial.begin(9600);
  myservo.attach(11);  //pin 11 end of row
  ems=micros(); //init easeoften() end of setup
}
void loop(){
//if(!eo_servo()){
if(!(m=easeoften(0,0))){
//if(!(m=eo_degrpm(95,1000))){
  //Serial.println(maxmove/11.377);
  //Serial.println(millis()/1000.0);
  //myservo.write(140);delay(300);myservo.write(22);delay(300);
  while(1);
}
//delay(10);  // <chgms/multsp-10
delayMicroseconds(m/2); //m/2 safe
}
int eo_servo(){
//multsp <1 jerky >4 plateau shaped
const float cellD2=6514; //ease.xls
const float multsp=3,multdeg=1.1;
const int degoffset=-22; //-22
const int chgms=74,mspa[]={ //from easeoften()
6450,3640,2413,1751,1338,1068,892,785,732,732,785,892,1068,1338,1747,2422,3640,6450,21015,
-21015,-6450,-3640,-2422,-1747,-1338,-1068,-892,-785,-732,-732,-785,-892,-1068,-1338,-1751,-2413,-3640,-6450,0};
int ii=chgms/multsp;
//int n=millis();
while(((m=millis())%ii)>5); //always catch 0? servo interrupt skipped this ==0
//Serial.print(m-n);wr32
int i=mspa[m/ii];
if(!i)return(0);
prev+=cellD2/i;
int deg=(prev+0.5+degoffset)*multdeg;
//Serial.print(m/ii);wr32
//Serial.print(prev);wr32
//Serial.print(cellD2/i);wr32
//Serial.print(deg);wr32 
myservo.write(deg);
return(deg);
}
int eo_degrpm(int deg, int rpm100) {
//no floats used in easeoften()
const int chgms=74,rpm=2000,degmov=90; //from ease.xls
deg=(float(deg)/degmov-1)*chgms+0.5;
rpm100=(float(rpm100)/rpm-1)*chgms+0.5;
while(easeoften(deg,rpm100));return(0);
//return(easeoften(deg,rpm100)); //calcs floats every iteration
}
int easeoften(int adjdist, int adjspeed){
//do not change adjspeed,dist once you begin
//call every << return() micros about r/2
//line interval 74ms 20RPM see ease.xls
const int chgms=74,mspa[]={
6450,3640,2413,1751,1338,1068,892,785,732,732,785,892,1068,1338,1747,2422,3640,6450,21015,
-21015,-6450,-3640,-2422,-1747,-1338,-1068,-892,-785,-732,-732,-785,-892,-1068,-1338,-1751,-2413,-3640,-6450,0};
int easi=millis()/(chgms-adjspeed+adjdist); //might be late, plus adjspeed is shorter cuz faster
long i=abs(mspa[easi]);
if(!i)return(0); //done
long j=abs(mspa[easi+1]);  //next speed wrong 0 during last line
i-=long(adjspeed)*i/chgms;
j-=long(adjspeed)*j/chgms;  //not important used for return only
boolean bcw=(mspa[easi]>0);
if(bcw)stepnum++; else stepnum--;
if(bcw)movecnt++; else movecnt--;
if(movecnt>maxmove)maxmove=movecnt;
motorSpeed=1; //1?
ems+=i;
//no time to print
//Serial.print(movecnt);wr32
//Serial.print(easi);wr32
//Serial.print(ems);wr13
while(micros()<ems){  //blocking do something else too?  
};
st07();
//if(movecnt%55==0){  //5 deg
//Serial.print(movecnt/11.377);wr32}
if(j<i)i=j; //j or i smaller guess worst case, i is usually correct, so wrong 1/2 of the time
return(i); //could be too short to wait for next step, j not adj4speed above?, check micros()>ems+i-100 in loop
}
void st07(){
if(stepnum==-1)stepnum=7;
if(stepnum== 8)stepnum=0;
switch(stepnum){
case 0:
  dw(mp4, HIGH);
  dw(mp3, LOW);
  dw(mp2, LOW);
  dw(mp1, LOW);
  break;
case 1:
  dw(mp4, HIGH);
  dw(mp3, HIGH);
  dw(mp2, LOW);
  dw(mp1, LOW);
  break;
case 2:
  dw(mp4, LOW);
  dw(mp3, HIGH);
  dw(mp2, LOW);
  dw(mp1, LOW);
  break;
case 3:
  dw(mp4, LOW);
  dw(mp3, HIGH);
  dw(mp2, HIGH);
  dw(mp1, LOW);
  break;
case 4:
  dw(mp4, LOW);
  dw(mp3, LOW);
  dw(mp2, HIGH);
  dw(mp1, LOW);
  break;
case 5:
  dw(mp4, LOW);
  dw(mp3, LOW);
  dw(mp2, HIGH);
  dw(mp1, HIGH);
  break;
case 6:
  dw(mp4, LOW);
  dw(mp3, LOW);
  dw(mp2, LOW);
  dw(mp1, HIGH);
  break;
case 7:
  dw(mp4, HIGH);
  dw(mp3, LOW);
  dw(mp2, LOW);
  dw(mp1, HIGH);
  break;
}
dm(motorSpeed);
}