/* ------------------------------------------------------------- * * shutter92.c * * Project: IMACS * * shutter controller program * * v0.01 2000-12-20 Christoph C. Birk, OCIW, Pasadena * v0.10 2001-01-12 1st beta * v0.20 2001-01-16 2nd beta * v0.30 2001-03-22 power-down during wait * v0.40 use demand position * v0.50 2001-03-23 new init/move_blade() * v0.60 2001-03-29 add 2nd serial interface (Linux) * v0.61 2001-04-30 add 'open', 'close' commands * v0.62 2001-05-02 add 'pause', 'continue' commands * v0.63 2001-07-20 #ifdef USE_PORTB * v0.64 2001-07-26 asymmetric POWERON_DELAY * v0.70 2002-05-10 2 shutter (4 blade) control * v0.80 2003-01-15 init-cur=100%, lm_closed[2] * v0.90 2003-02-16 ignore move_blade() error * v0.91 -03- single blade version * v0.92 2003-04-02 dual blades, remove unnecessary variables, * ignore init/move-errors * * todo add return exptime,etc if no '=' on command line * * ------------------------------------------------------------- */ /* DEFINEs ----------------------------------------------------- */ #define DEBUG 0 //#define USE_PORTB #define BLADE_A_LED 0 #define BLADE_B_LED 1 #define BLADE_C_LED 2 #define BLADE_D_LED 3 #define LED_OFF 1 // led status #define LED_ON 0 #define BLINK_DELAY 80 // blink interval (ms) #define BLADE_MOVING 0 // blade status #define BLADE_OPEN 1 #define BLADE_CLOSED 2 #define E_linmot 1 // serial I/O error #define E_timeout 2 // initialization timeout #define E_blade 3 // blade not in position #ifdef USE_PORTB #define E_linux 4 // illegal command #endif #ifdef USE_PORTB #define BINBUFSIZE 31 // serial I/O buffers (->Linux) #define BOUTBUFSIZE 31 #endif #define CINBUFSIZE 31 // serial I/O buffers (->Linmot) #define COUTBUFSIZE 31 #define LINMOT_INPOS_A 0x0001 #define LINMOT_INPOS_B 0x0002 #define LINMOT_INPOS_C 0x0004 #define LINMOT_INPOS_D 0x0008 #define LINMOT_INITNOTDONE 0x0040 #define LINMOT_ERRORSTATE 0x0100 #define LM_VI 0.190735 // (mm/s)/bit #define LM_AI 238.419 // (mm/s^2)/bit #define LM_PI 0.01953125 // (mm)/bit #define LM_CI 23.438 // (mA)/bit #define POWERON_DELAY 100 #define POWER_ON 213 #define POWER_OFF 0 #define DSP1 0 // DSP number #define DSP2 1 #define DISABLED 0 // busy #define ENABLED 1 // ready-to-go #define FALSE 0 #define TRUE 1 /* GLOBALs ----------------------------------------------------- */ shared int init_flag; // initialization #ifdef USE_PORTB shared int exposure_flag; // exposure shared int exposure_abort; // abort exposure shared int pause_allowed; // pause allowed shared long exposure_time; // (ms) #endif char blade_to_open[2]; // next blade to open char blade_to_close[2]; // next blade to close int blade_a,blade_b; // blade status int blade_c,blade_d; int cold_start; // power-on reset float speed,acc; // speed [mm/s] and acc [mm/s^2] float lm_open=0.; // open position [mm] float lm_closed[2]={339.0,339.0}; // closed positions [mm] #ifdef USE_PORTB char cmdbuf[32],ansbuf[32]; int cmdlen; #endif long last_exptime[2]; unsigned long t1,t2,t3; /* function prototypes */ void set_led (int,int); int linmot_send (char*,char*); int linmot_speed (float); int linmot_acc (float); float get_timeout (float,float,float,float); int power_set (char,int); int is_inpos (char); char other_blade (char); void set_blade_status (char,int); cofunc int move_blade (char*,int,int); #ifdef USE_PORTB cofunc void handle_command (void); #endif /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif void main(void) { static int i,err,init_delay,init_timeout,c,timeout,flag,dsp; static long linmot_state; static char cmd[32],buf[32]; /* initializations ------------------------------------------- */ jrioInit(); // init I/O drivers WrPortI(SPCR,&SPCRShadow,0x84); // setup parallel port A as output WrPortI(PADR,&PADRShadow,0xff); // turn off all LED's speed = ((lm_closed[0] > lm_closed[1]) ? lm_closed[0] : lm_closed[1])/0.5; acc = (speed*speed)/100.; #ifdef USE_PORTB exposure_flag = 0; exposure_time = 1000; #endif last_exptime[DSP1] = last_exptime[DSP2] = 0; #ifdef USE_PORTB serBopen(9600); // Linux PC cmdlen = 0; #endif serCopen(9600); // Linmot controller init_flag = cold_start = 1; // auto-init /* main loop ------------------------------------------------- */ while (1) { // endless loop BigLoopTop(); // begin a big endless loop // blade initialization task costate { if (init_flag != 1) abort; // skip this task digOut(DSP1,DISABLED); digOut(DSP2,DISABLED); // signal 'not-ready' #ifdef USE_PORTB exposure_abort = 1; #endif if (cold_start) { // power-on reset #if (DEBUG == 0) blade_a = blade_b = blade_c = blade_d = BLADE_CLOSED; waitfor(DelaySec(5)); // wait for LinMot controller #endif cold_start = 0; } else { // init-command for (i=0; i<4; i++) { // loop over drives A..D sprintf(cmd,"!GP%c",(char)('A'+i)); (void)linmot_send(cmd,buf); // get current position sprintf(cmd,"!SP%d%c",atoi(buf+1),(char)('A'+i)); (void)linmot_send(cmd,buf); // set as demand position } } (void)linmot_acc(acc); // max. acceleration for (i=0; i<4; i++) (void)power_set((char)('A'+i),POWER_ON); err = linmot_send("!SR-1",buf); // clear RUN flag if (err) { init_flag=-1; abort; } err = linmot_send("!SI+1",buf); // set INIT flag if (err) { init_flag=-1; abort; } blade_a = blade_b = blade_c = blade_d = BLADE_MOVING; init_timeout=90; init_delay=8; do { waitfor(DelaySec(init_delay)); err = linmot_send("!EX1",buf); // get status if (err) break; linmot_state = atol(buf+1); // analyze status if (linmot_state & LINMOT_ERRORSTATE) { // error #if (DEBUG > 0) printf("linmot_state= 0x%04x\n",(unsigned)linmot_state); for (i=0; i<4; i++) { sprintf(cmd,"!EE%c",'A'+i); (void)linmot_send(cmd,buf); printf("linmot_error[%c]= 0x%04x\n",'A'+i,(unsigned)atoi(buf+1)); sprintf(cmd,"!EW%c",'A'+i); (void)linmot_send(cmd,buf); printf("linmot_warn[%c]= 0x%04x\n",'A'+i,(unsigned)atoi(buf+1)); } #endif err = E_linmot; break; } if (!(linmot_state & LINMOT_INITNOTDONE)) break; // ok init_timeout -= init_delay; if (init_delay > 1) init_delay--; #if (DEBUG > 0) printf("init_timeout=%d, init_delay=%d\n",init_timeout,init_delay); #endif } while (init_timeout > 0); if (init_timeout <= 0) err = E_timeout; if (err) { init_flag=-1; abort; } (void)linmot_send("!SI-1",buf); // clear INIT flag (void)linmot_send("!SR+1",buf); // set RUN flag (void)linmot_speed(100.); // slow for (i=DSP1; i<=DSP2; i++) { blade_to_open[i] = ((i==DSP1) ? 'A' : 'C'); waitfordone { err = move_blade(&blade_to_open[i],BLADE_OPEN,FALSE); } #if 0 if (err) break; #endif blade_to_close[i] = ((i==DSP1) ? 'B' : 'D'); waitfordone { err = move_blade(&blade_to_close[i],BLADE_CLOSED,TRUE); } #if 0 if (err) break; #endif if (i == DSP1) { blade_a = BLADE_OPEN; power_set('A',POWER_OFF); } else { blade_c = BLADE_OPEN; power_set('C',POWER_OFF); } blade_to_open[i] = ((i==DSP1) ? 'B' : 'D'); } if (err) { init_flag=-1; abort; } speed = ((lm_closed[0] > lm_closed[1]) ? lm_closed[0] : lm_closed[1])/0.5; (void)linmot_speed(speed); // regular speed init_flag = 0; #ifdef USE_PORTB exposure_flag = 0; #endif digOut(DSP1,ENABLED); digOut(DSP2,ENABLED); // signal 'ready' } // end of initialization-costate // task will check switch S2/S4 (PB3/5 input from DSP-1) -> exposure-mode=2 costate { if (init_flag) abort; // initialization running/failed #ifdef USE_PORTB if (exposure_flag) abort; // exposure command active #endif if (!BitRdPortI(PBDR,3)) dsp = DSP1; // open shutter-1 else if (!BitRdPortI(PBDR,5)) dsp = DSP2; // open shutter-2 else abort; // if no button down skip out of costatement #ifdef USE_PORTB exposure_flag = 2; #endif digOut(DSP1,DISABLED); digOut(DSP2,DISABLED); // signal 'not-ready' t1 = MS_TIMER; // record open time waitfordone { err = move_blade(&blade_to_open[dsp],BLADE_OPEN,TRUE); } #if 0 if (err) { init_flag=-1; abort; } // ignore error #endif digOut(dsp,ENABLED); // signal 'ready' /* --- */ if (dsp == DSP1) waitfor(BitRdPortI(PBDR,3)); // wait for button go up else waitfor(BitRdPortI(PBDR,5)); if (init_flag) abort; digOut(dsp,DISABLED); // signal 'not-ready' t2 = MS_TIMER; // record close time waitfordone { err = move_blade(&blade_to_close[dsp],BLADE_CLOSED,TRUE); } #if 0 if (err) { init_flag=-1; abort; } // ignore error #endif last_exptime[dsp] = (long)(t2-t1); #if (DEBUG > 0) printf("eff.exposure-time(%d)=%ld (ms)\n",dsp+1,last_exptime[dsp]); #endif digOut(DSP1,ENABLED); digOut(DSP2,ENABLED); // signal 'ready' to DSP #ifdef USE_PORTB exposure_flag = 0; #endif } // end of exposure-button-costate // task will check switch S1 (PB2 input from DSP) -> Initialization costate { if (init_flag > 0) abort; // initialization running if (BitRdPortI(PBDR,2)) abort; // if button not down skip out of costatement waitfor(DelayMs(200)); if (BitRdPortI(PBDR,2)) abort; // if button not down skip out of costatement waitfor(BitRdPortI(PBDR,2)); // wait for button to go up init_flag = 1; } // end of init-button-costate costate { // display blade-A LED status if (blade_a == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,0,BLADE_A_LED); // update LED } else set_led(blade_a,BLADE_A_LED); waitfor(DelayMs(BLINK_DELAY)); if (blade_a == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,1,BLADE_A_LED); // update LED } else set_led(blade_a,BLADE_A_LED); waitfor(DelayMs(BLINK_DELAY)); } // end of blade-a-costate costate { // display blade-B LED status if (blade_b == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,0,BLADE_B_LED); // update LED } else set_led(blade_b,BLADE_B_LED); waitfor(DelayMs(BLINK_DELAY)); if (blade_b == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,1,BLADE_B_LED); // update LED } else set_led(blade_b,BLADE_B_LED); waitfor(DelayMs(BLINK_DELAY)); } // end of blade-b-costate costate { // display blade-C LED status if (blade_c == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,0,BLADE_C_LED); // update LED } else set_led(blade_c,BLADE_C_LED); waitfor(DelayMs(BLINK_DELAY)); if (blade_c == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,1,BLADE_C_LED); // update LED } else set_led(blade_c,BLADE_C_LED); waitfor(DelayMs(BLINK_DELAY)); } // end of blade-c-costate costate { // display blade-D LED status if (blade_d == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,0,BLADE_D_LED); // update LED } else set_led(blade_d,BLADE_D_LED); waitfor(DelayMs(BLINK_DELAY)); if (blade_d == BLADE_MOVING) { BitWrPortI(PADR,&PADRShadow,1,BLADE_D_LED); // update LED } else set_led(blade_d,BLADE_D_LED); waitfor(DelayMs(BLINK_DELAY)); } // end of blade-b-costate #ifdef USE_PORTB costate { // check serial interface if ((c=serBgetc()) == -1) abort; cmdbuf[cmdlen++] = (char)c; if (cmdlen == sizeof(cmdbuf)) cmdlen=1; if (iscntrl((int)c)) { // ,,etc cmdbuf[cmdlen-1] = '\0'; waitfordone { handle_command(); } // act on command c = serBputs(ansbuf); // send answer #if (DEBUG > 0) printf("->Linux: '%s' (%d)\n",ansbuf,c); #endif (void)serBputc((char)0x0d); // send #if (DEBUG > 0) (void)serBputc((char)0x0a); // send #endif cmdlen = 0; } } // end of linux-costate #endif // USE_PORTB #ifdef USE_PORTB costate { // execute 'expose' command if (init_flag) abort; if (exposure_flag != 1) abort; BitWrPortI(PADR,&PADRShadow,LED_OFF,STATUS_LED); // signal 'busy' to DSP flag = 0; t1 = MS_TIMER; // record open-time waitfordone { err = move_blade(&blade_to_open[DSP1],BLADE_OPEN,FALSE); } #if 0 if (err) { init_flag=-1; abort; } // ignore #endif do { // wait for exposure-time if (t1+exposure_time-MS_TIMER > 2000) { // more than 2 seconds to go if (!flag) { // blade still opening if (is_inpos(blade_to_open[DSP1])) { // blade arrived power_set(blade_to_open[DSP1],0); // power down set_blade_status(blade_to_open[DSP1],BLADE_OPEN); blade_to_open[DSP1] = other_blade(blade_to_open[DSP1]); pause_allowed = flag = 1; } else { waitfor(DelayMs(500)); } } else if (pause_allowed && (exposure_flag == 4)) { t3 = MS_TIMER; // store time waitfordone { err = move_blade(&blade_to_close[DSP1],BLADE_CLOSED,TRUE); } do { waitfor(DelayMs(500)); t1 += (MS_TIMER-t3); t3 = MS_TIMER; if (exposure_abort) break; if (init_flag) abort; } while (exposure_flag == 4); waitfordone { err = move_blade(&blade_to_open[DSP1],BLADE_OPEN,TRUE); } } else { waitfor(DelaySec(1)); } if (exposure_abort || init_flag) break; // exposure aborted } else { // exposure-time almost over pause_allowed = 0; timeout = (int)(t1+exposure_time-MS_TIMER); waitfor(DelayMs(timeout)); } } while (MS_TIMER < (unsigned long)(t1+exposure_time)); if (init_flag) abort; t2 = MS_TIMER; // record close-time waitfordone { err = move_blade(&blade_to_close[DSP1],BLADE_CLOSED,TRUE); } if (!flag) { // power not shutoff yet (void)power_set(blade_to_open[DSP1],0); set_blade_status(blade_to_open[DSP1],BLADE_OPEN); blade_to_open[DSP1] = other_blade(blade_to_open[DSP1]); } #if 0 if (err) { init_flag=-1; abort; } #endif last_exptime = (long)(t2-t1); #if (DEBUG > 0) printf("eff.exposure-time=%ld\n",last_exptime); #endif BitWrPortI(PADR,&PADRShadow,LED_ON,STATUS_LED); // signal 'ready' to DSP exposure_flag = 0; } // end of exposure-costate #endif // USE_PORTB } // end of while(1) loop #ifdef USE_PORTB serBclose(); #endif serCclose(); } // end of main /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif void set_led(int status,int led) { if (status == BLADE_OPEN) { if (PADRShadow & (1 << led)) { // led is OFF BitWrPortI(PADR,&PADRShadow,0,led); // switch LED ON } } else if (status == BLADE_CLOSED) { if (!(PADRShadow & (1 << led))) { // led is ON BitWrPortI(PADR,&PADRShadow,1,led); // switch LED OFF } } } /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif cofunc int move_blade(char* blade,int status,int sync) { static int err,timeout,dsp; static long lms; static float pos; static char cmd[32],buf[32]; set_blade_status(*blade,BLADE_MOVING); // set led blinking dsp = ((*blade=='A') || (*blade=='B')) ? DSP1 : DSP2; (void)power_set(*blade,POWER_ON); // set current = 5000 mA if (status == BLADE_CLOSED) waitfor(DelayMs(POWERON_DELAY)); else waitfor(DelayMs(POWERON_DELAY+3)); if (status == BLADE_CLOSED) pos = lm_closed[dsp]; else pos = lm_open; sprintf(cmd,"!SP%d%c",(int)floor(pos/LM_PI+0.5),*blade); err = linmot_send(cmd,buf); if ((!sync) || err) return(err); // do not wait timeout = (int)(1000.*get_timeout(lm_closed[dsp],lm_open,speed,acc)); // (ms) #if (DEBUG > 0) printf("dsp=%d, timeout=%d\n",dsp,timeout); #endif waitfor(DelayMs(timeout)); do { if (is_inpos(*blade)) break; // blade in position waitfor(DelayMs(100)); timeout -= 250; } while (timeout >= 0); power_set(*blade,POWER_OFF); // power down set_blade_status(*blade,status); // update LED *blade = other_blade(*blade); // prepare for next move if (timeout < 0) return(E_blade); return(0); } /* ------------------------------------------------------------- */ #define LINMOT_TIMEOUT 1000 #if (DEBUG == 0) nodebug #endif int linmot_send(char* command,char* answer) { static int ok; #if (DEBUG > 1) printf("linmot_send(%s)\n",command); #endif ok = serCputs(command); // send string if (!ok) return(E_linmot); ok = serCputc((char)0x0d); // send if (!ok) return(E_linmot); if (answer) { while (serCpeek() == -1) { // wait for first byte if (--ok < -LINMOT_TIMEOUT) return(E_linmot); } ok = serCread((void*)answer,31,50); if (!ok) return(E_linmot); answer[ok-1] = '\0'; #if (DEBUG > 1) printf("linmot_recv(%d) = '%s'\n",ok,answer); #endif } return(0); } #undef LINMOT_TIMEOUT /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif int linmot_speed(float value) { auto int i,err,v; auto char cmd[32],buf[32]; v = (int)floor(value/LM_VI+0.5); if (v < 6) v = 6; if (v > 24576) v = 24576; for (i=0; i<4; i++) { sprintf(cmd,"!SV%d%c",v,(char)('A'+i)); err = linmot_send(cmd,buf); if (err) break; } if (!err) speed = (float)v*LM_VI; #if (DEBUG > 0) printf("set speed = %f\n",speed); #endif return(err); } /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif int linmot_acc(float value) { auto int i,err,v; auto char cmd[32],buf[32]; v = (int)floor(value/LM_AI+0.5); if (v < 1) v = 1; if (v > 1536) v = 1536; for (i=0; i<4; i++) { sprintf(cmd,"!SA%d%c",v,(char)('A'+i)); err = linmot_send(cmd,buf); if (err) break; } if (!err) acc = (float)v*LM_AI; #if (DEBUG > 0) printf("set acc = %f\n",acc); #endif return(err); } /* ------------------------------------------------------------- */ #ifdef USE_PORTB #if (DEBUG == 0) nodebug #endif cofunc void handle_command(void) { auto int err; #ifdef DEBUG printf("LinuxPC: '%s'\n",cmdbuf); #endif if (!strcmp(cmdbuf,"init")) { // 'init' command ---------- #ifdef DEBUG printf("start initialization\n"); #endif init_flag = 1; // start initialization strcpy(ansbuf,"ok"); } else if (!strcmp(cmdbuf,"status")) { // 'status' command -------- if (init_flag == 0) { if (exposure_flag == 4) sprintf(ansbuf,"closed-%c (pause)",blade_to_open); else if (exposure_flag == 3) sprintf(ansbuf,"open (%ld)",MS_TIMER-t1); else if (exposure_flag == 2) sprintf(ansbuf,"open-waiting (%ld)",MS_TIMER-t1); else if (exposure_flag == 1) sprintf(ansbuf,"open-exposing (%ld)", t1+exposure_time-MS_TIMER); else sprintf(ansbuf,"closed-%c (%ld)",blade_to_open,last_exptime); } else if (init_flag == 1) strcpy(ansbuf,"initializing"); else strcpy(ansbuf,"error"); } else if (!strcmp(cmdbuf,"open")) { // 'open' command ---------- if (init_flag || exposure_flag) { // not possible strcpy(ansbuf,"-notnow"); return; } exposure_flag = 3; BitWrPortI(PADR,&PADRShadow,LED_OFF,STATUS_LED); // signal 'busy' to DSP t1 = MS_TIMER; // record open time waitfordone { err = move_blade(&blade_to_open,BLADE_OPEN,TRUE); } if (err) { init_flag=-1; strcpy(ansbuf,"-error"); return; } strcpy(ansbuf,"ok"); } else if (!strcmp(cmdbuf,"close")) { // 'close' command --------- if (exposure_flag != 3) { strcpy(ansbuf,"-notnow"); return; } t2 = MS_TIMER; // record close time waitfordone { err = move_blade(&blade_to_close,BLADE_CLOSED,TRUE); } if (err) { init_flag=-1; strcpy(ansbuf,"-error"); return; } last_exptime = (long)(t2-t1); #ifdef DEBUG printf("eff.exposure-time=%ld (ms)\n",last_exptime); #endif sprintf(ansbuf,"ok (%ld)",last_exptime); BitWrPortI(PADR,&PADRShadow,LED_ON,STATUS_LED); // signal 'ready' to DSP exposure_flag = 0; } else if (!strcmp(cmdbuf,"expose")) { // 'expose' command -------- if (exposure_flag || init_flag) { // not possible strcpy(ansbuf,"-notnow"); return; } #ifdef DEBUG printf("start exposure (%ld ms)\n",exposure_time); #endif exposure_abort = pause_allowed = 0; exposure_flag = 1; // start exposure strcpy(ansbuf,"ok"); } else if (!strcmp(cmdbuf,"pause")) { // 'pause' command --------- if ((exposure_flag != 1) || (!pause_allowed)) { strcpy(ansbuf,"-notnow"); return; } exposure_flag = 4; strcpy(ansbuf,"ok"); } else if (!strcmp(cmdbuf,"continue")) { // 'continue' command ------ if (exposure_flag != 4) { strcpy(ansbuf,"-notnow"); return; } exposure_flag = 1; strcpy(ansbuf,"ok"); } else if (!strcmp(cmdbuf,"abort")) { // 'abort' command --------- #ifdef DEBUG printf("abort exposure\n"); #endif exposure_abort = 1; // abort exposure strcpy(ansbuf,"ok"); } else if (!strncmp(cmdbuf,"exptime=",8)) { // 'exptime' command ------- exposure_time = atol(cmdbuf+8); // milliseconds #ifdef DEBUG printf("set exptime=%ld\n",exposure_time); #endif sprintf(ansbuf,"exptime=%ld",exposure_time); } else if (!strncmp(cmdbuf,"speed=",6)) { // 'speed' command --------- speed = (float)atof(cmdbuf+6); #ifdef DEBUG printf("set speed=%f\n",speed); #endif if (linmot_speed(speed) != 0) sprintf(ansbuf,"-failed"); else sprintf(ansbuf,"speed=%f",speed); } else if (!strncmp(cmdbuf,"acc=",4)) { // 'acc' command ----------- acc = (float)atof(cmdbuf+4); #ifdef DEBUG printf("set acceleration=%f\n",acc); #endif if (linmot_acc(acc) != 0) sprintf(ansbuf,"-failed"); else sprintf(ansbuf,"acc=%f",acc); } else { // unknown command --------- strcpy(ansbuf,"-unknown"); } } #endif // USE_PORTB /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif float get_timeout(float p1,float p2,float v,float a) { auto float s; s = (float)fabs(p1-p2); return((s-v*v/a)/v + 2.*v/a); } /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif int power_set(char blade,int power) { auto int err; auto char cmd[32],buf[32]; sprintf(cmd,"!SC%d%c",power,blade); err = linmot_send(cmd,buf); return(err); } /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif int is_inpos(char blade) { auto long lms; auto char buf[32]; (void)linmot_send("!EX1",buf); // get status lms = atol(buf+1); // analyze status #if (DEBUG > 0) printf("is_inpos(%c): %ld (%s)\n",blade,lms,buf); #endif if (blade == 'A') { if (lms & LINMOT_INPOS_A) return(1); } else if (blade == 'B') { if (lms & LINMOT_INPOS_B) return(1); } else if (blade == 'C') { if (lms & LINMOT_INPOS_C) return(1); } else if (blade == 'D') { if (lms & LINMOT_INPOS_D) return(1); } return(0); } /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif char other_blade(char blade) { if (blade == 'A') return('B'); if (blade == 'B') return('A'); if (blade == 'C') return('D'); return('C'); } /* ------------------------------------------------------------- */ #if (DEBUG == 0) nodebug #endif void set_blade_status(char blade,int status) { if (blade == 'A') blade_a = status; else if (blade == 'B') blade_b = status; else if (blade == 'C') blade_c = status; else blade_d = status; } /* ------------------------------------------------------------- */ /* ------------------------------------------------------------- */ /* ------------------------------------------------------------- */