int stepper_motor::go(int motnum, int steps) {
if (motnum<0 || motnum>2) {return(emsg(3));};
// do it //
if (!is_motor_ready(motnum)) {return(emsg(4));};
ioctl(handle,SM32Post | mcGo | MOT[motnum], steps);
while (get_mot_pos(motnum)!=steps) {
}
if (waitmode)
if (!is_motor_there(motnum)) {return(emsg(5));};
nanosleep(&twenty_millisec,&time_left);
return(0);
}
int parse_command(char * cmd) { /* returns 0 on success, or error code */
char cmdi2[MAXINLEN+1];
int idx, retval, converted,arg2;
int motnum, steps, inspeed;
float involts;
if (1!=sscanf(cmd,"%s",cmdi2)){
idx=0; /* no command */
} else {
/* get command number */
for(idx=NUMCOMMANDS;(idx>0)&&(strcmp(cmdi2,commands[idx]));idx--);
};
retval=0;
#ifdef DEBUG
printf("command:>%s<, cmd index:%d\n",cmd,idx);
#endif
switch (idx) {
case 1:case 7: /* terminate */
retval=1;break;
case 2: /* go to a certain position */
switch(sscanf(cmd,"%s%d%d",cmdi2,&motnum,&steps)) {
case 2: steps=0;
case 3:
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
/* do it */
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcGo;sm30cmd.sel=(1<<motnum);sm30cmd.data[motnum]=steps;
ioctl(handle,SM30Write,&sm30cmd);
if (waitmode)
if (!is_motor_there(motnum)) {retval=-emsg(5);break;};
nanosleep(&twenty_millisec,&time_left);
break;
default:
retval=-emsg(2);
};
break;
case 3: /* init command */
arg2=sscanf(cmd,"%s%d%f%d",cmdi2,&motnum,&involts,&inspeed);
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
switch (arg2) {
case 4:
if (inspeed<0.0 || inspeed >MAXSPEED) {retval = -emsg(6);break;};
speed[motnum]=inspeed;
case 3:
if (involts<0.0 || involts >MAXVOLT) {retval = -emsg(5);break;};
volts[motnum]=involts;
case 2:
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcU;sm30cmd.sel=(1<<motnum); /* set voltage */
sm30cmd.data[motnum]=(int)(volts[motnum]*10);
ioctl(handle,SM30Write,&sm30cmd);
nanosleep(&twenty_millisec,&time_left);
sm30cmd.cmd=mcPosMode;sm30cmd.sel=(1<<motnum);
sm30cmd.data[motnum]=(movemode[motnum]?mmMove:mmPos);
ioctl(handle,SM30Write,&sm30cmd); /* set position/move mode */
nanosleep(&twenty_millisec,&time_left);
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcF;sm30cmd.sel=(1<<motnum); /* set speed */
sm30cmd.data[motnum]=(int)(movemode[motnum]?0:speed[motnum]);
ioctl(handle,SM30Write,&sm30cmd);
nanosleep(&twenty_millisec,&time_left);
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcPower;sm30cmd.sel=(1<<motnum);sm30cmd.data[motnum]=1;
ioctl(handle,SM30Write,&sm30cmd);/* switch on */
nanosleep(&twenty_millisec,&time_left);
break;
default:
retval=-emsg(2);
};
break;
case 4:case 5: /* on/off command */
arg2=sscanf(cmd,"%s%d",cmdi2,&motnum);
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
switch (arg2) {
case 2:
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcPower;sm30cmd.sel=(1<<motnum);
sm30cmd.data[motnum]=(idx==4?1:-1);
ioctl(handle,SM30Write,&sm30cmd);/* switch on */
break;
default:
retval=-emsg(2);
};
break;
case 6: /* set pos command */
steps=0;
switch (sscanf(cmd,"%s%d%d",cmdi2,&motnum,&steps)) {
case 3:case 2:
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcPosition;sm30cmd.sel=(1<<motnum);
sm30cmd.data[motnum]=steps;
ioctl(handle,SM30Write,&sm30cmd);/* switch on */
break;
default:
retval=-emsg(2);
};
break;
case 8: /* set actual and default voltage */
switch (sscanf(cmd,"%s%d%f",cmdi2,&motnum,&involts)) {
case 2:
involts=DEFAULT_VOLT;
break;
case 3:
if (involts<0.0 || involts >MAXVOLT) {retval = -emsg(5);break; };
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
break;
default:
retval = -emsg(2);
};
if (retval==0) {
volts[motnum]=involts;
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcU;sm30cmd.sel=(1<<motnum);
sm30cmd.data[motnum]=(int)(10.0*volts[motnum]);
ioctl(handle,SM30Write,&sm30cmd);/* set voltage */
};
break;
case 9: /* set actual and default speed */
switch (sscanf(cmd,"%s%d%d",cmdi2,&motnum,&inspeed)) {
case 2:
inspeed=DEFAULT_SPEED;
break;
case 3:
if (!movemode)
if (inspeed<0.0 || inspeed >MAXSPEED) {retval = -emsg(6);break; };
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
break;
default:
retval = -emsg(2);
};
if (retval==0) {
speed[motnum]=inspeed;
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcF;sm30cmd.sel=(1<<motnum);
sm30cmd.data[motnum]=(int)speed[motnum];
ioctl(handle,SM30Write,&sm30cmd);/* set speed */
};
break;
case 10: /* reset command */
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcReset;
ioctl(handle,SM30Write,&sm30cmd);
break;
case 11: /* break command */
switch (sscanf(cmd,"%s%d",cmdi2,&motnum)) {
case 2:
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
if (!is_motor_ready()) {retval=-emsg(4);break;};
sm30cmd.cmd=mcBreak;sm30cmd.sel=(1<<motnum);
ioctl(handle,SM30Write,&sm30cmd);
break;
default:
retval = -emsg(2);
};
break;
case 12:case 13: /* vmode/pmode command */
switch (sscanf(cmd,"%s%d",cmdi2,&motnum)) {
case 2:
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
if (!is_motor_ready()) {retval=-emsg(4);break;};
movemode[motnum]=(idx=12?1:0);;
sm30cmd.cmd=mcPosMode;sm30cmd.sel=(1<<motnum);
sm30cmd.data[motnum]=(movemode[motnum]?mmMove:mmPos);
ioctl(handle,SM30Write,&sm30cmd);
break;
default:
retval = -emsg(2);
};
default:case 0: /* no command */
};
return retval;
}
int main(int argc, char * argv[]) {
int idx, ir, cmo;
int inh=0; /* stdin */
/* open stepper motor device */
handle=open(DEVICE,O_RDWR);
if (handle==-1) return -emsg(1);
do {
/* read in one line, waiting for newline */
idx=0;
while ((ir=read(inh,&cmd[idx],1))==1 && idx<MAXINLEN) {
if (cmd[idx]=='\n' || cmd[idx]==';') break;
idx++;
};
cmd[idx]='\0';
if (idx==MAXINLEN)
while ((ir=read(inh,&cmo,1))==1) if (cmo=='n' || cmo == ',') break;
/* command parser */
cmo=parse_command(cmd);
#ifdef DEBUG
printf("parser return: %d\n",cmo);
#endif
} while (!cmo);
close(handle);
return 0;
}
int stepper_motor::parse_command(const char * cmd) { // returns 0 on success, or error code //
std::cout<<"Debug"<<std::endl;
char cmdi2[MAXINLEN+1];
int idx, retval, converted,arg2;
int motnum, steps, inspeed;
float involts;
if (1!=sscanf(cmd,"%s",cmdi2)){
idx=0; // no command //
} else {
// get command number //
for(idx=NUMCOMMANDS;(idx>0)&&(strcmp(cmdi2,(commands[idx]).c_str()));idx--);
};
retval=0;
#ifdef DEBUG
printf("command:>%s<, cmd index:%d\n",cmd,idx);
#endif
std::cout<<idx<<std::endl;
switch (idx) {
case 1:case 7: // terminate //
retval=1;break;
case 2: // go to a certain position //
switch(sscanf(cmd,"%s%d%d",cmdi2,&motnum,&steps)) {
case 2: steps=0;
case 3:
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
// do it //
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
ioctl(handle,SM32Post | mcGo | MOT[motnum], steps);
if (waitmode)
if (!is_motor_there(motnum)) {retval=-emsg(5);break;};
nanosleep(&twenty_millisec,&time_left);
break;
default:
retval=-emsg(2);
};
break;
case 3: // init command //
arg2=sscanf(cmd,"%s%d%f%d",cmdi2,&motnum,&involts,&inspeed);
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
switch (arg2) {
case 4:
if (inspeed<0.0 || inspeed >MAXSPEED) {
retval = -emsg(6);break;};
speed[motnum]=inspeed;
case 3:
if (involts<0.0 || involts >MAXVOLT) {
retval = -emsg(5);break;};
volts[motnum]=involts;
case 2:
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
// set voltage //
ioctl(handle,SM32Post | mcU | MOT[motnum],
(int)(volts[motnum]*10.));
// set pos / move mode //
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
ioctl(handle,SM32Post | mcPosMode | MOT[motnum],
(movemode[motnum]?mmMove:mmPos));
// set speed //
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
ioctl(handle,SM32Post | mcF | MOT[motnum],
(int)(movemode[motnum]?0:speed[motnum]));
// switch on //
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
ioctl(handle,SM32Post | mcPower | MOT[motnum], 1);
break;
default:
retval=-emsg(2);
};
break;
case 4:case 5: // on/off command //
arg2=sscanf(cmd,"%s%d",cmdi2,&motnum);
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
switch (arg2) {
case 2:
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
ioctl(handle,SM32Post | mcPower |MOT[motnum],
(idx==4?1:0));
break;
default:
retval=-emsg(2);
};
break;
case 6: // set pos command //
steps=0;
switch (sscanf(cmd,"%s%d%d",cmdi2,&motnum,&steps)) {
case 3:case 2:
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
ioctl(handle, SM32Post| mcPosition | MOT[motnum], steps);
break;
default:
retval=-emsg(2);
};
break;
case 8: // set actual and default voltage //
switch (sscanf(cmd,"%s%d%f",cmdi2,&motnum,&involts)) {
case 2:
involts=DEFAULT_VOLT;
break;
case 3:
if (involts<0.0 || involts >MAXVOLT) {
retval = -emsg(5);break; };
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
break;
default:
retval = -emsg(2);
};
if (retval==0) {
volts[motnum]=involts;
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
// set voltage //
ioctl(handle,SM32Post | mcU | MOT[motnum],
(int)(10.0*volts[motnum]));
};
break;
case 9: // set actual and default speed //
switch (sscanf(cmd,"%s%d%d",cmdi2,&motnum,&inspeed)) {
case 2:
inspeed=DEFAULT_SPEED;
break;
case 3:
if (!movemode)
if (inspeed<0.0 || inspeed >MAXSPEED) {
retval = -emsg(6);break; };
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
break;
default:
retval = -emsg(2);
};
if (retval==0) {
speed[motnum]=inspeed;
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
// set speed //
ioctl(handle,SM32Post | mcF | MOT[motnum], (int)speed[motnum]);
};
break;
case 10: // reset command //
if (!is_motor_ready(0)) {retval=-emsg(4);break;};
ioctl(handle, SM32Post | mcReset, 0);
break;
case 11: // break command //
switch (sscanf(cmd,"%s%d",cmdi2,&motnum)) {
case 2:
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
ioctl(handle,SM32Post | mcBreak | MOT[motnum],0);
break;
default:
retval = -emsg(2);
};
break;
case 12:case 13: // vmode/pmode command //
switch (sscanf(cmd,"%s%d",cmdi2,&motnum)) {
case 2:
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
if (!is_motor_ready(motnum)) {retval=-emsg(4);break;};
movemode[motnum]=(idx=12?1:0);
ioctl(handle,SM32Post | mcPosMode | MOT[motnum],
(movemode[motnum]?mmMove:mmPos));
break;
default:
retval = -emsg(2);
};
case 14: // get position
sscanf(cmd,"%s%d",cmdi2,&motnum);
if (motnum<0 || motnum>2) {retval=-emsg(3);break;};
//printf("%ld\n", get_mot_pos(motnum), "\n");
fprintf(fp, "%ld\n", get_mot_pos(motnum), "\n");
fflush(fp);
break;
case 0:default: ;
// no command //
};
return retval;
}
