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; }