/************************************************************************* * Title mmh_code.c * * Programmer Lee Cofer * * Date 4/14/2002 * * Version 9 * * * * Description * * This program performs obstacle avoidance, magnet detection, * * magnet marking, and border avoidance. * * ************************************************************************/ /*************************** Includes ********************************/ #include #include #include #include #include /************************ End of Includes ****************************/ /*************************** Constants ********************************/ #define CDSL CDS[0] #define CDSR CDS[5] #define IRE_OUT *(unsigned char *)(0xffb9) /* Address of Digital_Out */ #define IRE_ON 0x1f /* IR emitters on */ #define IRE_OFF 0x00 /* IR emitters off */ #define LED_AVOID 0x3f /* Avoid LED on */ #define LED_MINE_DET 0x5f /* Mine detection LED on */ #define LED_BORDER 0x9f /* Border LED on */ #define SERVO1 0 #define IRR analog(2) #define IRL analog(4) #define IRC analog(5) #define IR_THRESHOLD 100 #define CDS_THRESHOLD 95 #define FORWARD 100 #define REVERSE -100 #define HFORW 80 #define HREV -80 #define STOP 0 #define RIGHT_MOTOR 1 #define LEFT_MOTOR 0 #define BUMPER_FUZZY_ZERO 12 /* Noise immunity for bumper readings */ #define HALL1 analog(6) #define HALL2 analog(7) #define HALL3 IRDT[8] #define HALL4 IRDT[9] #define HALL5 IRDT[10] #define HALL6 IRDT[11] #define HALL7 IRDT[12] /************************ End of Constants ****************************/ /*************************** Prototypes *********************************/ void turn(void); void avoid(void); void mineDetect(void); void change_speed(void); void check_cds(void); void borderDetect(void); void init_CDS(void); void front_bump(void); void back_bump(void); /************************ End of Prototypes *****************************/ /***************************** Globals **********************************/ int irdr,irdl,irdc,rspeed,Rspeed= 0, lspeed,Lspeed = 0, test1 = 0 ; int hall1, hall2, hall3, hall4, hall5, hall6, hall7, hall8, hall9, hall10, hall11, hall12; int open=2600, close=2200, BBtime = 80; int CDSR_THRESHOLD,CDSL_THRESHOLD; /************************** End of Globals ******************************/ void main(void) /****************************** Main ***********************************/ { init_analog(); init_motortk(); init_clocktk(); init_serial(); init_servos(); /*Start MMH when front bumper is pressed*/ while(front_bumper() IR_THRESHOLD)||(irdl > IR_THRESHOLD)|| (irdc > IR_THRESHOLD)) { IRE_OUT = LED_AVOID; if(irdc < irdl || irdc < irdr) { if(irdr > irdl) /* Start turning when something in front and keep turning until nothing is in front */ { lspeed = HREV; rspeed = HFORW; } else { lspeed = HFORW; rspeed = HREV; } } else { lspeed = HFORW; rspeed = HREV; } change_speed(); while((irdr > IR_THRESHOLD ) || (irdl > IR_THRESHOLD )|| (irdc > IR_THRESHOLD)) { irdr = IRR; irdl = IRL-5; irdc = IRC-13; if(front_bumper()>BUMPER_FUZZY_ZERO) // Check bumper { front_bump(); break; } borderDetect(); } wait(400); IRE_OUT = IRE_ON; } lspeed = rspeed = FORWARD; change_speed(); /* This checks the front bumper. If the bumper is pressed, TALRIK will back up, and turn. */ front_bump(); } void turn() /************************************************************************ * Function: Will turn in a random direction for a random amount of * * time * * Returns: None * * * * Inputs * * Parameters: None * * Globals: None * * Registers: TCNT * * Outputs * * Parameters: None * * Globals: None * * Registers: None * * Functions called: None * * Notes: * ************************************************************************/ { int i; unsigned rand; rand = TCNT; if (rand & 0x0001) { lspeed = HREV; rspeed = HFORW; change_speed(); } else { lspeed = HFORW; rspeed = HREV; change_speed(); } i=((rand % 1024) + 35) * 2; wait(i); } void mineDetect() {/************************************************************************ * Function: Will detect magnets and drop a BB on it * * time * * Returns: None * * * * Inputs * * Parameters: None * * Globals: None * * Registers: None * * Outputs * * Parameters: None * * Globals: None * * Registers: None * * Functions called: read_IR * * Notes: * ************************************************************************/ read_IR(); hall1=HALL1; hall2=HALL2; hall3=HALL3; hall4=HALL4; hall5=HALL5; hall6=HALL6; hall7=HALL7; if(hall1 < 10 || hall2 < 10 || hall3 < 10 || hall4 < 10 || hall5 < 10 || hall6 < 10 || hall7 < 10) { IRE_OUT = LED_MINE_DET; // Turn on LED for mine detection lspeed = rspeed = STOP; change_speed(); servo(SERVO1,open); wait(BBtime); servo(SERVO1,close); wait(2200); lspeed = rspeed = FORWARD; change_speed(); wait(300); IRE_OUT = IRE_ON; } } void change_speed() {/************************************************************************ * Function: Will slowly change the speed of both motors. * * time * * Returns: None * * * * Inputs * * Parameters: None * * Globals: None * * Registers: None * * Outputs * * Parameters: None * * Globals: None * * Registers: None * * Functions called: * * Notes: * ************************************************************************/ while (lspeed != Lspeed || rspeed != Rspeed) { if (Lspeed > lspeed) --Lspeed; else if (Lspeed < lspeed) ++Lspeed; if (Rspeed > rspeed) --Rspeed; else if (Rspeed < rspeed) ++Rspeed; motortk(LEFT_MOTOR,Lspeed); motortk(RIGHT_MOTOR,Rspeed); wait(1); } } void borderDetect() {/************************************************************************ * Function: Will check if robot is on border of search area and will * * turn in random direction if so. * * Returns: None * * * * Inputs * * Parameters: None * * Globals: None * * Registers: None * * Outputs * * Parameters: None * * Globals: None * * Registers: None * * Functions called: turn * * Notes: * ************************************************************************/ int i; IRE_OUT = LED_BORDER; read_CDS(); /* Read all 6 CDS photoresistor voltage dividers */ if(CDSR < CDSR_THRESHOLD || CDSL < CDSL_THRESHOLD) { motortk(LEFT_MOTOR, STOP); motortk(RIGHT_MOTOR, STOP); wait(100); lspeed = rspeed = HREV; change_speed(); wait(1000); turn(); wait(2000); lspeed = rspeed = FORWARD; change_speed(); } IRE_OUT = IRE_ON; } void init_CDS() {/************************************************************************ * Function: Will configure the threshold value of the CDS cells * * * * Returns: None * * * * Inputs * * Parameters: None * * Globals: None * * Registers: None * * Outputs * * Parameters: None * * Globals: None * * Registers: None * * Functions called: * * Notes: * ************************************************************************/ int i; for(i=0; i<30; i++) { read_CDS(); wait(150); } read_CDS(); /* Read all 6 CDS photoresistor voltage dividers */ CDSR_THRESHOLD = CDSR * 0.70 ; CDSL_THRESHOLD = CDSL * 0.70 ; } void front_bump() {/************************************************************************ * Function: This checks the front bumper. If the bumper is pressed, * MMH will back up, and turn. * * * * Returns: None * * * * Inputs * * Parameters: None * * Globals: None * * Registers: None * * Outputs * * Parameters: None * * Globals: None * * Registers: None * * Functions called: change_speed * * Notes: * ************************************************************************/ if(front_bumper()>BUMPER_FUZZY_ZERO) { motortk(LEFT_MOTOR, STOP); motortk(RIGHT_MOTOR, STOP); wait(300); lspeed = rspeed = HREV; change_speed(); wait(1000); turn(); } } void back_bump() {/************************************************************************ * Function: This checks the back bumper. If the bumper is pressed, * MMH will go forward, and turn. * * * * Returns: None * * * * Inputs * * Parameters: None * * Globals: None * * Registers: None * * Outputs * * Parameters: None * * Globals: None * * Registers: None * * Functions called: change_speed * * Notes: * ************************************************************************/ if(rear_bumper()>BUMPER_FUZZY_ZERO) { motortk(LEFT_MOTOR, STOP); motortk(RIGHT_MOTOR, STOP); wait(300); lspeed = rspeed = HFORW; change_speed(); wait(700); turn(); } }