/* * The MIT License * * Copyright (c) 2007 MIT 6.270 Robotics Competition * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. * */ /* Gyro demo program for gyro with noise injection. Howard Samuels, 11/16/2004 howard.samuels@analog.com Modified for Happyboard Ross Glashan, 07/05/2005 rng@mit.edu */ #include #include #include /** Led sensor threshold value */ uint16_t lst = 400; /** Variable that keeps track of what step in our plan we're in */ uint16_t plan_step = 0; /** Starting positions 1-7 i.e 0 = black side facing N, 1 = black side facing E etc */ uint8_t start_pos = 0; /** Desired score at the end of the game */ uint16_t score = 19; /** ***********UTILITY FUNCTIONS************************************************ */ /** Display prompt and ask user to press go */ void prompt(char testName[]) { printf("\n%sPress Go",testName); go_click(); } /** pop balls with the motor set at velocity */ void pop_balls(uint8_t balls,uint16_t velocity){ encoder_reset(27); while(encoder_read(27) <= 111*balls){\ printf("\nticks=%d",encoder_read(27)); motor_set_vel(4,velocity); pause(50); } motor_set_vel(4,0); } /** Rotate a certain number of degrees */ void rotate(int16_t degrees){ encoder_reset(24); encoder_reset(25); if (degrees < 0){ while(((int16_t) encoder_read(25) + (degrees+1)) < 0){ motor_set_vel(3,180); motor_set_vel(2,-180); printf("degrees=%d",degrees); } motor_set_vel(3,0); motor_set_vel(2,0); } else{ while(encoder_read(24) < (degrees+1)){ motor_set_vel(2,180); motor_set_vel(3,-180); } motor_set_vel(2,0); motor_set_vel(3,0); } } void set_start_direction(){ //depending on where we started (global variable start_pos), rotate a certain number of degrees switch(start_pos){ case 0: rotate(0-10); break; case 1: rotate(90-10); break; case 2: rotate(180-10); break; case 3: rotate(-90-10); break; case 4: rotate(-180+10); break; case 5: rotate(-90+10); break; case 6: rotate(0+10); break; case 7: rotate(90+10); break; default: ; } } //drive to our four bin void drive_to_4bin(){ encoder_reset(24); encoder_reset(25); uint32_t start_time = get_time(); uint16_t e24 = encoder_read(24); uint16_t e25 = encoder_read(25); motor_set_vel(2, -230); motor_set_vel(3, -230); while ((get_time() - start_time) <= 6000){ pause(20); e24 = encoder_read(24); e25 = encoder_read(25); int16_t error = (int16_t) e25 - e24; motor_set_vel(2,-(235 - (error*3))); motor_set_vel(3,-(235 + (error*3))); } } //drive to our four bin void drive_to_4bin2(){ encoder_reset(24); encoder_reset(25); uint32_t start_time = get_time(); uint16_t e24 = encoder_read(24); uint16_t e25 = encoder_read(25); motor_set_vel(2, 230); motor_set_vel(3, 230); while ((get_time() - start_time) <= 5000){ pause(20); e24 = encoder_read(24); e25 = encoder_read(25); int16_t error = (int16_t) e25 - e24; motor_set_vel(2,(235 - (error*3))); motor_set_vel(3,(235 + (error*3))); } motor_set_vel(2,0); motor_set_vel(3,0); } void drop_balls_in_4bin(){ pop_balls((score+1)/4,200); } /** ************************** PLANNER ************************************** * Turn robot to face the right direction, drive directly to our four bin, * turn around (about 120 degrees), drop balls into our four bin, * drive straight to opponents 4 bin, stayput */ void plan(){ while (plan_step < 5){ switch(plan_step){ case 0 : set_start_direction(); //rotate until we face our four bin plan_step++; break; case 1 : drive_to_4bin(); //drive to our four bin plan_step++; break; case 2 : drop_balls_in_4bin(); plan_step++; break; case 3 : //drop balls depending on the global variable "score" plan_step++; break; case 4: drive_to_4bin2(); //drive to opponents four bin and park there plan_step++; break; default: ; } } } /** ************************* STARTUP FUNCTIONS ************************** */ //set the black-white sensor threshold corresponding to values obtained //from led sensors by placing robot over white and black surfaces respectively void set_sensor_threshold(){ uint16_t port1 = analog_read(8); uint16_t port2 = analog_read(10); uint16_t port3 = analog_read(12); uint16_t port4 = analog_read(14); uint16_t wval = 75; uint16_t bval = 800; while (!stop_press()){ port1 = analog_read(8); port2 = analog_read(10); port3 = analog_read(12); port4 = analog_read(14); wval = (port1 + port2 + port3 + port4 )/4; printf("\nwval=%3d",wval); pause(50); } prompt("\nBlack Surface "); while (!stop_press()){ port1 = analog_read(8); port2 = analog_read(10); port3 = analog_read(12); port4 = analog_read(14); bval = (port1 + port2 + port3 + port4)/4; printf("\nbval=%3d",bval); pause(50); } lst = ((bval - wval)/2) + wval; } void find_startPos(){ uint16_t port1 = analog_read(8); uint16_t port2 = analog_read(10); uint16_t port3 = analog_read(12); uint16_t port4 = analog_read(14); while (!stop_press()){ port1 = analog_read(8); port2 = analog_read(10); port3 = analog_read(12); port4 = analog_read(14); if(port1 > lst && port2 > lst && port3 > lst && port4 < lst){ printf("\nBlack side facing East"); start_pos = 1; } else if(port1 > lst && port2 > lst && port3 < lst && port4 > lst){ printf("\nBlack side facing North"); start_pos = 0; } else if(port1 > lst && port2 < lst && port3 > lst && port4 > lst){ printf("\nBlack side facing West"); start_pos = 3; } else if(port1 < lst && port2 > lst && port3 > lst && port4 > lst){ printf("\nBlack side facing South"); start_pos = 2; } else if(port1 < lst && port2 < lst && port3 < lst && port4 > lst){ printf("\nWhite side facing South"); start_pos = 6; } else if(port1 < lst && port2 < lst && port3 > lst && port4 < lst){ printf("\nWhite side facing East"); start_pos = 5; } else if(port1 < lst && port2 > lst && port3 < lst && port4 < lst){ printf("\nWhite side facing North"); start_pos = 4; } else if(port1 > lst && port2 < lst && port3 < lst && port4 < lst){ printf("\nWhite side facing West"); start_pos = 7; } else { printf("\nError"); } } } void set_game_score(){ while (!stop_press()){ score = (frob_read()/127) + 15; printf("\nscore=%d",score); pause(50); } } void calibrate(){ encoder_reset(24); encoder_reset(25); encoder_reset(27); prompt("\nEnter Game Score"); set_game_score(); prompt("\nWhite Surface "); set_sensor_threshold(); while(!stop_press()){ printf("\nCalibration done "); pause(50); } } // usetup is called during the calibration period. It must return before the // period ends. int usetup (void) { calibrate(); return 0; } int umain(void) { //find starting position and set it while(!stop_press()){ find_startPos(); printf("\nstart=%d",start_pos); pause(50); } plan(); return 0; }