void drive_dump()//dumps the basket into the caldera by driving
{
move_block_arm(BLA_MID);//get the arm out of the way
servo_set(BASKET_ARM, BA_UP, 1);//raise the arm
move_block_arm(BLA_UP);//driving position
time_drive(-50, -50, 1000);//drive back into the caldera
msleep(1000);//let stuff fall out
time_drive(50, 50, 500);//one more attempted dump
time_drive(-50, -50, 1000);//
time_drive(50, 50, 500);//get off the caldera a bit (hopefully reducing catching)
msleep(1000);//more time to fall out
move_block_arm(BLA_MID);//get the arm out of the way
servo_set(BASKET_ARM, BA_DOWN, 1);//bring the basket down
move_block_arm(BLA_UP);//driving position
}
void grab_blocks()//goes through the routine to pick up the blocks
{//starts once near to the pipe, with plowed tribbles; ends back from the pipe, with the tribbles in front, tribble arm down
servo_set(BLOCK_CLAW, BC_CLOSE, .1);//drop the claw
move_block_arm(BLA_DOWN);//
servo_set(BLOCK_CLAW, BC_OPEN, .1);//
time_drive(50,50,1700);//drive up to blocks
servo_set(BLOCK_CLAW, BC_CLOSE,.75);//and pick them up
msleep(250);
move_block_arm(BLA_LIFT);//get off the ground
thread hold=thread_create(hold_ba_lift);//this will hold the block arm at the lift location
thread_start(hold);//
back(6, 60);//back away from the pipe
thread_destroy(hold);//don't want to hold it up any more
servo_set(TRIBBLE_ARM, TA_JUMP, .5);//bring the ta up to keep the blocks in the basket
servo_set(TRIBBLE_CLAW, TC_CLOSE, .5);//
servo_set(TRIBBLE_ARM, TA_UP, .5);//
move_block_arm(BLA_UP);//drop in the basket
msleep(500);
motor(BLOCK_ARM, -50);//stall it into the basket
servo_set(BLOCK_CLAW, BC_OPEN,.75);//
msleep(300);
servo_set(BLOCK_CLAW, BC_START, .4);//shake the claw a bit to try to push the blocks in if they didn't go in the first time
msleep(100);
servo_set(BLOCK_CLAW, BC_OPEN, .75);//
msleep(300);
servo_set(BLOCK_CLAW, BC_START, .4);//
off(BLOCK_ARM);//stop stalling the motor
servo_set(TRIBBLE_CLAW, TC_PART_OPEN, .4);//put the claw back down
servo_set(TRIBBLE_ARM, TA_START, .4);//
msleep(200);
servo_set(TRIBBLE_CLAW, TC_OPEN, .6);//
servo_set(TRIBBLE_ARM, TA_DOWN, .1);//
}
static float retime_drive(cdrom_drive *d, FILE *progress, FILE *log, int lba, int readahead, float oldmean){
int sectors = 2000;
int total;
float newmean;
if(sectors*oldmean > 5000) sectors=5000/oldmean;
readahead*=10;
readahead/=9;
if(readahead>sectors)sectors=readahead;
printC("\bo");
logC("\n\tRetiming drive... ");
total = time_drive(d,NULL,log,lba,sectors,1);
newmean = total/(float)sectors;
logC("\n\tOld mean=%.2fms/sec, New mean=%.2fms/sec\n",oldmean,newmean);
printC("\b");
if(newmean>oldmean)return newmean;
return oldmean;
}
int main()
{
set_a_button_text("pipe jumping");
set_b_button_text("center drive");
set_c_button_text("testing");
int strategy;
WAIT(a_button()||b_button()||c_button());
if(a_button())
{
strategy=PIPE_JUMP;
}
else if(b_button())
{
strategy=CENTER_DRIVE;
}
else//c
{
WAIT(side_button());
msleep(1000);
set_servo_position(TRIBBLE_ARM, TA_DOWN);
set_servo_position(TRIBBLE_CLAW, TC_OPEN);
set_servo_position(BLOCK_CLAW, BC_START);
set_servo_position(BASKET_ARM, BA_DOWN);
enable_servos();
msleep(2000);
forward(20, 60);
return 0;
//set_up_drive();//gets the servos and stuff in driving position
//test_driving();
reset_buttons();
set_up_drive();
printf("press black button to begin\n");
WAIT(side_button());
printf("starting in 2 seconds...\n");
msleep(2000);
back_line_follow_time(60, 5000);
return 9001;
}
WAIT(!a_button()&&!b_button());
set_a_button_text("light start");//only gets here if running one of the real code options
set_b_button_text("no ls");
set_c_button_text("-");
boolean l_s=false;//whether or not is doing light start
WAIT(a_button()||b_button());
if(a_button())//if b, does nothing-->l_s already false
l_s=true;
reset_buttons();
if(strategy==PIPE_JUMP)
set_up_jump();
else//drive
set_up_drive();
if(l_s)
{
light_start(LIGHT_SENSOR);
}
else//no light start
{
printf("press black button when ready\n");
WAIT(side_button());
printf("starting in 2 seconds...\n");
msleep(2000);
}
shutdownin(239.5);
start();//timing
if(strategy==PIPE_JUMP)
{
ready_to_jump();
time_drive(-90, -90, 1300);//jump!
servo_set(TRIBBLE_ARM, TA_UP,.3);//move the claw up so it can square up
time_drive(-50, -50, 1500);//get towards the pipe
physical_squareup(false);//square up on the back
time_drive(50, 50, 2500);//go back towards the other wall
physical_squareup(true);//square up on the front
time_drive(-50, -50, 1000);//get back to the center
right(87, 0, 50);//turn towards the edge
time_drive(-60, -60, 1500);//move towards edge
physical_squareup(false);//square up on the outside of the field
tribble_claw_drop();//drop the claw-->plow position
time_drive(68, 60, 1100);//arc into the wall to wall follow to the blocks
//forward(5.25, 60);//move to block position
grab_blocks();//grab the blocks...
forward(20, 60);//plow the tribbles!
servo_set(TRIBBLE_CLAW, TC_CLOSE, .7);//close claw to catch tribbles
servo_set(TRIBBLE_ARM, TA_UP, 1);//put the arm up to get across the center
forward(20, 60);//get across the center
servo_set(TRIBBLE_ARM, TA_START, .5);//push into the ground so the claw doesn't jump
servo_set(TRIBBLE_CLAW, TC_OPEN, .8);//back into plow position
servo_set(TRIBBLE_ARM, TA_DOWN, .1);//back to drive position
msleep(250);
forward(7, 60);//get to dumping location
nowstr("first dump started at");
drive_dump();//dump...
forward(7, 60);//replow the tribbles (maybe get a few extra)
back(2, 60);//get to optimal grab location
tribble_claw_dump();//put the tribbles in the basket
back(5, 60);//back to dump location
tribble_claw_drop();//put the claw back down
drive_dump();//dump again
nowstr("first dump finished at");
//forward(15, 60);//plow the second set of tribbles, get to block location
time_drive(66, 60, 3000);//arc towards the right wall to make sure it follows the right wall
grab_blocks();//grab the blocks...
forward(8, 60);//plow the remaining tribbles
servo_set(TRIBBLE_CLAW, TC_CLOSE, .4);//grab the tribbles
servo_set(TRIBBLE_ARM, TA_DUMP, 1);//raise the arm (to allow it to square up)
time_drive(60, 60, 1500);//get towards the wall
physical_squareup(true);//and square up on it
msleep(200);
back_line_follow(35.8, 60);//back up to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//put the arm down to get it out of the way
nowstr("second dump started at");
drive_dump();//dump
tribble_claw_dump();//put the tribbles in the basket
tribble_claw_drop();//
drive_dump();//dump again
nowstr("second dump finished at");
servo_set(TRIBBLE_CLAW, TC_PART_OPEN, .3);//get the claw up and out of the way
servo_set(TRIBBLE_ARM, TA_UP, .3);
back(6, 60);//back up to get the caught tribbles
servo_set(TRIBBLE_ARM, TA_START, .3);//put the claw back down
servo_set(TRIBBLE_CLAW, TC_OPEN, .5);//
servo_set(TRIBBLE_ARM, TA_DOWN, .1);//
forward(38, 60);//push the tribbles (get any that went way down the field)
back(2, 60);//tribbles near edge of claw
tribble_claw_dump();
servo_set(TRIBBLE_CLAW, TC_CLOSE, .3);//close the claw, just for good measure
back_line_follow(30, 60);//get back to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//get the arm out of the way
nowstr("third dump started at");
drive_dump();
nowstr("third dump finished at");
back_line_follow(17, 60);//get back across the center
right(180, 0, 50);//turn around
tribble_claw_drop();//put the claw down...
forward(25, 60);//and plow!
back(2, 60);//optimal grab location
tribble_claw_dump();//well duh...
servo_set(TRIBBLE_CLAW, TC_CLOSE, .3);//close the claw for good measure
time_drive(60, 60, 1500);//move towards the wall
physical_squareup(true);//and square up on it
back_line_follow(35.8, 60);//back up to the dumping location
servo_set(TRIBBLE_ARM, TA_DOWN, .5);//get the claw out of the way
nowstr("fourth dump started at");
drive_dump();//dump stuff
move_block_arm(BLA_MID);//get the block arm back out of the way
servo_set(BASKET_ARM, BA_UP, 1);//put the basket back up
time_drive(-50, -50, 1500);//one last chance to dump (stay there just cause...you never know)
}
else//drive strategy
{
forward(38, 90);//get towards the center
right(90, 0, 70);//turn towards the middle of the gap
forward(17, 90);//get through the gap and to the black tape
left(85, 0, 70);//turn parallel to the black tape
servo_set(TRIBBLE_ARM, TA_JUMP, .4);//get the claw into plow position
servo_set(TRIBBLE_CLAW, TC_PART_OPEN, .4);//
servo_set(TRIBBLE_ARM, TA_DOWN, .4);//
servo_set(TRIBBLE_CLAW, TC_OPEN, .4);//
}
nowstr("finished at");
return 42;
}
int analyze_cache(cdrom_drive *d, FILE *progress, FILE *log, int speed){
/* Some assumptions about timing:
We can't perform cache determination timing based on looking at
average transfer times; on slow setups, the speed of a drive
reading sectors via PIO will not be reliably distinguishable from
the same drive returning data from the cache via pio. We need
something even more noticable and reliable: the seek time. It is
unlikely we'd ever see a seek latency of under ~10ms given the
synchronization requirements of a CD and the maximum possible
rotational velocity. A cache hit would always be faster, even
with PIO.
Further complicating things, we have to watch the data collection
carefully as we're not always going to be on an unloaded system,
and we even have to guard against other apps accessing the drive
(something that should never happen on purpose, but could happen
by accident). As we know in our testing when seeks should never
occur, a sudden seek-sized latency popping up in the middle of a
collection is an indication that collection is possibly invalid.
A second cause of 'spurious latency' would be media damage; if
we're consistently hitting latency on the same sector during
initial collection, may need to move past it. */
int i,j,ret=0,x;
int firstsector=-1;
int lastsector=-1;
int firsttest=-1;
int lasttest=-1;
int offset;
int warn=0;
int current=1000;
int hi=15000;
int cachesize=0;
int readahead=0;
int rollbehind=0;
int cachegran=0;
float mspersector=0;
if(speed<=0)speed=-1;
reportC("\n=================== Checking drive cache/timing behavior ===================\n");
d->error_retry=0;
/* verify the lib and cache analysis match */
if(strcmp(VERSIONNUM,paranoia_version())){
reportC("\nWARNING: cdparanoia application (and thus the cache tests) does not match the"
"\ninstalled (or in use) libcdda_paranoia.so library. The final verdict of this"
"\ntesting may or may not be accurate for the actual version of the paranoia"
"library. Continuing anyway...\n\n");
}
/* find the longest stretch of available audio data */
for(i=0;i<d->tracks;i++){
if(cdda_track_audiop(d,i+1)==1){
if(firsttest == -1)
firsttest=cdda_track_firstsector(d,i+1);
lasttest=cdda_track_lastsector(d,i+1);
if(lasttest-firsttest > lastsector-firstsector){
firstsector=firsttest;
lastsector=lasttest;
}
}else{
firsttest=-1;
lasttest=-1;
}
}
if(firstsector==-1){
reportC("\n\tNo audio on disc; Cannot determine timing behavior...");
return -1;
}
/* Dump some initial timing data to give a little context for human
eyes. Take readings ten minutes apart (45000 sectors) and at end of disk. */
{
int best=0;
int bestcount=0;
int iterating=0;
offset = lastsector-firstsector-current-1;
reportC("\nSeek/read timing:\n");
while(offset>=firstsector){
int m = offset/4500;
int s = (offset-m*4500)/75;
int f = offset-m*4500-s*75;
int sofar;
if(iterating){
reportC("\n");
}else{
printC("\r");
logC("\n");
}
reportC("\t[%02d:%02d.%02d]: ",m,s,f);
/* initial seek to put at at a small offset past end of upcoming reads */
if((ret=cdda_read(d,NULL,offset+current+1,1))<0){
/* media error! grr! retry elsewhere */
if(ret==-404)return -1;
reportC("\n\tWARNING: media error during read; continuing at next offset...");
offset = (offset-firstsector+44999)/45000*45000+firstsector;
offset-=45000;
continue;
}
sofar=time_drive(d,progress, log, offset, current, 1);
if(offset==firstsector)mspersector = sofar/(float)current;
if(sofar==-404)
return -1;
else if(sofar<0){
reportC("\n\tWARNING: media error during read; continuing at next offset...");
offset = (offset-firstsector+44999)/45000*45000+firstsector;
offset-=45000;
continue;
}else{
if(!iterating){
if(best==0 || sofar*1.01<best){
best= sofar;
bestcount=0;
}else{
bestcount+=sofar;
if(bestcount>sofar && bestcount>4000)
iterating=1;
}
}
}
if(iterating){
offset = (offset-firstsector+44999)/45000*45000+firstsector;
offset-=45000;
printC(" ");
}else{
offset--;
printC(" spinning up... ");
}
}
}
reportC("\n\nAnalyzing cache behavior...\n");
/* search on cache size; cache hits are fast, seeks are not, so a
linear search through cache hits up to a miss are faster than a
bisection */
{
int under=1;
int onex=0;
current=0;
offset = firstsector+10;
while(current <= hi && under){
int i,j;
under=0;
current++;
if(onex){
if(speed==-1){
logC("\tAttempting to reset read speed to full... ");
}else{
logC("\tAttempting to reset read speed to %dx... ",speed);
}
if(cdda_speed_set(d,speed)){
logC("failed.\n");
}else{
logC("drive said OK\n");
}
onex=0;
}
printC("\r");
reportC("\tFast search for approximate cache size... %d sectors ",current-1);
logC("\n");
for(i=0;i<25 && !under;i++){
for(j=0;;j++){
int ret1=0,ret2=0;
if(i>=15){
int sofar=0;
if(i==15){
printC("\r");
reportC("\tSlow verify for approximate cache size... %d sectors",current-1);
logC("\n");
logC("\tAttempting to reduce read speed to 1x... ");
if(cdda_speed_set(d,1)){
logC("failed.\n");
}else{
logC("drive said OK\n");
}
onex=1;
}
printC(".");
logC("\t\t>>> ");
while(sofar<current){
ret1 = cdda_read_timed(d,NULL,offset+sofar,current-sofar,&x);
logC("slow_read=%d:%d:%d ",offset+sofar,ret1,x);
if(ret1<=0)break;
sofar+=ret1;
}
}else{
ret1 = cdda_read_timed(d,NULL,offset+current-1,1,&x);
logC("\t\t>>> fast_read=%d:%d:%d ",offset+current-1,ret1,x);
/* Some drives are 'easily distracted' when readahead is
running ahead of the read cursor, causing accesses of
the earliest sectors in the cache to show bursty
latency. If there's no seek here after a borderline
long read of the earliest sector in the cache, then the
cache must not have been dumped yet. */
if(ret==1 && i && x<MIN_SEEK_MS){
under=1;
logC("\n");
break;
}
}
ret2 = cdda_read_timed(d,NULL,offset,1,&x);
logC("seek_read=%d:%d:%d\n",offset,ret2,x);
if(ret1<=0 || ret2<=0){
offset+=current+100;
if(j==10 || offset+current>lastsector){
reportC("\n\tToo many read errors while performing drive cache checks;"
"\n\t aborting test.\n\n");
return(-1);
}
reportC("\n\tRead error while performing drive cache checks;"
"\n\t choosing new offset and trying again.\n");
}else{
if(x==-1){
reportC("\n\tTiming error while performing drive cache checks; aborting test.\n");
return(-1);
}else{
if(x<MIN_SEEK_MS){
under=1;
}
break;
}
}
}
}
}
}
cachesize=current-1;
printC("\r");
if(cachesize==hi){
reportC("\tWARNING: Cannot determine drive cache size or behavior! \n");
return 1;
}else if(cachesize){
reportC("\tApproximate random access cache size: %d sector(s) \n",cachesize);
}else{
reportC("\tDrive does not cache nonlinear access \n");
return 0;
}
/* does the readahead cache exceed the maximum Paranoia currently expects? */
{
cdrom_paranoia *p=paranoia_init(d);
if(cachesize > paranoia_cachemodel_size(p,-1)){
reportC("\nWARNING: This drive appears to be caching more sectors of\n"
" readahead than Paranoia can currently handle!\n");
warn=1;
}
paranoia_free(p);
}
if(speed==-1){
logC("\tAttempting to reset read speed to full... ");
}else{
logC("\tAttempting to reset read speed to %d... ",speed);
}
if(cdda_speed_set(d,speed)){
logC("failed.\n");
}else{
logC("drive said OK\n");
}
/* This is similar to the Fast search above, but just in case the
cache is being tracked as multiple areas that are treated
differently if non-contiguous.... */
{
int seekoff = cachesize*3;
int under=0;
reportC("\tVerifying that cache is contiguous...");
for(i=0;i<20 && !under;i++){
printC(".");
for(j=0;;j++){
int ret1,ret2;
if(offset+seekoff>lastsector){
reportC("\n\tOut of readable space on CDROM while performing drive checks;"
"\n\t aborting test.\n\n");
return(-1);
}
ret1 = cdda_read_timed(d,NULL,offset+seekoff,1,&x);
logC("\t\t>>> %d:%d:%d ",offset+seekoff,ret1,x);
ret2 = cdda_read_timed(d,NULL,offset,1,&x);
logC("seek_read:%d:%d:%d\n",offset,ret2,x);
if(ret1<=0 || ret2<=0){
offset+=cachesize+100;
if(j==10){
reportC("\n\tToo many read errors while performing drive cache checks;"
"\n\t aborting test.\n\n");
return(-1);
}
reportC("\n\tRead error while performing drive cache checks;"
"\n\t choosing new offset and trying again.\n");
}else{
if(x==-1){
reportC("\n\tTiming error while performing drive cache checks; aborting test.\n");
return(-1);
}else{
if(x<MIN_SEEK_MS)under=1;
break;
}
}
}
}
printC("\r");
if(under){
reportC("\nWARNING: Drive cache does not appear to be contiguous!\n");
warn=1;
}else{
reportC("\tDrive cache tests as contiguous \n");
}
}
/* The readahead cache size ascertained above is likely qualified by
background 'rollahead'; that is, the drive's readahead process is
often working ahead of our actual linear reads, and if reads stop
or are interrupted, readahead continues and overflows the cache.
It is also the case that the cache size we determined above is
slightly too low because readahead is probably always working
ahead of reads.
Determine the rollahead size a few ways (which may disagree:
1) Read number of sectors equal to cache size; pause; read backward until seek
2) Read sectors equal to cache-rollahead; verify reading back to beginning does not seek
3) Read sectors equal to cache; pause; read ahead until seek delay
*/
{
int lower=0;
int gran=64;
int it=3;
int tests=0;
int under=1;
readahead=0;
while(gran>1 || under){
tests++;
if(tests>8 && gran<64){
gran<<=3;
tests=0;
it=3;
}
if(gran && !under){
gran>>=3;
tests=0;
if(gran==1)it=10;
}
under=0;
readahead=lower+gran;
printC("\r");
logC("\n");
reportC("\tTesting background readahead past read cursor... %d",readahead);
printC(" \b\b\b\b\b\b\b\b\b\b\b");
for(i=0;i<it;i++){
int sofar=0,ret;
logC("\n\t\t%d >>> ",i);
while(sofar<cachesize){
ret = cdda_read_timed(d,NULL,offset+sofar,cachesize-sofar,&x);
if(ret<=0)goto error;
logC("%d:%d:%d ",offset+sofar,ret,x);
/* some drives can lose sync and perform an internal resync,
which can also cause readahead to restart. If we see
seek-like delays during the initial cahe load, retry the
preload */
sofar+=ret;
}
printC(".");
/* what we'd predict is needed to let the readahead process work. */
{
int usec=mspersector*(readahead)*(6+i)*200;
int max= 13000*2*readahead; /* corresponds to .5x */
if(usec>max)usec=max;
logC("sleep=%dus ",usec);
usleep(usec);
}
/* seek to offset+cachesize+readahead */
ret = cdda_read_timed(d,NULL,offset+cachesize+readahead-1,1,&x);
if(ret<=0)break;
logC("seek=%d:%d:%d",offset+cachesize+readahead-1,ret,x);
if(x<MIN_SEEK_MS){
under=1;
break;
}else if(i%3==1){
/* retime the drive just to be conservative */
mspersector=retime_drive(d, progress, log, offset, readahead, mspersector);
}
}
if(under)
lower=readahead;
}
readahead=lower;
}
