diags2.c
#define looppause() ((msleep((long) 100)))
#define waitpause() ((msleep((long) 1000)))
#define knob100() ((100*knob()/255))
/* LAYOUT OF MOTORS AND SENSORS
----------------------------
Drive Motors 0,2 (+ is forward)
Turn Motor 3 (+ is right)
Dump Motor 1 (+ is dump)
Whap Servos
Left Shaft Encoder 7
Right Shaft Encoder 8
Linefollow Sensors 16 17 19 20 22
Front Line Sensors 23 31
Orientation Sensors 25 26 28 29
*/
#define drivemotor1 0
#define drivemotor2 2
#define turnmotor 3
#define dumpmotor 1
#define whapleftservo 0
#define whaprightservo 4
#define whapliftservo 2
#define leftencoder 7
#define rightencoder 8
#define startsensor 19
/* Driving speeds */
#define BASESPEED 50
#define SLOWSPEED 40
#define TURNSPEED 80
/* Linefollow State Values */
#define LFstraight 0
#define LFright 10
#define LFhardright 11
#define LFafterright 12
#define LFleft 20
#define LFhardleft 21
#define LFafterleft 22
#define LFpastintersection 50
/* Stop Events */
#define CLICKS 0
#define INTERSECTION 1
#define MIDINTERSECTION 2
#define FRONTINTERSECTION 3
#define CENTERSENSORBLACK 5
#define MIDONLINE 9
#define TIME 12
/* Whap Status */
int whapliftstatus= 9999; /* Bogus */
int whaparmstatus= 9999;
#define WHAPARMUP 6
#define WHAPARMDRIVE 7
#define WHAPARMDOWN 8
#define WHAPARMSUPERDOWN 9
#define WHAPARMLOOSE 10
#define WHAPLIFTUP 11
#define WHAPLIFTDOWN 12
/* For Timeout + Stall */
#define NOSTALL 0
#define STALLED 10
#define TIMEOUT 20
#define DEFAULT_STALL_TIME ((long) 5000)
long stalltimetilldie = DEFAULT_STALL_TIME;
long lastmotiontime;
int lastleftmotioncount;
int lastrightmotioncount;
#define DEFAULT_TIMEOUT ((long) 10000)
long actionfailsat;
/* Sensor Arrays */
int linefollowports[5];
int orientationports[4];
int frontlineports[2];
/* Thresholds for calibration */
int linefollowthreshold[5];
int frontlinethreshold[2];
int orientationthreshold[4];
/* globals to hold current drive and turn speed */
/* needed for acceleration and deceleration */
int CurrentTurnSpeed;
int CurrentDriveSpeed;
/* Measured values to compare for orientation */
int orientationdata[16];
int facing;
/* LineFollower Values */
int LFstate = 0;
float LFtime; /* Time we entered most recent state */
float LFcorrection;
#define LFturnpower 40
#define LFrebound .3 /* Fraction of turn time to correct back after LF turn */
/* Constant Guesses */
#define right90guess 760
#define left90guess 700
#define right90guessclicks 18
#define left90guessclicks 24
#define right180guessclicks 42
#define left180guessclicks 55
#define LEFT -1
#define RIGHT 1
int startpath = RIGHT; /* -1 is left, 1 is right (default) */
void
main() {
int tmpports[20];
printf("Kiss my shiny Lego ass! \n");
initportconstants();
enable_servos();
enable_encoder(leftencoder);
enable_encoder(rightencoder);
whapliftdown();
whapdrive();
/* Defaults for LAB TABLE */
linefollowthreshold[0] = 0x32;
linefollowthreshold[1] = 0x31;
linefollowthreshold[2] = 0x30;
linefollowthreshold[3] = 0x30;
linefollowthreshold[4] = 0x75;
frontlinethreshold[0] = 0x60;
frontlinethreshold[1] = 0xE6;
while(1) {
printf("All Stop ?\n");
if(startstoppress())
allstop();
printf("Normal Plan?\n");
if(startstoppress())
planR();
printf("AntiCrosser Plan?\n");
if(startstoppress())
planX();
printf("AntiProfessor Grabber?\n");
if(startstoppress())
planP();
printf("Drive Control?\n");
if(startstoppress())
drivecontrol();
printf("TurnThisFar?\n");
if(startstoppress())
turnthisfar();
printf("Ruler?\n");
if(startstoppress())
ruler();
printf("Grab Prof?\n");
if(startstoppress())
grabprofessor();
printf("LineFollow Calib?\n");
if(startstoppress())
linefollowcalib();
printf("Linefollow Disp?\n");
if(startstoppress())
linefollowdisplay();
printf("Test Exception- 5s killdrive\n");
if(startstoppress()) {
die_in((long) 10000);
printf("Result:%d\n",
driveuntil(CLICKS, 10000, BASESPEED));
startstoppress();
}
printf("Dump Test?\n");
if(startstoppress())
dump();
}
}
/* *****************Scripts *************** */
void
planP() { /* Profgrabber plan */
int tmp;
disable_servos();
linefollowcalib();
calibrateorientation();
enable_servos();
waitpause();
whapliftup();
whapdown();
printf("Ready...\n");
startstoppress();
waitpause();
start_machine(startsensor);
startingturneasy(); /* Always turns right if needed */
driveuntil(CLICKS, 8, BASESPEED); /* Get off starting light */
grabhacker(); /* leaves us stopped in jail */
msleep((long) 300);
whapliftupquick(); /* go over hacker */
driveuntil(CLICKS, 8, -BASESPEED);
driveuntil(CLICKS, 8, -SLOWSPEED); /* get off hacker */
smartdrive(0);
msleep((long) 200);
if(startpath == LEFT){ /* turn through front of board */
tmp = right180guessclicks;
printf("Turning RIGHT\n");
}
else if(startpath == RIGHT){
tmp = left180guessclicks;
printf("Turning LEFT\n");
}
turnuntil(CLICKS, tmp, -startpath*TURNSPEED);
grabhacker(); /* leaves us in other jail */
smartdrive(0);
doquad1(); /* grab first quad */
finishquad(); /* grab pair and second quad */
dump();
printf("Done.\n");
}
void
planX() { /* Crossing plan */
int tmp;
disable_servos();
linefollowcalib();
calibrateorientation();
enable_servos();
waitpause();
whapliftup();
whapdown();
printf("Ready...\n");
startstoppress();
waitpause();
start_machine(startsensor);
startingturneasy();
startpath = - startpath;
driveuntil(CLICKS, 8, BASESPEED);
grabhacker();
msleep((long) 300);
whapliftupquick();
driveuntil(CLICKS, 8, -BASESPEED);
driveuntil(CLICKS, 8, -SLOWSPEED);
smartdrive(0);
doquad1();
grabprofessor();
/* return to jail */
whapliftup();
driveuntil(CLICKS, 6, -BASESPEED);
linefollowuntil(INTERSECTION, 1, -BASESPEED);
turnonintersection(-startpath);
smartdrive(0);
driveuntil(CLICKS, 6, BASESPEED);
linefollowuntil(INTERSECTION, 1, BASESPEED);
if(linefollowuntil(INTERSECTION, 0, BASESPEED))
{while(1);}
dump();
printf("Done.\n");
}
void
planR() {
int tmp;
disable_servos();
linefollowcalib();
calibrateorientation();
enable_servos();
waitpause();
whapliftup();
whapdown();
printf("Ready...\n");
startstoppress();
waitpause();
start_machine(startsensor);
startingturneasy(); /* Always turns right if needed */
driveuntil(CLICKS, 8, BASESPEED); /* Get off starting light */
grabhacker(); /* leaves us stopped in jail */
msleep((long) 300);
whapliftupquick(); /* go over hacker */
driveuntil(CLICKS, 8, -BASESPEED);
driveuntil(CLICKS, 8, -SLOWSPEED); /* get off hacker */
smartdrive(0);
msleep((long) 200);
if(startpath == LEFT){ /* turn through front of board */
tmp = right180guessclicks;
printf("Turning RIGHT\n");
}
else if(startpath == RIGHT){
tmp = left180guessclicks;
printf("Turning LEFT\n");
}
turnuntil(CLICKS, tmp, -startpath*TURNSPEED);
grabhacker(); /* leaves us in other jail */
smartdrive(0);
doquad1(); /* grab first quad */
grabprofessor();
/* return to jail */
whapliftup();
driveuntil(CLICKS, 6, -BASESPEED);
linefollowuntil(INTERSECTION, 1, -BASESPEED);
turnonintersection(-startpath);
smartdrive(0);
driveuntil(CLICKS, 6, BASESPEED);
linefollowuntil(INTERSECTION, 1, BASESPEED);
if(linefollowuntil(INTERSECTION, 0, BASESPEED))
{while(1);}
dump();
printf("Done.\n");
}
void
grabhacker() {
driveuntil(MIDINTERSECTION, 0, BASESPEED);
/* whap is already up */
if(facing == 0 || facing == 2)
linefollowuntil(MIDINTERSECTION, 1, BASESPEED);
else
linefollowuntil(MIDINTERSECTION, 1, BASESPEED);
smartdrive(0);
msleep((long) 200);
driveuntil(MIDINTERSECTION, 1, -SLOWSPEED);
smartdrive(0);
whapliftdowndrive();
driveuntil(FRONTINTERSECTION, 1, -BASESPEED);
driveuntil(FRONTINTERSECTION, 0, -BASESPEED);
driveuntil(MIDINTERSECTION, 1, -BASESPEED);
driveuntil(CLICKS, 2, -BASESPEED);
smartdrive(0);
}
void
grabprofessor() {
whapliftupquick();
linefollowuntil(INTERSECTION, 0, BASESPEED); /* off int */
linefollowuntil(INTERSECTION, 1, BASESPEED); /* hit street */
driveuntil(CLICKS, 40, BASESPEED);
smartdrive(0);
whapliftdown();
driveuntil(CLICKS, 60, -BASESPEED);
smartdrive(0);
}
void
doquad1() {
driveuntil(INTERSECTION, 0, BASESPEED);
smartdrive(0);
whapliftupquick();
linefollowuntil(INTERSECTION, 1, BASESPEED);
smartdrive(0);
msleep((long) 250);
driveuntil(INTERSECTION, 1, -BASESPEED);
smartdrive(0);
turnonintersection(startpath);
whapliftdowndrive();
linefollowuntil(CLICKS, 20, BASESPEED);
linefollowuntil(FRONTINTERSECTION, 1, SLOWSPEED);
smartdrive(0);
intersectionwhap_lengthwise();
whapliftdowndrive();
linefollowuntil(CLICKS, 40, BASESPEED);
linefollowuntil(CLICKS, 10, SLOWSPEED);
smartdrive(0);
whap();
linefollowuntil(FRONTINTERSECTION, 1, SLOWSPEED);
smartdrive(0);
}
void
finishquad() {
intersectionwhap_sideways();
linefollowuntil(CLICKS, 40, BASESPEED);
linefollowuntil(INTERSECTION, 1, SLOWSPEED);
smartdrive(0);
turnonintersection(-startpath);
linefollowuntil(INTERSECTION, 0, -BASESPEED);
if(linefollowuntil(INTERSECTION, 1, -BASESPEED)) {
while(1);
}
smartdrive(0);
whapliftdown();
}
void
intersectionwhap_lengthwise() {
driveuntil(CLICKS, 3, -SLOWSPEED);
smartdrive(0);
msleep((long) 100);
whapdown();
whaploose();
whapup();
driveuntil(FRONTINTERSECTION, 0, SLOWSPEED);
/* msleep((long) 300); */
smartdrive(0);
whapliftup();
superwhapdown();
whapup();
driveuntil(MIDINTERSECTION, 1, SLOWSPEED);
smartdrive(0);
whapliftdown();
whapdown();
whaploose();
whapup();
driveuntil(INTERSECTION, 1, SLOWSPEED);
smartdrive(0);
whapliftup();
turnonintersection(startpath);
}
void
intersectionwhap_sideways() {
driveuntil(CLICKS, 3, -40);
smartdrive(0);
msleep((long) 50);
whapdown();
whaploose();
whapup();
driveuntil(FRONTINTERSECTION, 0, SLOWSPEED);
/* Not driving forward here??? */
smartdrive(0);
whapliftup();
superwhapdown();
whapup();
driveuntil(MIDINTERSECTION, 1, SLOWSPEED);
smartdrive(0);
whapliftdown();
whapdown();
whaploose();
whapup();
driveuntil(MIDINTERSECTION, 0, SLOWSPEED);
smartdrive(0);
whapdown();
driveuntil(MIDINTERSECTION, 1, -SLOWSPEED);
driveuntil(MIDINTERSECTION, 0, -SLOWSPEED);
smartdrive(0);
whapliftup();
superwhapdown();
driveuntil(INTERSECTION, 1, SLOWSPEED);
turnonintersection(startpath);
}
/* *********** TEST FUNCTIONS ***************** */
void
ruler() {
int initleft;
int initright;
waitpause();
initleft = read_encoder(leftencoder);
initright = read_encoder(rightencoder);
while(!start_button() && !stop_button()) {
printf("L%d R%d\n",
read_encoder(leftencoder) - initleft,
read_encoder(rightencoder) - initright);
looppause();
}
waitpause();
}
void
drivecontrol() {
while(1){
while(!start_button() && !stop_button()) {
printf("Drive Control: %d\n", (2*knob100())-100);
looppause();
}
if(stop_button())
return;
while(start_button() || stop_button());
waitpause();
smartdrive((2*knob100())-100);
msleep((long)3000);
smartdrive(0);
waitpause();
}
}
void
linefollowcalib() {
int whitevalues[5];
int blackvalues[5];
int flwhitevalues[2];
int flblackvalues[2];
int tmpvalues[5]; /* Main linefollow */
int tmp2values[2]; /* Frontlinefollow */
int i;
/* WHITE CALIBRATION */
while(1) {
smootharray(5, linefollowports, tmpvalues);
smootharray(2, frontlineports, tmp2values);
printf("%x%x%x%xWC-%x%x%x\n",
tmpvalues[0], tmpvalues[1], tmpvalues[2],
tmpvalues[3], tmpvalues[4], tmp2values[0], tmp2values[1]);
if(stop_button()) {
waitpause();
return; /* If user wants to break out */
}
if(start_button())
break;
looppause();
}
/* Copy white values */
for(i = 0 ; i < 5 ; i++) {
whitevalues[i] = tmpvalues[i];
}
flwhitevalues[0] = tmp2values[0];
flwhitevalues[1] = tmp2values[1];
waitpause();
/* BLACK CALIBRATION */
while(1) {
smootharray(5, linefollowports, tmpvalues);
smootharray(2, frontlineports, tmp2values);
printf("%x%x%x%xBC-%x%x%x\n",
tmpvalues[0], tmpvalues[1], tmpvalues[2],
tmpvalues[3], tmpvalues[4], tmp2values[0], tmp2values[1]);
if(stop_button()) {
waitpause();
return; /* If user wants to break out */
}
if(start_button())
break;
looppause();
}
/* Copy black values */
for(i = 0 ; i < 5 ; i++) {
blackvalues[i] = tmpvalues[i];
}
flblackvalues[0] = tmp2values[0];
flblackvalues[1] = tmp2values[1];
waitpause();
/* SET THRESHOLDS */
for(i = 0 ; i < 5 ; i++) {
linefollowthreshold[i] =
(blackvalues[i] + whitevalues[i]) / 2;
}
frontlinethreshold[0] = (flblackvalues[0] +
flwhitevalues[0]) / 2;
frontlinethreshold[1] = (flblackvalues[1] +
flwhitevalues[1]) / 2;
printf("%x%x%x%xTh-%x%x%x\n",
linefollowthreshold[0], linefollowthreshold[1],
linefollowthreshold[2], linefollowthreshold[3],
linefollowthreshold[4],
frontlinethreshold[0], frontlinethreshold[1]);
startstoppress();
waitpause();
}
/* Linefollow Display */
void
linefollowdisplay() {
int tmp[5];
int tmp2[2];
while(1) {
binarylinearray(tmp);
binaryfrontarray(tmp2);
printf("Linefollow: %d%d %d%d%d%d%d\n",
tmp2[0], tmp2[1],
tmp[0], tmp[1], tmp[2], tmp[3], tmp[4]);
looppause();
if(stop_button())
break;
}
waitpause();
}
void
turnonintersection(int direction) {
int linearray[5];
int sawleft = 0;
int sawright = 0;
smartturn(direction*TURNSPEED);
msleep((long) 250); /* Just a small guess */
while(1) {
binarylinearray(linearray);
sawleft = sawleft || linearray[0];
sawright = sawright || linearray[4];
if(sawleft && sawright)
break;
}
smartturn(0);
}
void
orientationtest() {
while(1) {
printf("I'm facing: %d\n", whichfacing());
if(stop_button()) {
waitpause();
break;
}
looppause();
}
}
int
startingturneasy() {
facing = whichfacing();
if(facing == 0) {
turnuntil(CLICKS, right90guessclicks, TURNSPEED);
startpath = LEFT;
}
else if (facing == 1) {
startpath = RIGHT;
}
else if (facing == 2) {
turnuntil(CLICKS, right90guessclicks, TURNSPEED);
startpath = RIGHT;
}
else if (facing == 3) {
startpath = LEFT;
}
return(facing);
}
void
calibrateorientation() {
int i;
int tmpdata0[4];
int tmpdata1[4];
int tmpdata2[4];
int tmpdata3[4];
while (1) {
printf("Set facing 0: %d\n", analog(orientationports[0]));
if(stop_button()) {
waitpause();
return; /* If user wants to break out */
}
if(start_button())
break;
looppause();
}
if(startstoppress()) {
waitpause();
smootharray(4, orientationports, tmpdata0);
}
else return;
printf("Set facing 1: %d\n", analog(orientationports[1]));
if(startstoppress()) {
waitpause();
smootharray(4, orientationports, tmpdata1);
}
else return;
printf("Set facing 2: %d\n", analog(orientationports[2]));
if(startstoppress()) {
waitpause();
smootharray(4, orientationports, tmpdata2);
}
else return;
printf("Set facing 3: %d\n", analog(orientationports[3]));
if(startstoppress()) {
waitpause();
smootharray(4, orientationports, tmpdata3);
}
else return;
printf("Keep This Data?\n");
if(startstoppress()) {
for(i=0 ; i < 4 ; i++) {
orientationdata[i] = tmpdata0[i];
orientationdata[i+4] = tmpdata1[i];
orientationdata[i+8] = tmpdata2[i];
orientationdata[i+12] = tmpdata3[i];
}
}
waitpause();
}
/* ************************************************* */
/* ************************************************* */
/* ************************************************* */
/* ************************************************* */
/* ************************************************* */
/* ********** UTILITY FUNCTIONS *************** */
int startstoppress() {
int answer;
while(!stop_button() && !start_button()) {
msleep((long) 10);
}
answer = start_button();
while(start_button() || stop_button()) {
msleep((long) 10);
}
return(answer);
}
int abs(int num) {
if(num < 0)
return(-num);
else
return(num);
}
int sign(int num) {
if(num > 0)
return(1);
else if (num < 0)
return(-1);
else
return(0);
}
void
die_in(long delta) {
actionfailsat = mseconds() + delta;
}
int
checkstall() {
if((read_encoder(leftencoder) > lastleftmotioncount) ||
(read_encoder(rightencoder) > lastrightmotioncount)) {
/* we've moved */
lastmotiontime = mseconds();
lastleftmotioncount = read_encoder(rightencoder);
lastrightmotioncount = read_encoder(leftencoder);
}
else if(mseconds() > (lastmotiontime + stalltimetilldie)) {
return(STALLED); /* no movement */
}
if(mseconds() > actionfailsat) {
return(TIMEOUT);
}
return(NOSTALL);
}
void
binarylinearray(int binarray[]) {
int i;
smootharray(5, linefollowports, binarray);
for(i = 0 ; i < 5 ; i++) {
binarray[i] = (binarray[i] >= linefollowthreshold[i]);
}
}
void
binaryfrontarray(int binarray[]) {
int i;
smootharray(2, frontlineports, binarray);
for(i = 0 ; i < 2 ; i++) {
binarray[i] = (binarray[i] >= frontlinethreshold[i]);
}
}
void
whap() {
/* smartdrive(-100);
msleep((long) 20);
smartdrive(0);*/
driveuntil(CLICKS, 3, -40);
smartdrive(0);
msleep((long) 100);
whapdown();
whaploose();
whapup();
driveuntil(CLICKS, 8, 30);
/* msleep((long) 300); */
smartdrive(0);
whapliftup();
superwhapdown();
whapup();
driveuntil(CLICKS, 16, 30);
smartdrive(0);
whapliftdown();
whapdrive();
}
void
whapup() {
if(whaparmstatus == WHAPARMUP)
return;
servo(whapleftservo, 3900);
servo(whaprightservo, 100);
msleep((long) 700);
whaparmstatus = WHAPARMUP;
}
void
whapdown() {
if(whaparmstatus == WHAPARMDOWN)
return;
servo(whapleftservo, 680);
servo(whaprightservo, 3440);
msleep((long) 300);
whaparmstatus = WHAPARMDOWN;
}
void
superwhapdown() {
if(whaparmstatus == WHAPARMSUPERDOWN)
return;
servo(whapleftservo, 0);
servo(whaprightservo, 4000);
msleep((long) 300);
whaparmstatus = WHAPARMSUPERDOWN;
}
void
whapdrive() {
if(whaparmstatus == WHAPARMDRIVE)
return;
servo(whapleftservo, 2520);
servo(whaprightservo, 1600);
msleep((long) 300);
whaparmstatus = WHAPARMDRIVE;
}
void
whaploose() {
if(whaparmstatus == WHAPARMLOOSE)
return;
servo(whapleftservo, 720);
servo(whaprightservo, 3400);
msleep((long) 50);
whaparmstatus = WHAPARMLOOSE;
}
void
whapliftup() {
if(whapliftstatus == WHAPLIFTUP)
return;
if((whaparmstatus != WHAPARMUP) ||
(whaparmstatus != WHAPARMDRIVE)) {
whapdrive();
}
servo(whapliftservo, 4000);
msleep((long) 1500);
}
void
whapliftupquick() {
if(whapliftstatus == WHAPLIFTUP)
return;
if((whaparmstatus != WHAPARMUP) ||
(whaparmstatus != WHAPARMDRIVE)) {
whapdrive();
}
servo(whapliftservo, 4000);
msleep((long) 300);
}
void
whapliftdown() {
if(whapliftstatus == WHAPLIFTDOWN)
return;
if((whaparmstatus != WHAPARMUP) ||
(whaparmstatus != WHAPARMDRIVE)) {
whapdrive();
}
servo(whapliftservo, 0);
msleep((long) 800);
}
void
whapliftdowndrive() {
if((whapliftstatus == WHAPLIFTDOWN) &&
(whaparmstatus == WHAPARMDRIVE)) {
return;
}
else if(whaparmstatus == WHAPARMDRIVE) {
servo(whapliftservo, 0);
msleep((long) 800);
}
else if(whapliftstatus == WHAPLIFTDOWN) {
whapdrive();
}
else if(whaparmstatus == WHAPARMUP) {
servo(whapliftservo, 0);
servo(whapleftservo, 2520);
servo(whaprightservo, 1600);
msleep((long) 800);
}
else {
whapliftdown();
}
}
void
dump() {
motor(dumpmotor, 50); /* gears jam down sometimes */
msleep((long) 1000);
motor(dumpmotor, 0);
msleep((long) 600);
motor(dumpmotor, -60);
msleep((long) 500);
motor(dumpmotor, 0);
}
void
drive(int speed) {
motor(drivemotor1, speed);
motor(drivemotor2, speed);
}
void
smartdrive(int minspeed) {
int leftencoderinit;
int acc;
if(minspeed < 0)
minspeed =(int)(((float)minspeed) *1.2);
if(abs(minspeed) > 100) minspeed = 100*sign(minspeed);
drive(minspeed);
return();
leftencoderinit = read_encoder(leftencoder);
acc = minspeed - CurrentDriveSpeed;
if(CurrentDriveSpeed == 0 && minspeed != 0){
CurrentDriveSpeed += acc/3;
while(abs(CurrentDriveSpeed) <= abs(minspeed) || leftencoderinit == read_encoder(leftencoder)){
CurrentDriveSpeed += sign(acc);
drive(CurrentDriveSpeed);
msleep((long)2);
}
}
else{
CurrentDriveSpeed += acc/3;
while( CurrentDriveSpeed != minspeed) {
CurrentDriveSpeed += sign(acc);
drive(CurrentDriveSpeed);
msleep((long)1);
}
}
}
void
turn(int speed){
motor(turnmotor, speed);
/*if(speed == 0) CurrentTurnSpeed = 0;*/
}
void
smartturn(int speed) {
turn(speed);
return();
motor(turnmotor, (speed + CurrentTurnSpeed)/2);
msleep((long)20);
CurrentTurnSpeed = speed;
motor(turnmotor, CurrentTurnSpeed);
}
void
allstop(){
drive(0);
turn(0);
}
void
initportconstants() {
linefollowports[0] = 16;
linefollowports[1] = 17;
linefollowports[2] = 19;
linefollowports[3] = 20;
linefollowports[4] = 22;
orientationports[0] = 25;
orientationports[1] = 26;
orientationports[2] = 28;
orientationports[3] = 29;
frontlineports[0] = 23;
frontlineports[1] = 31;
}
void
smootharray(int numports, int portarray[], int samples[]) {
int port;
for(port = 0 ; port < numports ; port++) {
samples[port] = analog(portarray[port]);
}
}
int
whichfacing() {
int bestchoice;
int sensordata[4];
float errorval;
int i;
smootharray(4, orientationports, sensordata);
bestchoice = 0;
errorval = sq4error(sensordata, orientationdata, 0);
for(i = 1 ; i < 4 ; i++) { /* For each facing */
if( (sq4error(sensordata, orientationdata, i)) < errorval) {
/* If it's a better fit */
errorval = sq4error(sensordata, orientationdata, i);
bestchoice = i;
}
}
return(bestchoice);
}
float
sq4error(int sensordata[], int orientationdata[], int offset) {
int j;
float errorval;
float tmp;
errorval = (float) 0;
for(j = 0 ; j < 4 ; j++) {
tmp = ((float) (sensordata[j] - orientationdata[(4*offset)+j]))/((float)sensordata[j]);
errorval += (tmp*tmp);
}
return(errorval);
}
int
linefollowuntil(int event, int option, int speed) {
/* Event is clicks, intersect, sensor trigger, time */
/* Option can be an integer (in case of clicks, time) */
/* or a boolean indicating the presence (1) or absence (0) of a condition. */
/* Speed is obvious */
int frontarray[2];
int linearray[5];
int leftencoderinit;
int rightencoderinit;
long inittime;
int sawleft = 0;
int sawright = 0;
int isstalled;
leftencoderinit = read_encoder(leftencoder);
rightencoderinit = read_encoder(rightencoder);
lastmotiontime = mseconds(); /* Reset stall time */
inittime = mseconds();
LFstate = 0;
/* If user didn't specify a timeout, set default.
(User didn't specify if actionfailsat = 0) */
if(actionfailsat < mseconds())
actionfailsat = (mseconds() + DEFAULT_TIMEOUT);
smartdrive(speed);
while(1) {
/* Check for stall. Return stall code if stalled. */
isstalled = checkstall();
if(isstalled) {
actionfailsat = (long) 0;
alloff();
return(isstalled);
}
binaryfrontarray(frontarray);
binarylinearray(linearray);
if(event == CLICKS) {
/* CLICKS CODE */
if(((read_encoder(leftencoder) - leftencoderinit) >= option)
|| ((read_encoder(rightencoder) - rightencoderinit) >= option))
break;
}
else if(event == INTERSECTION) {
/* INTERSECTION CODE */
if(option) { /* Looking for intersection */
sawleft = sawleft || linearray[0];
sawright = sawright || linearray[4];
if(sawleft && sawright)
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
sawleft = sawleft || (!linearray[0]);
sawright = sawright || (!linearray[4]);
if(sawleft && sawright)
break;
}
}
else if(event == MIDINTERSECTION) {
/* FRONT INTERSECTION CODE */
if(option) { /* Looking for intersection */
if(linearray[1] && linearray[2] && linearray[3])
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
if(!linearray[1] || !linearray[2] || !linearray[3])
break;
}
}
else if(event == FRONTINTERSECTION) {
/* FRONT INTERSECTION CODE */
if(option) { /* Looking for intersection */
if(frontarray[0] && frontarray[1])
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
if(!frontarray[0] || !frontarray[1])
break;
}
}
else if(event == CENTERSENSORBLACK) {
/* CENTERSENSOR CODE */
if(option) { /* Looking for center to hit black */
if(linearray[2])
break;
}
else { /* Looking for center to hit white */
if(!linearray[2])
break;
}
}
else if(event == TIME) { /* Event happens for option ms */
if((mseconds() - inittime) > ((long) option))
break;
}
linefollowadjust(linearray, sign(speed));
}
turn(0);
/* If we succede, reset actionfailsat */
actionfailsat = ((long) 0);
}
int
driveuntil(int event, int option, int speed) {
/* Event is clicks, intersect, sensor trigger, time */
/* Option can be an integer (in case of clicks, time) */
/* or a boolean indicating the presence (1) or absence (0) of a condition. */
/* Speed is obvious */
int frontarray[2];
int linearray[5];
int leftencoderinit;
long inittime;
int sawleft = 0;
int sawright = 0;
int isstalled;
leftencoderinit = read_encoder(leftencoder);
lastmotiontime = mseconds(); /* Reset stall time */
inittime = mseconds();
smartdrive(speed);
/* If user didn't specify a timeout, set default.
(User didn't specify if actionfailsat = 0) */
if(actionfailsat < mseconds())
actionfailsat = (mseconds() + DEFAULT_TIMEOUT);
while(1) {
/* Check for stall. Return stall code if stalled. */
isstalled = checkstall();
if(isstalled) {
actionfailsat = (long) 0;
alloff();
return(isstalled);
}
binaryfrontarray(frontarray);
binarylinearray(linearray);
if(event == CLICKS) {
/* CLICKS CODE */
if(((read_encoder(leftencoder) - leftencoderinit) >= option))
break;
}
else if(event == INTERSECTION) {
/* INTERSECTION CODE */
if(option) { /* Looking for intersection */
sawleft = sawleft || linearray[0];
sawright = sawright || linearray[4];
if(sawleft && sawright)
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
sawleft = sawleft || (!linearray[0]);
sawright = sawright || (!linearray[4]);
if(sawleft && sawright)
break;
}
}
else if(event == MIDINTERSECTION) {
/* MID INTERSECTION CODE */
if(option) { /* Looking for intersection */
if(linearray[1] && linearray[2] && linearray[3])
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
if(!linearray[1] || !linearray[2] || !linearray[3])
break;
}
}
else if(event == FRONTINTERSECTION) {
/* FRONT INTERSECTION CODE */
if(option) { /* Looking for intersection */
if(frontarray[0] && frontarray[1])
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
if(!frontarray[0] || !frontarray[1])
break;
}
}
else if(event == CENTERSENSORBLACK) {
/* CENTERSENSOR CODE */
if(option) { /* Looking for center to hit black */
if(linearray[2])
break;
}
else { /* Looking for center to hit white */
if(!linearray[2])
break;
}
}
else if(event == TIME) { /* Event happens for option ms */
if((mseconds() - inittime) > ((long) option))
break;
}
}
/* If we succede, reset actionfailsat */
actionfailsat = ((long) 0);
}
int
turnuntil(int event, int option, int speed) {
/* Event is clicks, intersect, sensor trigger, time */
/* Option can be an integer (in case of clicks, time) */
/* or a boolean indicating the presence (1) or absence (0) of a condition. */
/* Speed is obvious */
int frontarray[2];
int linearray[5];
int rightencoderinit;
long inittime;
int sawleft = 0;
int sawright = 0;
int isstalled;
rightencoderinit = read_encoder(rightencoder);
lastmotiontime = mseconds(); /* Reset stall time */
inittime = mseconds();
/* If user didn't specify a timeout, set default.
(User didn't specify if actionfailsat = 0) */
if(actionfailsat < mseconds())
actionfailsat = (mseconds() + DEFAULT_TIMEOUT);
turn(0);
smartturn(speed);
while(1) {
/* Check for stall. Return stall code if stalled. */
isstalled = checkstall();
if(isstalled) {
actionfailsat = (long) 0;
alloff();
return(isstalled);
}
binaryfrontarray(frontarray);
binarylinearray(linearray);
if(event == CLICKS) {
/* CLICKS CODE- NOT VALID ON TURN */
if((read_encoder(rightencoder) - rightencoderinit) >= option)
break;
}
else if(event == INTERSECTION) {
/* INTERSECTION CODE */
if(option) { /* Looking for intersection */
sawleft = sawleft || linearray[0];
sawright = sawright || linearray[4];
if(sawleft && sawright)
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
sawleft = sawleft || (!linearray[0]);
sawright = sawright || (!linearray[4]);
if(sawleft && sawright)
break;
}
}
else if(event == MIDINTERSECTION) {
/* MID INTERSECTION CODE */
if(option) { /* Looking for intersection */
if(linearray[1] && linearray[2] && linearray[3])
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
if(!linearray[1] || !linearray[2] || !linearray[3])
break;
}
}
else if(event == FRONTINTERSECTION) {
/* FRONT INTERSECTION CODE */
if(option) { /* Looking for intersection */
if(frontarray[0] && frontarray[1])
break;
}
else { /* Looking for end of intersection-
Triggers when any of 3 sensors sees white */
if(!frontarray[0] || !frontarray[1])
break;
}
}
else if(event == CENTERSENSORBLACK) {
/* CENTERSENSOR CODE */
if(option) { /* Looking for center to hit black */
if(linearray[2])
break;
}
else { /* Looking for center to hit white */
if(!linearray[2])
break;
}
}
else if(event == TIME) { /* Event happens for option ms */
if((mseconds() - inittime) > ((long) option))
break;
}
}
smartturn(0);
/* If we succed, reset actionfailsat */
actionfailsat = ((long) 0);
}
int
linefollowadjust(int linearray[], int forwardbackward) {
/* Called to make line following adjustments */
/* linearray[] is a 5 value array of the binary line values */
/* Return Values */
int no_error = 0;
int ambiguous = 1;
int no_lines = 2;
int freaky = 3;
int threebitvalue; /* holds binary representation of inner sensors */
threebitvalue = ((linearray[1] << 2) + (linearray[2] << 1) + (linearray[3] << 0));
if(LFstate == LFstraight){
/* Begin LFstraight */
if(threebitvalue == 0){ /* 000- No Lines */
return(no_lines);
}else if(threebitvalue == 2){ /* Straight */
}else if(threebitvalue == 1 /* 001, 011- Need right turn */
|| threebitvalue == 3){
LFstate = LFright;
LFtime = seconds();
turn((forwardbackward*LFturnpower));
}else if(threebitvalue == 4 /* 100, 110- Need left turn */
|| threebitvalue == 6){
LFstate = LFleft;
LFtime = seconds();
turn(-(forwardbackward*LFturnpower));
}else if(threebitvalue == 5){ /* 101- Ambiguous */
return(ambiguous);
}else if(threebitvalue == 7){ /* 111- Intersection */
LFstate = LFpastintersection;
LFtime = seconds();
turn(0);
}
return(no_error);
}else if(LFstate == LFright){
/* Begin LFright */
if(threebitvalue == 0){ /* 000- No Lines */
return(no_lines);
}else if(threebitvalue == 2){ /* Straighten out */
LFstate = LFafterright;
LFcorrection = (LFtime - seconds()) * LFrebound;
LFtime = seconds();
turn(-(forwardbackward*LFturnpower));
}else if(threebitvalue == 1 /* 001, 011- Keep turning */
|| threebitvalue == 3){
}else if(threebitvalue == 4 /* 100, 110- Need left turn */
|| threebitvalue == 6){
LFstate = LFleft;
LFtime = seconds();
turn(-(forwardbackward*LFturnpower));
}else if(threebitvalue == 5){ /* 101- Ambiguous */
return(ambiguous);
}else if(threebitvalue == 7){ /* 111- Intersection */
LFstate = LFpastintersection;
LFtime = seconds();
turn(0);
}
return(no_error);
}else if(LFstate == LFafterright){
/* Begin LFafterright */
if(threebitvalue == 0){ /* 000- No Lines */
return(no_lines);
}else if(threebitvalue == 2){ /* Straight */
if((seconds() - LFtime) > LFcorrection) {
LFstate = LFstraight;
LFtime = seconds();
turn(0);
}
}else if(threebitvalue == 1 /* 001, 011- Need right turn */
|| threebitvalue == 3){
LFstate = LFright;
LFtime = seconds();
turn((forwardbackward*LFturnpower));
}else if(threebitvalue == 4 /* 100, 110- Need left turn */
|| threebitvalue == 6){
LFstate = LFleft;
LFtime = seconds();
turn(-(forwardbackward*LFturnpower));
}else if(threebitvalue == 5){ /* 101- Ambiguous */
return(ambiguous);
}else if(threebitvalue == 7){ /* 111- Intersection */
LFstate = LFpastintersection;
LFtime = seconds();
turn(0);
}
return(no_error);
}else if(LFstate == LFleft){
/* Begin LFleft */
if(threebitvalue == 0){ /* 000- No Lines */
return(no_lines);
}else if(threebitvalue == 2){ /* Straighten out */
LFstate = LFafterleft;
LFcorrection = (LFtime - seconds()) * LFrebound;
LFtime = seconds();
turn((forwardbackward*LFturnpower));
}else if(threebitvalue == 4 /* 100, 110- Keep turning */
|| threebitvalue == 6){
}else if(threebitvalue == 1 /* 001, 011- Need right turn */
|| threebitvalue == 3){
LFstate = LFright;
LFtime = seconds();
turn((forwardbackward*LFturnpower));
}else if(threebitvalue == 5){ /* 101- Ambiguous */
return(ambiguous);
}else if(threebitvalue == 7){ /* 111- Intersection */
LFstate = LFpastintersection;
LFtime = seconds();
turn(0);
}
return(no_error);
}else if(LFstate == LFafterleft){
/* Begin LFafterleft */
if(threebitvalue == 0){ /* 000- No Lines */
return(no_lines);
}else if(threebitvalue == 2){ /* Straight */
if((seconds() - LFtime) > LFcorrection) {
LFstate = LFstraight;
LFtime = seconds();
turn(0);
}
}else if(threebitvalue == 1 /* 001, 011- Need left turn */
|| threebitvalue == 3){
LFstate = LFright;
LFtime = seconds();
turn((forwardbackward*LFturnpower));
}else if(threebitvalue == 4 /* 100, 110- Need left turn */
|| threebitvalue == 6){
LFstate = LFleft;
LFtime = seconds();
turn(-(forwardbackward*LFturnpower));
}else if(threebitvalue == 5){ /* 101- Ambiguous */
return(ambiguous);
}else if(threebitvalue == 7){ /* 111- Intersection */
LFstate = LFpastintersection;
LFtime = seconds();
turn(0);
}
return(no_error);
}
/* LFpastintersection */
else if(LFstate == LFpastintersection){
if(threebitvalue == 0) {
return(no_lines); }
return(no_error);
}
}
void
turnthisfar() {
/* int leftinit;*/
int rightinit;
int thisfar;
while(1) {
while(!start_button() && !stop_button()) {
printf("Turn This Far: %d\n", 2*knob100()-100);
looppause();
}
if(stop_button()) {
waitpause();
break;
}
while(start_button());
thisfar = 2*knob100()-100;
rightinit = read_encoder(rightencoder);
turn(sign(thisfar)*100);
while((abs((read_encoder(rightencoder) - rightinit))
< abs(thisfar))
&& !stop_button()
&& !start_button()) {
printf("Turning %d\n",
sign(thisfar)*(read_encoder(rightencoder) - rightinit));
msleep((long) 5);
}
turn(0);
waitpause();
printf("Turned: %d\n",
read_encoder(rightencoder) - rightinit);
startstoppress();
}
}
#ifdef undef
void crossmassave(int direction) {
driveuntil(INTERSECTION, 0, direction*BASESPEED); /*hit stripe1*/
driveuntil(INTERSECTION, 1, direction*BASESPEED); /*off stripe2*/
driveuntil(INTERSECTION, 0, direction*BASESPEED); /*hit stripe2*/
}
#endif
/* ************************************
******SCRAP CODE **********************
************************************ */
#ifdef undef
void
servotest(int port) {
while(1) {
printf("Servo %d Test: %d\n", port, 40*knob100());
servo(port, 40*knob100());
if(stop_button())
break;
looppause();
}
waitpause();
}
void
motortest(int port) {
while(!start_button() && !stop_button()) {
printf("Motor %d Test: %d\n", port, (2*knob100())-100);
looppause();
}
if(stop_button())
return;
while(start_button() || stop_button());
while(1) {
printf("Motor %d Test: %d\n", port, (2*knob100())-100);
motor(port, (2*knob100())-100);
if(stop_button())
break;
looppause();
}
motor(port, 0);
waitpause();
}
void
driveandturnanddrive() {
whapliftup();
waitpause();
linefollowuntil(INTERSECTION, 1, 50);
smartdrive(0);
waitpause();
turnonintersection(RIGHT);
waitpause();
driveuntil(TIME, 600, 50);
linefollowuntil(INTERSECTION, 1, 60);
smartdrive(0);
waitpause();
}
void
whaptest() {
while(1) {
printf("WhapTest Push START to go\n");
if(startstoppress()) {
printf("Whapping!\n");
whap();
}
else {
waitpause();
break;
}
}
printf("HALTED\n");
}
#endif
Output generated by grind.