void srun() { /* simulation RUN routine */
int lj, dcounter ; double tmm ; /* dcounter for MM displays <***/
runf=1 ; /* to keep display label in command mode */
if (relval[mmm+15]<1) relval[mmm+15]=1 ; /* MM must be >=1 */
if (relval[mmm+1]<=1) relval[mmm+1]=251 ; /* AN=NN output pts. */
if (relval[mmm+4]<=0) { /* TMAX */
if (size) derror(87) ;
else relval[mmm+4]=relval[mmm+1]-1 ; /* irule 0 */
}
zCOMINT=relval[mmm+4]/(relval[mmm+1]-1) ; /* COMINT =TMAX/(NN-1) <*****/
if (INITE1>0) ICOM=1 ; /* initially, ICOM>0 only if INITE1>0 */
else ICOM= -1 ; /* for "continued-run" store, etc. */
if (size) { /* "true" integration */
if (relval[mmm+5]==1.1E-275) relval[mmm+5]=0 ; /**!!**//* t */
if (relval[mmm+3]<=0) relval[mmm+3]=zCOMINT/2 ; /*default DT <****/
if (relval[mmm+3]>zCOMINT) derror(110) ;
/* <**** DT is too large *******/
/*** since
DT <=TMAX/(NN-1)=COMINT ,
NN <= (TMAX/DT) + 1 ***/
relval[mmm-3]=relval[mmm+3] ; /* save DT for reset */
for (lj=1 ; lj<= nn1 ; lj++)
storesv[lj]=dpsv[lj]=relval[lj] ;
/* save state vars. for reset, make duplicates for integration */
hdt=relval[mmm+3]/2 ; /* DT/2 */
if ((size>3)&&(size!=5)) { /* prepare for variable-step integration */
if (relval[mmm+9]<=0) relval[mmm+9]=0.001 ; /* ERMAX */
if (size<9) {
if (relval[mmm+7]<=0 || relval[mmm+7]>zCOMINT)
relval[mmm+7]=zCOMINT ; /* DTMAX <****/
if (relval[mmm+8]<=0 || relval[mmm+8]>relval[mmm+7])
relval[mmm+8]=relval[mmm+3]/16 ; /* DTMIN */
if (relval[mmm+10]<=0 || relval[mmm+10]>relval[mmm+9])
relval[mmm+10]=0.01*relval[mmm+9] ; /* ERMIN */
if (relval[mmm+2]>nn1) derror(93) ; /*CHECKN too large*/
} /* 3 < irule < 9 and not 5 */
else if (jcstart==0 && (size==10 || size==14))
derror(229) ; /* missing JACOBIAN */
} /* irule > 3 and not 5 */
} /* irule != 0 */
else if (relval[mmm+5]==1.1E-275) relval[mmm+5]=1 ;
/***!!!***/ /* irule 0 */
t0=relval[mmm-2]=relval[mmm+5] ; /* save t0=t for reset */
tmm=t0+relval[mmm+4] ; /* tmm = t0+TMAX */
ztnext=t0+zCOMINT ; /* tnext = t0 + COMINT <*****/
dflag=dispp && dbegin ; sflag=disk && sbegin ;
/* ------------------------------------------------------------------ */
if (dflag) { /* start display, if any */
lj=dbegin ; /* start of display label, if any */
if (dmode<0) { /* "type"; do header label */
if (!typeflg) { /* ## suppresses header */
WRITELN ;
fprintf(outptr,"\n\nt, ") ;
if (ferror(outptr)) derror(106) ;
while (warea[lj]!=lf && warea[lj]!='|')
putc(warea[lj++],outptr) ;
}
fprintf(outptr,"\n\n") ; /* could be printer or file */
} /* if "type" */
else if (dispp>0) drstart() ; /* dmode>=0, start CRT graph */
} /* unless we have display 2 */
/* ------------------------------------------------------------------ */
if (rbegin) {
if (fd[maxchan-1]==NULL) { chanlno=maxchan-1 ; derror(64) ; }
} /* not open */
if (sflag) { /* connect .TIM file(s) */
strcpy(fname,pname) ;
lj=0 ;
if (fname[lj]=='.') lj++ ; if (fname[lj]=='.') lj++ ;
do lj++ ; while (fname[lj]!='.') ; /* get extension */
fname[lj+1]='T' ; fname[lj+2]='I' ; fname[lj+3]='M' ;
fname[lj+4]=0 ;
if (fd[maxchan]!=NULL)
{ chanlno=maxchan ; derror(63) ; } /* busy */
if ((fd[maxchan]=fopen(fname,"w"))==NULL) derror(11) ;
}
/* ------------------------------------------------------------------ */
/* communication loop */
/* NOTE: on "continued" runs, displays and stash files
DUPLICATE the last point of a run as the first point
of the "continued" run. "Continued" runs reproduced
from .RCV files necessarily have THE SAME t0, TMAX,
and NN as the original runs, so that the duplicated
points are reproduced correctly.
Time histories reproduced from STORE or DELAY arrays,
however, do NOT contain duplicated points and can be
used anywhere in a subsequent run. */
/* ------------------------------------------------------------------ */
IDERIV=1 ; /* IDERIV>0, state variables valid */
dcounter=0 ; /* initialize dcounter <*****/
/* initially, ICOM>0 only if INITE1>0; otherwise, for continued-run
STORE etc., ICOM<0, so the duplicate endpoints are not stored.
Now make the initial derivative call; this scheme ensures that
defined variables can be computed for output */
/* ------------------------------------ */
if (size>8) { /* use GEAR module, initialize if INITE1>0 */
if (nn1>EQUMAX) derror(100) ; /* too many state variables */
TOUT=ztnext ; /* tnext for GEARER.C */
if (INITE1>0) aINDEX=1 ; /* ** was in gearstart ** */
do { /* because of duplicate derve1, ICOM, INITE1 are OFF! */
if (rbegin) { recovr() ; if (errnum) goto L5 ; }
drun00(2) ; if (errnum) goto L5 ;
if (dcounter == 0) {
/* display only once in MM communication points <***/
if (dflag) { if (dmode>=0) disply() ; else typex() ; }
if (sflag) dstor() ;
}
if (++dcounter==relval[mmm+15]) dcounter=0 ; /* <*****/
ICOM=INITE1= -1 ; /* ICOM<0, INITE1<0 for store, etc. */
DRIVE(); if (errnum) goto L5 ;
/* driver routine in GEAR also increments TOUT by COMINT */
ICOM=1 ; /* ICOM>0, mark next output point */
if (spare1) { /* look at ctl-c flag */
spare1=0 ;
if (lj=getchar()!=lf) {errnum=306 ; goto L55 ; }
}
} while (relval[mmm+5]-tmm<0) ; /* t - (t0 + TMAX) < 0 */
} /* if Gear module */
else do { /* NOT Gear module; ICOM, INITE1 are OFF! */
if (rbegin) { recovr() ; if (errnum) goto L5 ; }
drun00(2) ; if (errnum) goto L5 ;
/* initial values of defined variables */
if (dcounter == 0) {
/* display only once in MM communication points <***/
if (dflag) { if (dmode>=0) disply() ; else typex() ; }
if (sflag) dstor() ;
}
if (++dcounter==relval[mmm+15]) dcounter=0 ; /* <*****/
ICOM=INITE1= -1 ; /* ICOM<0, INITE1<0
for store, etc. */
switch(size) {
case 0 : relval[mmm+5]=ztnext ; break ;
/* dummy integrator: t=tnext */
case 1 : ru1() ; break ; /* integration loops */
case 2 : ru2() ; break ;
case 3 : ru3() ; break ;
case 4 : ru4() ; break ;
case 5 : ru5() ; break ;
case 6 : ru6() ; break ;
case 7 : ru7() ; break ;
case 8 : ru8() ; break ;
} if (errnum) goto L5 ;
ztnext+=zCOMINT ; /* tnext=tnext+COMINT <*****/
ICOM=1 ; /* ICOM>0, mark next output point */
if (spare1) { /* look at ctl-c flag */
spare1=0 ;
if (lj=getchar()!=lf) {errnum=306 ; goto L55 ; }
}
} while (relval[mmm+5]-tmm<0) ; /* t - (t0 + TMAX) < 0 */
/* ------------------------------------------------------------------- */
/* last point */
if (rbegin) { recovr() ; if (errnum) goto L5 ; }
drun00(2) ; if (!errnum) goto L6 ;
/* clean up; some of those errors are not errors! */
L5 : if (errnum==303) { errnum=0 ; goto L6 ; } /* derve1 terminated */
if (errnum==305) {
printf("\n Warning: END OF FILE or error on recover - ") ;
errnum=0 ; /* EOF or error on recover */
}
if (errnum>=304) errnum=0 ; /* eof */
goto L55 ; /* close stash and/or recovr file(s), if any */
L6 : if (dflag) { if (dmode>=0) disply() ; else typex() ; } /* last */
if (sflag) dstor() ; /* point */
L55 : if (sflag) { /* close stash file, if any */
if (fclose(fd[maxchan])==EOF) derror(95) ;
fd[maxchan]=NULL ;
}
if (rbegin) { /* close recover file, if any */
if (fclose(fd[maxchan-1])==EOF) derror(95) ;
fd[maxchan-1]=NULL ;
}
if (!dflag || dmode<0) WRITELN ; else dflush() ;
if (errnum) derror(errnum) ;
} /* srun */
int main()
{
printf("checklist");
printf("hit black button when ready");
while(black_button());
printf("MAKE SURE CREATE IS ON!!!!!!!!");
while(!black_button());
printf("MAKE SURE ARM IS IN DOWN POSITION!!!!!!!!");
while(!black_button());
printf("MAKE SURE ARM AND CLAW WORK");
while(!black_button());
printf("CHECK POSITION OF CLAW AND ARM");
while(!black_button());
printf("MAKE SURE LIGHT SENSORT IS CALIBRATED");
while(!black_button());
// this starts the robot SECTion 1
wait_for_light(0);
shut_down_in(120.0);
//this shuts the robot down after 120.0 seconds
while(create_connect());
//connects create to cbc
set_servo_position(3, 0);
set_servo_position(0, 1800);
enable_servos();
msleep(500);
create_full();
//section 2
create_drive_direct(SUPER_FAST, SUPER_FAST);
msleep(1646);
//leave beach
create_stop();
msleep(300);
create_drive_direct(SUPER_FAST, -SUPER_FAST);
msleep(435);
//turn towards center
create_stop();
msleep(300);
create_drive_direct(SUPER_FAST, SUPER_FAST);
msleep(1600);
//move towards botguy
create_stop();
msleep(500);
//section 3
set_servo_position(0, 1424);
msleep(1800);
set_servo_position(0, 1100);
msleep(1800);
set_servo_position(0, 924);
msleep(1800);
set_servo_position(0, 724);
msleep(1800);
set_servo_position(3, 1024);
msleep(1800);
set_servo_position(0, 924);
msleep(1800);
set_servo_position(0, 1100);
msleep(1800);
set_servo_position(0, 1200);
msleep(2000);
//retrieve botguy
//section 4
create_drive_direct(-FAST, FAST);
msleep(445);
create_stop();
create_drive_direct(-FAST, -FAST);
msleep(2000);
create_stop();
create_drive_direct(FAST, -FAST);
msleep(445);
create_stop();
create_drive_direct(-FAST, -FAST);
msleep(900);
create_stop();
msleep(1600);
create_stop();
set_servo_position(3, 0);
set_servo_position(0, 1800);
msleep(1200);
set_servo_position(0, 1200);
msleep(1000);
set_servo_position(0, 1800);
msleep(1000);
create_drive_direct(1000, 1000);
msleep(800);
create_drive_direct(-1000, -1000);
msleep(800);
DRIVE(SUPER_FAST);
MSLEEP(800);
TURN(-SUPER_FAST, SUPER_FAST);
MSLEEP(460);
DRIVE(SUPER_FAST);
MSLEEP(1380);
TURN(SUPER_FAST, -SUPER_FAST);
MSLEEP(425);
DRIVE(SUPER_FAST);
SLEEP(1.5);
while(get_create_lbump(0.01) == 0)
{
DRIVE(SUPER_FAST);
}
TURN(-SUPER_FAST, SUPER_FAST);
MSLEEP(435);
TURN(SUPER_FAST, 975);
MSLEEP(500);
while(get_create_rbump(0.01) == 0)
{
TURN(SUPER_FAST, 990);
}
create_stop();
create_disconnect();
}
#include <core/simulator_layout.hpp> #include <boost/preprocessor/repeat.hpp> #define MultiNicX BOOST_PP_CAT(MultiNic, FLEXUS_MULTI_NIC_NUMPORTS) #define FLEXUS_BEGIN_COMPONENT MultiNicX #include FLEXUS_BEGIN_COMPONENT_DECLARATION() #include <components/Common/Transports/MemoryTransport.hpp> #define NodeOutputPort(z,N,_) DYNAMIC_PORT_ARRAY( PushOutput, MemoryTransport, BOOST_PP_CAT(ToNode,N) ) #define NodeInputPort(z,N,_) DYNAMIC_PORT_ARRAY( PushInput, MemoryTransport, BOOST_PP_CAT(FromNode,N) ) COMPONENT_PARAMETERS( FLEXUS_PARAMETER( VChannels, int, "Virtual channels", "vc", 3 ) FLEXUS_PARAMETER( RecvCapacity, uint32_t, "Recv Queue Capacity", "recv-capacity", 1) FLEXUS_PARAMETER( SendCapacity, uint32_t, "Send Queue Capacity", "send-capacity", 1) ); COMPONENT_INTERFACE( DYNAMIC_PORT_ARRAY( PushOutput, MemoryTransport, ToNetwork ) DYNAMIC_PORT_ARRAY( PushInput, MemoryTransport, FromNetwork ) BOOST_PP_REPEAT(FLEXUS_MULTI_NIC_NUMPORTS, NodeOutputPort, _ ) BOOST_PP_REPEAT(FLEXUS_MULTI_NIC_NUMPORTS, NodeInputPort, _ ) DRIVE( MultiNicDrive ) ); #include FLEXUS_END_COMPONENT_DECLARATION() #define FLEXUS_END_COMPONENT MultiNicX
