#include "ME218_E128.h" /* Their modules */ #include /* Our modules */ #include "tape.h" //4 tape sensors, goal 3 bump sensor, dispensor bump sensor, sorter sensors #include "IR.h" //2 stereo IR detectors, 2 minute counter #include "motor.h" //drive motors, ball release motors, sorter motor #include "actions.h" #include "Encoder.h" #include "Sorter.h" #include "math.h" #include "testing.h" #include #define LEFT 1 #define RIGHT 2 /* * Function: BlinkDebugLEDs * ------------------------ * Function to blink LEDs an input number of times. * For debugging purposes. */ void BlinkDebugLEDs(char n){ char i; for(i = 0; i < n; i++) { PTP = PTP | (BIT2HI | BIT0HI); WaitForTime(100); PTP = PTP & (BIT2LO & BIT0LO); WaitForTime(100); } } /* * Function: Initialize * -------------------- * Calls all intialization function necessary to implement * robot functionality. */ void Initialize(void) { /* outputs for debugging */ DDRP = DDRP | (BIT0HI | BIT2HI); PTP = PTP & (BIT0LO & BIT2LO); Initialize_Motor(); Initialize_Tape(); TMRS12_Init(TMRS12_RATE_2MS); InitializeButtons(); IRInit(); EncoderInit(); } /* * Function: CloseEverything * ------------------------- * Function to direct robot to close hopper and shooter, * making sure they are closed for start of game. */ void CloseEverything(void) { CloseShooter(); CloseHopper(); } /* * Function: main * ------------------------- * * . */ void main(void) { char side; Initialize();//The program begins by closing all chutes and initializing the microcontroller CloseEverything(); while(!CheckFlashFlag()){//It then waits for the flash PTP = PTP & (BIT2LO & BIT0LO); EnablePID(); side = GetOutOfStart(); //Once the flash is detected, the robot turns left //until it either detects the ball dispenser beacon or the wall. If it detects //the wall it goes the other way. Once it detects the beacon, it looks for the //opponents goal 3 to determine which side of the field we are on if (side==RIGHT){//we're on the right side of the field DriveUntilLeftDoubleBlack(); //Drive to the side of the dispenser beacon until //we cross the black tape twice RotateLeftUntilBlackTape();//acquire with the tape FollowTapeToDispenser();//follow tape until the bump sensor detects the ball dispensor PickUpNBalls(4);//Get 5 (4+1) balls by driving forward and backward to depress //and release the button ReacquireDispenser(); //realign oursevles with the dispenser using the tape FollowTapeToDispenser(); PickUpNBalls(4);//Get 5 more balls BackUpToGotoGoalThree(side);// back up a certain amount RotateRightToGoalThree();// turn until we see our goal 3 DriveToGoalThree();// follow goal 3 beacon until we sense the goal with our bump sensor SecondDispenserRunRight();//Go back and get 10 more balls EnableSorter();//this time put 5 yellow balls in the goal 2 shooter RotateRightUntilBlackTape(); FollowTapeToDispenser(); PickUpNBalls(4); ReacquireDispenser(); FollowTapeToDispenser(); PickUpNBalls(4); BackUpToGotoGoalThree(side); RotateRightToGoalThree(); DriveToGoalThree(); FinalShotRight();//back up and turn until we see goal 2 in the goal 2 beacon detector, //drive until we see red tape, release the balls. } if (side ==LEFT){//we're on the left side of the field DriveUntilRightDoubleBlack();//Drive to the side of the dispenser beacon until //we cross the black tape twice RotateRightUntilBlackTape();//acquire with the tape FollowTapeToDispenser();//follow tape until the bump sensor detects the ball dispensor PickUpNBalls(4);//Get 5 (4+1) balls by driving forward and backward to depress //and release the button ReacquireDispenser();//realign oursevles with the dispenser using the tape FollowTapeToDispenser(); PickUpNBalls(4);//Get 5 more balls BackUpToGotoGoalThree(side);// back up a certain amount RotateLeftToGoalThree();// turn until we see our goal 3 DriveToGoalThree();// follow goal 3 beacon until we sense the goal with our bump sensor SecondDispenserRunLeft(); //Go back and get 10 more balls EnableSorter(); //this time put 5 yellow balls in the goal 2 shooter RotateLeftUntilBlackTape(); FollowTapeToDispenser(); PickUpNBalls(4); ReacquireDispenser(); FollowTapeToDispenser(); PickUpNBalls(4); BackUpToGotoGoalThree(side); RotateLeftToGoalThree(); DriveToGoalThree(); FinalShotLeft();//back up and turn until we see goal 2 in the goal 2 beacon detector, //drive until we see red tape, release the balls. } while(1) { BlinkDebugLEDs(3);//the fat lady has sung } } /* * Module: tape.h * -------------- * Low level module for handling inputs from sensors. * Not just tape sensors, but also ball sensors and bump * sensors. * * Readings from tape sensors. * Uses ports AD 0-3 on E128. * * Readings from button bump sensors. * Uses ports PS 2-3 on E128. * * Readings from ball sensors. * Uses ports AD 4-5 on E128. */ #ifndef _TAPE_h_ #define _TAPE_h_ /* * Function: Initialize_Tape * -------------------- * Function to initialize tape module * -A/D port */ void Initialize_Tape(void); /* Function: TSensor_1 * ------------------------- * Function returns value of tape sensor */ short TSensor_1(void); /* Function: TSensor_2 * ------------------------- * Function returns value of tape sensor */ short TSensor_2(void); /* Function: TSensor_3 * ------------------------- * Function returns value of tape sensor */ short TSensor_3(void); /* Function: TSensor_4 * ------------------------- * Function returns value of tape sensor */ short TSensor_4(void); /* * Function: MidRightValue * ----------------------- * Function returns AD value of midright tape sensor. */ short MidRightValue(void); /* * Function: MidLeftValue * ----------------------- * Function returns AD value of midleft tape sensor. */ short MidLeftValue(void); /********************************************* ************ BLACK TAPE ******************* ********************************************/ /* * Function: RightBlack * -------------------- * Returns 1 if right sensor shows black, 0 otherwise. */ char RightBlack(void); /* * Function: LeftBlack * ------------------- * Returns 1 if left sensor shows black, 0 otherwise. */ char LeftBlack(void); /* * Function: MidRightBlack * ----------------------- * Returns 1 if midright sensor shows black, 0 otherwise. */ char MidRightBlack(void); /* * Function: MidLeftBlack * ---------------------- * Returns 1 if midleft sensor shows black, 0 otherwise. */ char MidLeftBlack(void); /* * Function: AnyBlack * ------------------ * Returns 1 if any sensor shows black, 0 otherwise. */ char AnyBlack(void); /********************************************* ************ GREEN TAPE ******************* ********************************************/ /* * Function: RightGreen * -------------------- * Returns 1 if right sensor shows green, 0 otherwise. */ char RightGreen(void); /* * Function: LeftGreen * ------------------- * Returns 1 if left sensor shows green, 0 otherwise. */ char LeftGreen(void); /* * Function: MidRightGreen * ----------------------- * Returns 1 if midright sensor shows green, 0 otherwise. */ char MidRightGreen(void); /* * Function: MidLeftGreen * ---------------------- * Returns 1 if midleft sensor shows green, 0 otherwise. */ char MidLeftGreen(void); /* * Function: AnyGreen * ------------------ * Returns 1 if any sensor shows green, 0 otherwise. */ char AnyGreen(void); //------------------------------------------------------------------------- /* * Function: InitializeButtons * --------------------------- * Function to initialize port P for button inputs. */ void InitializeButtons(void); /********************************************** ************** BUMP SENSORS **************** **********************************************/ /* * Function: FrontHit * ------------------ * Returns 1 if front button is pressed, 0 otherwise. */ char FrontHit(void); /* * Function: Goal3Hit * ------------------ * Returns 1 if goal 3 button is pressed, 0 otherwise. */ char Goal3Hit(void); /********************************************** ************** BALL SENSORS **************** **********************************************/ /* * Function: BallSensor1 * --------------------- * Returns ball sensor AD value. */ short BallSensor1(void); /* * Function: BallSensor2 * --------------------- * Returns ball sensor AD value. */ short BallSensor2(void); /* * Function: BallDetectPosition * ---------------------------- * Returns 1 if sorter is in a position to detect ball color, * i.e. the slot is over the phototransistor, so that IR signals * can go through the slot if no ball is present. Returns 0 * otherwise. */ char BallDetectPosition(void); /* * Function: BlackBall * ---------------------------- * Returns 1 if a black ball is present in the sorter, 0 * otherwise. */ char BlackBall(void); /* * Function: YellowBall * ---------------------------- * Returns 1 if a yellow ball is present in the sorter, 0 * otherwise. */ char YellowBall(void); #endif #include "ME218_E128.h" #include "ADS12.h" #include "S12eVec.h" #include "tape.h" #include "termio.h" #include "stdio.h" #define BLACK_THRESHOLD 400 #define GREEN_THRESHOLD 200 #define BALL_THRESHOLD 150 #define YELLOWBALL_THRESHOLD 400 #define TAPE_MAX 730 /* * Function: Initialize_Tape * -------------------- * Function to initialize tape module * -A/D port */ void Initialize_Tape(void) { /* Initialize AD Port */ if(ADS12_Init("OOAAAAAA") == ADS12_OK) { } else { /* Error */ } } /* Function: TSensor_1 * ------------------------- * Function returns value of tape sensor */ short TSensor_1(void) { return ADS12_ReadADPin(0); } /* Function: TSensor_2 * ------------------------- * Function returns value of tape sensor */ short TSensor_2(void) { return ADS12_ReadADPin(1); } /* Function: TSensor_3 * ------------------------- * Function returns value of tape sensor */ short TSensor_3(void) { return ADS12_ReadADPin(2); } /* Function: TSensor_4 * ------------------------- * Function returns value of tape sensor */ short TSensor_4(void) { return ADS12_ReadADPin(3); } /* * Function: MidRightValue * ----------------------- * Function returns AD value of midright tape sensor. */ short MidRightValue(void) { return TSensor_4(); } /* * Function: MidLeftValue * ----------------------- * Function returns AD value of midleft tape sensor. */ short MidLeftValue(void) { return TSensor_2(); } /********************************************* ************ BLACK TAPE ******************* ********************************************/ /* * Function: RightBlack * -------------------- * Returns 1 if right sensor shows black, 0 otherwise. */ char RightBlack(void) { if(TSensor_3() > BLACK_THRESHOLD) return 1; return 0; } /* * Function: LeftBlack * ------------------- * Returns 1 if left sensor shows black, 0 otherwise. */ char LeftBlack(void) { if(TSensor_1() > BLACK_THRESHOLD) return 1; return 0; } /* * Function: MidRightBlack * ----------------------- * Returns 1 if midright sensor shows black, 0 otherwise. */ char MidRightBlack(void) { if(TSensor_4() > BLACK_THRESHOLD) return 1; return 0; } /* * Function: MidLeftBlack * ---------------------- * Returns 1 if midleft sensor shows black, 0 otherwise. */ char MidLeftBlack(void) { if(TSensor_2() > BLACK_THRESHOLD) return 1; return 0; } /* * Function: AnyBlack * ------------------ * Returns 1 if any sensor shows black, 0 otherwise. */ char AnyBlack(void) { if (RightBlack() || LeftBlack() || MidRightBlack() || MidLeftBlack()) return 1; return 0; } /********************************************* ************ GREEN TAPE ******************* ********************************************/ /* * Function: RightGreen * -------------------- * Returns 1 if right sensor shows green, 0 otherwise. */ char RightGreen(void) { if(TSensor_3() < GREEN_THRESHOLD) return 1; return 0; } /* * Function: LeftGreen * ------------------- * Returns 1 if left sensor shows green, 0 otherwise. */ char LeftGreen(void) { if(TSensor_1() < GREEN_THRESHOLD) return 1; return 0; } /* * Function: MidRightGreen * ----------------------- * Returns 1 if midright sensor shows green, 0 otherwise. */ char MidRightGreen(void) { if(TSensor_4() < GREEN_THRESHOLD) return 1; return 0; } /* * Function: MidLeftGreen * ---------------------- * Returns 1 if midleft sensor shows green, 0 otherwise. */ char MidLeftGreen(void) { if(TSensor_2() < GREEN_THRESHOLD) return 1; return 0; } /* * Function: AnyGreen * ------------------ * Returns 1 if any sensor shows green, 0 otherwise. */ char AnyGreen(void) { if (RightGreen() || LeftGreen() || MidRightGreen() || MidLeftGreen()) return 1; return 0; } /* * Function: InitializeButtons * --------------------------- * Function to initialize port S for button inputs. */ void InitializeButtons(void){ DDRS = DDRS & (BIT2LO & BIT3LO); } /* * Function: FrontHit * ------------------ * Returns 1 if front button is pressed, 0 otherwise. */ char FrontHit(void){ if (PTS & BIT2HI){ return 1; } return 0; } /* * Function: Goal3Hit * ------------------ * Returns 1 if goal 3 button is pressed, 0 otherwise. */ char Goal3Hit(void){ if (PTS & BIT3HI){ return 1; } return 0; } /********************************************** ************** BALL SENSORS **************** **********************************************/ /* * Function: BallSensor1 * --------------------- * Returns ball sensor AD value. */ short BallSensor1(void) { return ADS12_ReadADPin(4); } /* * Function: BallSensor2 * --------------------- * Returns ball sensor AD value. */ short BallSensor2(void) { return ADS12_ReadADPin(5); } /* * Function: BallDetectPosition * ---------------------------- * Returns 1 if sorter is in a position to detect ball color, * i.e. the slot is over the phototransistor, so that IR signals * can go through the slot if no ball is present. Returns 0 * otherwise. */ char BallDetectPosition(void) { if(BallSensor2() > BALL_THRESHOLD) return 1; return 0; } /* * Function: BlackBall * ---------------------------- * Returns 1 if a black ball is present in the sorter, 0 * otherwise. */ char BlackBall(void) { if(BallSensor2() < BALL_THRESHOLD) return 1; return 0; } /* * Function: YellowBall * ---------------------------- * Returns 1 if a yellow ball is present in the sorter, 0 * otherwise. */ char YellowBall(void) { if(BallSensor1() > YELLOWBALL_THRESHOLD) return 1; return 0; } #ifndef _IR_H #define _IR_H /* This is the interface to IR.c module * PIN Connections: IRLeft to T1 of E128 * IRRight to T2 of E128 * IRSelect to T3 of E128 * (See Electrical Hadware design) * * TIM0 is used for this module and it is initialized at 375 ticks * per MS * * TIM0_TC5,6 are used as input capture * TIM0_TC4 is used as output compare * The duty cycle is updated every 5 MS * * Five principals functions are available: * - IRInit: Init ports, timers * - SelectFrontEyes: Select the IR Detector pair at the front of the 'bot * - SelectSideEyes: Select the IR Detector pair at the shooter position * - IRLeftDuty: report the left duty cycle of the selected IR pair * - IRRightDuty: report the right duty cycle of the selected IR pair * */ /*********************************************************** * Function IRInit * * Takes no parameter and returns nothing * This function initializes timer TIM0 at 375 ticks a MS * and initilize TIM0_TC5 and TIM0_TC6 as input capture * ***********************************************************/ void IRInit(void); /*********************************************************** * Function SelectFrontEyes * * Takes no parameter and returns nothing * This function send a 0 to the A/B select of the MUX to select the signal from the front IR Detectors * to be treated by the micro controler. (see Electrical Hardware design) ************************************************************/ void SelectFrontEyes(void); /*********************************************************** * Function SelectSideEyes * * Takes no parameter and returns nothing * This function send a 1 to the A/B select of the MUX to select the signal from the side IR Detectors * to be treated by the micro controler. (see Electrical Hardware design) */ void SelectSideEyes(void); /*********************************************************** * Function IRLeftDuty * * Takes no parameter and returns * a number between 0 and 100 if it detects a signal * -1 if it receives no signal * ***********************************************************/ char IRLeftDuty(void); /*********************************************************** * Function IRRightDuty * * Takes no parameter and returns * a number between 0 and 100 if it detects a signal * -1 if it receives no signal * ***********************************************************/ char IRRightDuty(void); /*********************************************************** * Functions CheckFlagx() and ClearFlagx() * * We take advantage of the timer to raise some timer flag. These function is to be used * mainly for debugging. ***********************************************************/ char CheckFlag10MS(void); char CheckFlag50MS(void); char CheckFlag100MS(void); char CheckFlag1S(void); void ClearFlag10MS(void); void ClearFlag50MS(void); void ClearFlag100MS(void); void ClearFlag1S(void); #endif #include /* common defines and macros */ #include /* derivative information */ #include /* E128 bit definitions */ #include /* BIT0HI, BIT0LO....definitions */ int kbhit(void); #define disable() asm sei ; #define enable() asm cli ; #include #include "S12eVec.h" #include "IR.h" #include #include "Sorter.h" #define MS *375 #define _NO_SIGNAL (101) static unsigned int uPeriod1; static unsigned int LastRisingEdge1,LastFallingEdge1,HiTime1; static unsigned int uPeriod2; static unsigned int LastRisingEdge2,LastFallingEdge2,HiTime2; static unsigned int myCounter=0; static char Flag10MS=0,Flag50MS=0,Flag100MS=0,Flag1S,Flag5MS; static char duty1,duty2; static char IR1Flag=0,IR2Flag=0; void SelectFrontEyes(void){ PTT &=BIT3LO; // Pull the A/B pin of the mux low and thus select signal from A channels (Front IR Detectors) } void SelectSideEyes(void){ PTT |=BIT3HI; } char IRLeftDuty(void){ if ((duty1>100) || (duty1<0)) return -1; return 100-duty1; // ...because the circuit is designed to be active LOW } char IRRightDuty(void){ if ((duty2>100) || (duty2<0)) return -1; return 100-duty2; // The circuit is designed to be active LOW } void IRInit(void) { TIM0_TSCR1 = _S12_TEN; /* turn the timer system on */ TIM0_TSCR2 = _S12_PR1 | _S12_PR2; /* set pre-scale to /64 =375 timer clk */ /* Set up ICO04 (T0) to time the printf, duty cycle (motor speed) updates */ // OUTPUT COMPARE PART TIM0_TIOS = _S12_IOS4; /* set IOC04 to output compare rest are inputs */ TIM0_TCTL1 = TIM0_TCTL1 & ~(_S12_OL2 | _S12_OM2); /* no pin connected for OC2*/ TIM0_TC4 = TIM0_TCNT + 5 MS ; /* schedule first event */ TIM0_TFLG1 = _S12_C4F; /* clear OC2 flag */ TIM0_TIE |= _S12_C4I; /* enable OC2 interrupt */ /* Set up IOC05 to capture edges. */ TIM0_TCTL3 |= (_S12_EDG5B ); //capture falling edge TIM0_TCTL3 &= ~(_S12_EDG5A ); TIM0_TFLG1 = _S12_C5F; /* clear flag */ TIM0_TIE |= _S12_C5I; /* enable interrupt */ EnableInterrupts; //Set up IOC06 to capture edges. TIM0_TCTL3 |= (_S12_EDG6B ); //capture falling edge TIM0_TCTL3 &= ~(_S12_EDG6A ); TIM0_TFLG1 = _S12_C6F; // clear flag TIM0_TIE |= _S12_C6I; // enable interrupt DDRT |=BIT3HI; PTT &= BIT3LO; } /* INTERRUPT ROUTINES */ void interrupt _Vec_tim0ch4 Update (void) { TIM0_TFLG1 = _S12_C4F; /* clear OC2 flag */ EnableInterrupts; if ((IR1Flag==0)) //if nothing happens in the last 5MS duty1=(_NO_SIGNAL); //return -1 else duty1=(long)(100*HiTime1/uPeriod1); //else update the duty cycle if ((IR2Flag==0)) //if nothing happens in the last 5MS duty2=(_NO_SIGNAL);//return -1 else duty2=(long)(100*HiTime2/uPeriod2); //else update the duty cycle Flag5MS=1; //set a timer flag if ((myCounter%2)==0) Flag10MS=1; //every 2 executions raise a 10MS flag if ((myCounter%10)==0) Flag50MS=1; if ((myCounter%20)==0) Flag100MS=1; if ((myCounter%200)==0) Flag1S=1; //every 200 executions raise a 1S flag TIM0_TC4+=5 MS; //program next duty update IR1Flag=0; //Clear the event flag IR2Flag=0; //Clear the event flag SorterRun(); //Run the Ball Sorter } void interrupt _Vec_tim0ch5 SignalTimer5 (void) { TIM0_TFLG1 = _S12_C5F; // clear flag IR1Flag=1; //raise an event flag if (((TIM0_TC5-LastRisingEdge1>(1 MS/15)) && ((TIM0_TC5-LastFallingEdge1)>(1 MS /15)))) //only update if the edge detected is not too near the last edge (debouncing by program) { if ((TIM0_TCTL3 &_S12_EDG5B )==_S12_EDG5B ) // if currently looking for falling edge { LastFallingEdge1 = TIM0_TC5; //record this falling edge HiTime1=LastFallingEdge1-LastRisingEdge1; //compute the hitime } else if ((TIM0_TCTL3 &_S12_EDG5A)==_S12_EDG5A) // if currently looking for rising edge { uPeriod1=TIM0_TC5-LastRisingEdge1;//record this rising edge LastRisingEdge1 = TIM0_TC5;//compute the period } } TIM0_TCTL3^=(_S12_EDG5A | _S12_EDG5B); //if this time capture falling edge then next time wil capture rising edge and vice visa } void interrupt _Vec_tim0ch6 SignalTimer6 (void) /*Exactly same implementation as the above function */ { TIM0_TFLG1 = _S12_C6F; // clear flag IR2Flag=1; if (((TIM0_TC6-LastRisingEdge2>(1 MS/15)) && ((TIM0_TC6-LastFallingEdge2)>(1 MS /15)))) { if ((TIM0_TCTL3 & _S12_EDG6B )==_S12_EDG6B ) { LastFallingEdge2 = TIM0_TC6; HiTime2=LastFallingEdge2-LastRisingEdge2; } else if ((TIM0_TCTL3 &_S12_EDG6A)==_S12_EDG6A ) { uPeriod2=TIM0_TC6-LastRisingEdge2; LastRisingEdge2 = TIM0_TC6; } } TIM0_TCTL3^=(_S12_EDG6A | _S12_EDG6B); } char CheckFlag10MS(void){ return Flag10MS; }char CheckFlag50MS(void){ return Flag50MS; }char CheckFlag100MS(void){ return Flag100MS; } char CheckFlag1S(void){ return Flag1S; } void ClearFlag10MS(void){ Flag10MS=0; } void ClearFlag50MS(void){ Flag50MS=0; } void ClearFlag100MS(void){ Flag100MS=0; } void ClearFlag1S(void){ Flag1S=0; } /* Module: motor.h * --------------- * Module for controlling robot motors. Motors are controlled * using Drive-Braks Mode, with PWM frequency of * 20 KHz. Motors are driven using L293B H-Bridge, pins T0 and * T1 are PWM enabled and connect to one H-Bridge input. Pins * M0 and M1 are set to output and connected to other H-Bridge * input. */ #ifndef _MOTOR_h_ #define _MOTOR_h_ #define SA_PRESCALE 3 #define PWM_PERIOD 200 #define _BACKWARD 1 #define _FORWARD -1 /* * Function: Initialize_Motor * -------------------------- * Function to initialize the motor. Handles the PWM * initialization steps. Sets pins M0 and M1 to outputs. */ void Initialize_Motor(void); /* * Function: DisableMotor * ---------------------- * Shuts down motor operation. */ void DisableMotor(void); /* * Function: PWM_SetDuty * --------------------- * Sets the duty cycle of PWM. * Takes in number between 0 and 100 to be effective * Only works for channel 0 and 1 currently. */ void PWM_SetDuty(char channel, unsigned char duty); void LeftMotorEnc(int speedIn, char dirInLeft); /* * Function: PIDLeftMotor * ------------------- * */ void PIDLeftMotor(char requestedDuty); /* * Function: LeftMotor * ------------------- * Runs the left motor at the specified duty cycle. */ void LeftMotor(char duty); /* * Function: RightMotor * ------------------- * Runs the right motor at the specified duty cycle. */ void RightMotorEnc(int speedIn, char dirInLeft); /* * Function: PIDLeftMotor * ------------------- * */ void PIDRightMotor(char requestedDuty); void RightMotor(char duty); /* * Function: GetLeftDuty * --------------------- * Returns the current duty cycle for left motor. */ char GetLeftDuty(void); /* * Function: GetRightDuty * ---------------------- * Returns the current duty cycle for right motor. */ char GetRightDuty(void); /* * Function MoveVehicle * -------------------- * Function to command the motors to drive the robot, causing * it to move. Takes a velocity value between -100 and 100, and * a turnrate between -100 and 100. Conversion to duty cycles * is done by the function. */ void MoveVehicle(char velocity, char turnrate); /* Function FastStop * ----------------- * Function to make the robot motors stop immediately. */ void FastStop(void); /* * Function: OpenHopper * -------------------- * Function to open the hopper and drop balls into goal three. */ void OpenHopper(void); /* * Function: CloseHopper * --------------------- * Function to close the hopper. */ void CloseHopper(void); /* * Function: OpenShooter * --------------------- * Function to open the shooter ramp and shoot balls. */ void OpenShooter(void); /* * Function: CloseShooter * ---------------------- * Function to close the shooter ramp. */ void CloseShooter(void); /* * Function: SorterCW * ------------------ * Function to cause the sorter to spin clockwise. */ void SorterCW(void); /* * Function: SorterCCW * ------------------- * Function to cause the sorter to spin counter clockwise. */ void SorterCCW(void); /* * Function: SorterStop * -------------------- * Function to cause the sorter to stop spinning. */ void SorterStop(void); #endif #include "ME218_E128.h" #include "actions.h" #include "motor.h" #define SCALE 100/100 char MotorEnable; char RightDuty; char LeftDuty; char dirLeft, dirRight; int speed; /* * Function: PWM_SetDuty * --------------------- * Sets the duty cycle of PWM. * Takes in number between 0 and 100 to be effective * Only works for channel 0 and 1 currently. */ void PWM_SetDuty(char channel, unsigned char duty) { duty = (char) (int) (duty * SCALE); if(channel == 0) { PWMDTY0 = (duty*PWM_PERIOD)/100; } else if(channel == 1) { PWMDTY1 = (duty*PWM_PERIOD)/100; } } /* * Function: PWM_Init * ------------------ * Function to initialize PWM on microcontroller - 2 channels * -choose 8 bit mode * -left aligned * -high polarity * -clock SA with S prescale and SA prescale * -enable PWM for line U0 and U1 * -set PWM period */ void PWM_Init(void) { // Use 8 bit mode PWMCTL = 0x00; // Use Left aligned mode PWMCAE = 0x00; // Use High polarity only PWMPOL = 0xFF; // Choose Clock SA PWMCLK = PWMCLK | (_S12_PCLK0 | _S12_PCLK1); // Clock prescale for A no division PWMPRCLK = PWMPRCLK & ((~_S12_PCKA0) & (~_S12_PCKA1) & (~_S12_PCKA2)); // Clock prescale for clock SA PWMSCLA = SA_PRESCALE; // PWM enable for T0 and T1 PWME = PWME | (_S12_PWME0 | _S12_PWME1); // Set period PWMPER0 = PWM_PERIOD; PWMPER1 = PWM_PERIOD; // Set starting duty to 0, motors not running PWM_SetDuty(0, 0); PWM_SetDuty(1, 0); // Don't know what it does but you need it for PWM MODRR = MODRR | (_S12_MODRR0 | _S12_MODRR1); } /* * Function: Initialize_Motor * -------------------------- * Function to initialize the motor. Handles the PWM * initialization steps. Sets pins U7 and U6 to outputs. */ void Initialize_Motor(void) { PWM_Init(); DDRU = DDRU | (BIT7HI | BIT6HI); PTU = PTU & (BIT7LO & BIT6LO); /* Auxiliary motor control */ DDRU = DDRU | (BIT2HI | BIT3HI | BIT4HI | BIT5HI); PTU = PTU & (BIT2LO & BIT3LO & BIT4LO & BIT5LO); PTAD = PTAD & (BIT7LO & BIT6LO); RightDuty = 0; LeftDuty = 0; MotorEnable = 1; } /* * Function: DisableMotor * ---------------------- * Shuts down motor operation. */ void DisableMotor(void) { FastStop(); SorterStop(); MotorEnable = 0; } /* * Function: LeftMotorEnc * ---------------------- * */ void LeftMotorEnc(int speedIn, char dirInLeft) { SetRPMLeft(speedIn); dirLeft = dirInLeft; } /* * Function: PIDLeftMotor * ---------------------- * */ void PIDLeftMotor(char requestedDuty) { LeftDuty = requestedDuty; if(!MotorEnable) return; if(dirLeft == _FORWARD) { PTU = PTU | BIT7HI; requestedDuty = 100 - requestedDuty; } else { PTU = PTU & BIT7LO; if(requestedDuty > 100) requestedDuty = 100; } PWM_SetDuty(1, requestedDuty); } /* * Function: LeftMotor * ------------------- * Runs the left motor at the specified duty cycle. */ void LeftMotor(char duty) { if(!MotorEnable) return; LeftDuty = duty; duty = -duty; if(duty < 0) { PTU = PTU | BIT7HI; duty = 100 + duty; if(duty < 0) duty = 0; } else { PTU = PTU & BIT7LO; if(duty > 100) duty = 100; } PWM_SetDuty(1, duty); } /* * Function: LeftMotorEnc * ---------------------- * */ void RightMotorEnc(int speedIn, char dirInRight) { SetRPMRight(speedIn); dirRight = dirInRight; } /* * Function: PIDLeftMotor * ---------------------- * */ void PIDRightMotor(char requestedDuty) { RightDuty = requestedDuty; if(!MotorEnable) return; if(dirRight == _BACKWARD) { PTU = PTU | BIT6HI; requestedDuty = 100 - requestedDuty; } else { PTU = PTU & BIT6LO; if(requestedDuty > 100) requestedDuty = 100; } PWM_SetDuty(0, requestedDuty); } /* * Function: RightMotor * ------------------- * Runs the right motor at the specified duty cycle. */ void RightMotor(char duty) { if(!MotorEnable) return; RightDuty = duty; if(duty < 0) { PTU = PTU | BIT6HI; duty = 100 + duty; if(duty < 0) duty = 0; } else { PTU = PTU & BIT6LO; if(duty > 100) duty = 100; } PWM_SetDuty(0, duty); } /* * Function: GetLeftDuty * --------------------- * Returns the current duty cycle for left motor. */ char GetLeftDuty(void) { return LeftDuty; } /* * Function: GetRightDuty * ---------------------- * Returns the current duty cycle for right motor. */ char GetRightDuty(void) { return RightDuty; } /* * Function MoveVehicle * -------------------- * Function to command the motors to drive the robot, causing * it to move. Takes a velocity value between -100 and 100, and * a turnrate between -100 and 100. Conversion to duty cycles * is done by the function. */ void MoveVehicle(char velocity, char turnrate) { if(!MotorEnable) return; LeftMotor(velocity + turnrate); RightMotor(velocity - turnrate); } /* Function FastStop * ----------------- * Function to make the robot motors stop immediately. */ void FastStop(void) { LeftMotor(0); RightMotor(0); } /* * Function: OpenHopper * -------------------- * Function to open the hopper and drop balls into goal three. */ void OpenHopper(void){ if(!MotorEnable) return; PTU = PTU | BIT2HI; PTU = PTU & BIT3LO; WaitForTime(400); PTU = PTU & BIT2LO; } /* * Function: CloseHopper * --------------------- * Function to close the hopper. */ void CloseHopper(void){ if(!MotorEnable) return; PTU = PTU | BIT3HI; PTU = PTU & BIT2LO; WaitForTime(400); PTU = PTU & BIT3LO; } /* * Function: SorterCCW * ------------------- * Function to cause the sorter to spin counter clockwise. */ void SorterCCW(void){ if(!MotorEnable) return; PTU = PTU | BIT4HI; PTU = PTU & BIT5LO; } /* * Function: SorterCW * ------------------ * Function to cause the sorter to spin clockwise. */ void SorterCW(void){ if(!MotorEnable) return; PTU = PTU | BIT5HI; PTU = PTU & BIT4LO; } /* * Function: SorterStop * -------------------- * Function to cause the sorter to stop spinning. */ void SorterStop(void) { PTU = PTU & BIT5LO; PTU = PTU & BIT4LO; } /* * Function: OpenShooter * --------------------- * Function to open the shooter ramp and shoot balls. */ void OpenShooter(void){ if(!MotorEnable) return; PTAD = PTAD | BIT7HI; PTAD = PTAD & BIT6LO; WaitForTime(100); PTAD = PTAD & BIT7LO; } /* * Function: CloseShooter * ---------------------- * Function to close the shooter ramp. */ void CloseShooter(void){ if(!MotorEnable) return; PTAD = PTAD | BIT6HI; PTAD = PTAD & BIT7LO; WaitForTime(100); PTAD = PTAD & BIT6LO; } /* This is the interface to Encoder.c module * PIN Connections: Left Encoder to T5 of E128 * Right Encoder to T6 of E128 * Flash Detector to T4 of E128 * (See Electrical Hadware design) * Main purposes of this module * - Measure mileage of left and right wheel * - Report RPM * - PID control the speed of the robot when PIDMode is enable * * Flash detector is implemented with Timer System 1, hence Flash Detection and * Robot stopping after 2 minutes also also handled in this module * * */ /*********************************************************** * Function EncoderInit * * Takes no parameter and returns nothing. It initializes the module (including Encoders and Flash) * ***********************************************************/ void EncoderInit(void); /*********************************************************** * Function MileageLeft * * Takes no parameter and returns the mileage of the left wheel (initialized at the beginning * of the game by EncoderInit). The encoder is made so that 180 units = 1 round * ***********************************************************/ unsigned long MileageLeft(void); /*********************************************************** * Function MileageRight * * Takes no parameter and returns the mileage of the right wheel (initialized at the beginning * of the game by EncoderInit). The encoder is made so that 180 units = 1 round * ***********************************************************/ unsigned long MileageRight(void); /*********************************************************** * Function SetRPMLeft * * Takes no parameter and returns nothing. It tell the module the disired speed of the left wheel. * The module is implemented so that the speed of the wheel approach this desired speed bit by bit. * This method help prevent the robot from jumping at start and sliding at stop. * ***********************************************************/ void SetRPMLeft(int finalRPMLeft); /*********************************************************** * Function SetRPMRight * * Takes no parameter and returns nothing. It tell the module the disired speed of the left wheel. * The module is implemented so that the speed of the wheel approach this desired speed bit by bit. * This method help prevent the robot from jumping at start and sliding at stop. * ***********************************************************/ void SetRPMRight(int finalRPMRight); /*********************************************************** * Function GetRPMLeft * * Takes no parameter and returns the current speed of the left wheel (an unsigned int) * ***********************************************************/ unsigned int GetRPMLeft(void); /*********************************************************** * Function GetRPMRight * * Takes no parameter and returns the current speed of the right wheel (an unsigned int) * ***********************************************************/ unsigned int GetRPMRight(void); /*********************************************************** * Function EnablePID * * Takes no parameter and returns nothing. It enable the * closed loop control of speed. * * The module is implemented so that every 20 MS an interupt routine check to see if the PIDMode is enabled, if it is, the interupt * routine will adjust PWM duty cyle to achieve desired speed. (by PID of course) * ***********************************************************/ void EnablePID(void); /*********************************************************** * Function DisablePID * * Takes no parameter and returns nothing. It disable the * closed loop control of speed. * */ void DisablePID(void); /*********************************************************** * Function SetRampRate * * Takes no parameter and returns nothing. * The robot is programmed to avoid sudden start or stop (therefore prevent jumping and sliding) * The control signal in PID controller is not a step each time a desired RPM is set by SetRPMLeft/Right * but a ramp from current RPM to the desired RPM. The ramprate correspond determine the slope of this ramp (unit per 20MS) */ void SetRampRate(int rampRateIn); /*********************************************************** * Function CheckFlashFlag * * Takes no parameter and returns 0 or 1 indicating whether a flash is detected */ char CheckFlashFlag(void); /*********************************************************** * Function ClearFlashFlag * * Takes no parameter and returns nothing. It clear the Flash Flag */ void ClearFlashFlag(void); #include /* common defines and macros */ #include /* derivative information */ #include /* E128 bit definitions */ #include /* BIT0HI, BIT0LO....definitions */ int kbhit(void); /* functions to enable & disable interupts for CW 11/14/01 */ #define disable() asm sei ; #define enable() asm cli ; #include #include "S12eVec.h" #include "Encoder.h" #include "Motor.h" #include #define MS *3000 #define PER2RPM 1000000 static unsigned int Period1,Period2,LastEdge1,LastEdge2; static int RPMLeft=0,RPMRight=0,finalRPM1=0,finalRPM2=0; static int RPMError1=0,RPMError2=0,SumError1=0,SumError2=0,RequestedDuty1=0,RequestedDuty2=0; static int rampRPM1 = 0, rampRPM2 = 0; static char Flag1=0,Flag2=0,Flag3=0; static char FlashFlag=0; static unsigned long Mileage1=0,Mileage2=0; static char PIDEN=0; static int rampRate = 10; void EnablePID(void){ PIDEN=1; } void DisablePID(void){ PIDEN=0; } unsigned long MileageLeft(void){ return Mileage1; } unsigned long MileageRight(void){ return Mileage2; } void SetRampRate(int rampRateIn){ rampRate = rampRateIn; } void SetRPMLeft(int RPM){ finalRPM1 = RPM; } void SetRPMRight(int RPM){ finalRPM2 = RPM; } unsigned int GetRPMLeft(void){ return RPMLeft; } unsigned int GetRPMRight(void){ return RPMRight; } void EncoderInit(void) { TIM1_TSCR1 = _S12_TEN; /* turn the timer system on */ TIM1_TSCR2 = _S12_PR1 | _S12_PR0; /* set pre-scale to /8 */ /* Set up ICO07 (T7) to time the duty cycle (motor speed) updates */ // OUTPUT COMPARE PART TIM1_TIOS = _S12_IOS7; // set IOC07 to output compare rest are inputs / TIM1_TCTL1 = TIM1_TCTL1 & ~(_S12_OL2 | _S12_OM2); //o pin connected for OC2/ TIM1_TC7 = TIM1_TCNT + 5 MS ; //schedule first event/ TIM1_TFLG1 = _S12_C7F; // clear OC2 flag / TIM1_TIE |= _S12_C7I; // enable OC2 interrupt / / // Set up IO C05 to capture edges. (Left Encoder) TIM1_TCTL3 |= (_S12_EDG5B ); //capture falling edge TIM1_TCTL3 &= ~(_S12_EDG5A ); TIM1_TFLG1 = _S12_C5F; // clear flag / TIM1_TIE |= _S12_C5I; // enable interrupt/ EnableInterrupts; //Set up IOC06 to capture edges. (Right Encoder) TIM1_TCTL3 |= (_S12_EDG6B ); //capture falling edge TIM1_TCTL3 &= ~(_S12_EDG6A ); TIM1_TFLG1 = _S12_C6F; // clear flag TIM1_TIE |= _S12_C6I; // enable interrupt //Set up IOC04 to capture edges. Flash Detector TIM1_TCTL3 |= (_S12_EDG4A ); //capture rising edge TIM1_TCTL3 &= ~(_S12_EDG4B ); TIM1_TFLG1 = _S12_C4F; // clear flag TIM1_TIE |= _S12_C4I; // enable interrupt DDRP = DDRP | BIT5HI; } void interrupt _Vec_tim1ch4 FlashFlagInterupt (void) { TIM1_TFLG1 = _S12_C4F; // clear flag FlashFlag=1; } void interrupt _Vec_tim1ch5 EncoderLeft (void) { TIM1_TFLG1 = _S12_C5F; // clear flag EnableInterrupts; //allow other ticks or IR beacons to detect flags Flag1=1; //Throw flag for interttupt routine if ((TIM1_TCNT-LastEdge1)>(00)){ //Another flag seen in more than zero time Mileage1+=1; //another encoder tick has passed Period1=TIM1_TCNT-LastEdge1; //read time period LastEdge1=TIM1_TCNT; //reset last edge } } void interrupt _Vec_tim1ch6 EncoderRight (void) { TIM1_TFLG1 = _S12_C6F; // clear flag Flag2=1; EnableInterrupts; if ((TIM1_TCNT-LastEdge2)>(00)){ Mileage2+=1; Period2=TIM1_TCNT-LastEdge2; LastEdge2=TIM1_TCNT; } } /*********************************************************** * HEART OF THE MODULE (Speed updating and PID Control) * * * *********************************************************** ***********************************************************/ void interrupt _Vec_tim1ch7 EncoderUpdate (void) { static int counter=0; static const int iGain=20,pGain=60; //gain to be tuned by experiments TIM1_TFLG1 = _S12_C7F; // clear flag TIM1_TC7 += 20 MS ; //program next update/control if (counter>=6000) DisableMotor(); //if 2 Mins elasped then stop the main motor if (FlashFlag) counter++; //if game started then count PTP = PTP | BIT5HI; //Debug if (Flag1) { RPMLeft=(long)(PER2RPM/Period1); //compute speed if (RPMLeft>150) RPMLeft=150; //in reality 100 is the max speed ever attained by the motor Flag1=0; }else RPMLeft=0; if (Flag2) { RPMRight=(long)(PER2RPM/Period2); Flag2=0; if (RPMRight>150) RPMRight=150; }else RPMRight=0; if (PIDEN){ //if PIDMode is Enabled /*********************************************************** * CONTROL STRATEGY * * Each execution, the program check to see if the desired speed * and current speed is far away from each other (difference > ramprate) * If they are, then the PID controller set a target speed (call rampRPM below) * to be a certain number between current speed and desired speed. * * This is to avoid the robot from underwent a too brutal change in speed, * Exemple, say at t=0, speed=0 and desiredspeed=100,ramprate=10, * * at the t= 20MS, the controler set a Target speed at 10RPM * at the t= 40MS, the controler set a Target speed at 20RPM * at the t= 60MS, the controler set a Target speed at 30RPM * .... * from t= 200MS, the controler set a Target speed at 100RPM * * The rest of the control algorithm is the same as normal PID control */ if ((finalRPM1 - rampRPM1) > rampRate) rampRPM1 = rampRPM1 + rampRate; else if ((rampRPM1 - finalRPM1) > rampRate) rampRPM1 = rampRPM1 - rampRate; else rampRPM1 = finalRPM1; if ((finalRPM2 - rampRPM2) > rampRate) rampRPM2 = rampRPM2 + rampRate; else if ((rampRPM2 - finalRPM2) > rampRate) rampRPM2 = rampRPM2 - rampRate; else rampRPM2 = finalRPM2; //printf("%i %i %i %i %i %i %i %i\r\n", RPMLeft, rampRPM1, finalRPM1, RPMRight, rampRPM2, finalRPM2); /*********************************************************** Closed Loop Speed Control*/ RPMError1=rampRPM1-RPMLeft; SumError1+=RPMError1; RequestedDuty1= (long)((RPMError1*pGain) + (iGain*SumError1))/100; if (RequestedDuty1>100) { RequestedDuty1=100; SumError1-=RPMError1; /*anti windup */ } else if (RequestedDuty1<0) { RequestedDuty1= 0; SumError1-=RPMError1; /*anti windup */ } if (rampRPM1 < 20){ PIDLeftMotor(0); SumError1 = 0; } else{ PIDLeftMotor((char)RequestedDuty1); } RPMError2=rampRPM2-RPMRight; SumError2+=RPMError2; RequestedDuty2= (long)((RPMError2*pGain) + (iGain*SumError2))/100; if (RequestedDuty2>100) { RequestedDuty2=100; SumError2-=RPMError2; /*anti windup */ } if (RequestedDuty2 < 0) { RequestedDuty2= 0; SumError2-=RPMError2; /*anti windup */ } if (rampRPM2< 20){ SumError2 = 0; PIDRightMotor(0); } else{ PIDRightMotor((char)RequestedDuty2); } //printf(" %i %i %i %i %i %i\t\t", RPMRight, RPMError2, SumError2, rampRPM2, finalRPM2, RequestedDuty2); //printf(" %i %i %i %i %i %i\r\n", RPMLeft, RPMError1, SumError1, rampRPM1, finalRPM1, RequestedDuty1); } /************************************************************/ PTP = PTP & BIT5LO; //Debug (the computation time to be measure by scoping PTP5) } char CheckFlashFlag(void) { return FlashFlag; } void ClearFlashFlag(void) { FlashFlag=0; } #ifndef _SORTER_H #define _SORTER_H /* * This is the interface to module Sorter.c */ /* * Function EnableSorter * takes no parameters and returns nothing * this function set the mode of sorter to be "enable" */ void EnableSorter(void); /* * Function DisableSorter * takes no parameters and returns nothing * this function set the mode of sorter to be "disable" */ void DisableSorter(void); /* * Function Sorter * takes no parameters and returns nothing * Each execution, this function checks if the the sorter mode is Enable, * if it is then the sorter wait for a ball. If the ball is black or the shooter ramp is full * then the sorter turn and let the ball fall into the hopper. Otherwise, the ball is sorted to the shooter. * After sorting succesfully a ball the sorter goes back and wait for the next ball * * Note: This function is NOT blocking code, and to be called frequently in tby the client * (ie. Run SorterRun frequently in main to do ball sorting or make it time interupt driven) */ void SorterRun(void); #endif #include "ME218_E128.h" #include #include "tape.h" #include "motor.h" #include "Sorter.h" #include #define WAITFORBALL 0 #define CW 1 #define CCW 2 #define START 3 #define DEFAULTPOS 4 #define INITIALIZING 5 #define GOBACK 6 static char State=START; static char YellowBallCounter=0; static unsigned int LastRestTime=0; static char SortEN=0; static char goback=0; static char lastState=-1124; void EnableSorter(void){ SortEN=1; } void DisableSorter(void){ SortEN=0; } /* * IMPLEMENTATION OF SORTER * * This function is implemented as a (micro)State Machine with 2 mode * Mode1 (SorterOff: Ball always go to the hopper) * Mode2 (Active Sorting: high point ball to the hopper) * * Mode1: has 3 State: * START: initial state, set at the beginning. * INITIALZING: sorter turning toward the hopper position * DEFAULTPOS: desired position of the sorter (let all balls to sorter) * * State transitions are pretty simple * SorterRun do the following job depending on the current state: * if State=Start then rotate Sorter record Starting Time chang State to INITIALIZING * if State=INITIALIZING then check the time, if CurrentTime-StartingTime>=300MS, then stop and chane State go DEFAULTPOS * if State=DEFAULTPOS then stop the sorter motor and stay there. * * Mode1: has 4 State: * WAITFORBALL: position 0, sorter wait for ball * CW: sorter turning clockwise to sort balls to the shooter * CCW: sorter turning clockwise to sort balls to the shooter * GOBACK: ball sorted and sorter is going back to position 0 * * State transitions would be described better by a table, in words it is the following: * * if State is not the 4 above State the turn CCW and set State to GOBACK * if State=GOBACK and the sorter reaches the position 0 then stop, change State to WAITFORBALL * if State=WAITFORBALL and there's a yellowbal then rotate CW, change state to CW and record time * if State=WAITFORBALL and there's a blackball or shooter full then rotate CCW, change state and record time * if State=CW or CCW and CurrentTime-RecordedTime>=250 (ball fell to the right slot), turn the other way and change state to GOBACK * * This state analyse constitutes also a pseudo code, the actual code in C is the following */ void SorterRun(void){ //module variables static unsigned int SorterInitTime; static unsigned int LastBallTime; if (!SortEN){ if (State==START) { SorterCCW(); State=CCW; } else if (State==CCW && BallSensor2()>400) { SorterStop(); State=WAITFORBALL; SorterInitTime=TMRS12_GetTime(); } else if (State==WAITFORBALL) { SorterCCW(); State=INITIALIZING; } else if (State==INITIALIZING) { if ((TMRS12_GetTime() - SorterInitTime)>=190){ SorterStop(); State=DEFAULTPOS; } } else if (State==DEFAULTPOS) { SorterStop(); } else { SorterCCW(); State=CCW; } } if (SortEN) { if (State==DEFAULTPOS || State==START) { State=GOBACK; SorterCW(); return; } else if (State==WAITFORBALL){ if ((YellowBall()) && (YellowBallCounter<4)){ SorterCW(); LastBallTime=TMRS12_GetTime(); State=CW; YellowBallCounter++; return; } else if (BlackBall()){ SorterCCW(); LastBallTime=TMRS12_GetTime(); State=CCW; return; } else SorterStop(); } else if (((State==CW) || (State==CCW)) && TMRS12_GetTime() - LastBallTime>250){ if (State==CW){ SorterCCW(); State=CCW; State=GOBACK; } else if (State==CCW){ SorterCW(); State=CW; State=GOBACK; } } else if (State==GOBACK && BallDetectPosition()) { SorterStop(); State=WAITFORBALL; } } } /* * Module: actions.h * ----------------- * Actions is a high level module that specifies a list of functions * representing actions that the robot can take. Examples range from * waiting for a specified amount of time to driving forward until one * tape sensor detects black tape twice. Actions can be specific to * the playing field (GetOutOfStart) or general (RotateLeftUntilBeacon). */ #ifndef _ACTIONS_h_ #define _ACTIONS_h_ /* * Function: WaitForTime * --------------------- * Function that waits for an input amount of time. Uses * the Timers12 library. */ void WaitForTime(unsigned int time); /* * Function: WaitAndSort * --------------------- * Function that waits for an input amount of time, and ensures * that the robot is sorting balls at the same time. Uses the * Timers12 library. */ void WaitAndSort(unsigned int time); /* * Function: GetOutOfStart * ----------------------- * Function to cause robot to prepare to leave the starting position * to head towards the ball dispenser, and also determine which side * of the field it is on. * * Robot rotates left. If it hits a wall, determined by the requested * motor duty cycle determined by PID control, then the robot changes * the direction of rotation. Robot continues rotating until the 50% * duty cycle beacon is detected. Once that is detected, the robot * continues rotating in its current direction. Depending on what the * IR sensors detect next, the robot determines which side of the field * it is on. * * If the right IR sensor detects a 90% duty cycle, and the robot is * rotating right, then the robot is on the right side of the field. * If the left IR sensor detects no signal, and the robot is rotating * left, then the robot is on the right side of the field. In this * case, the robot recorrects by rotating right until the right * sensor detects a 90% duty cycle. * The decision process is reversed exactly for the left side of the * field. * * Once the side is determined, the robot stops, and the side is returned * as a char. */ char GetOutOfStart(void); /* * Function: DriveUntilLeftDoubleBlack * ----------------------------------- * Function to cause the robot to drive until it reaches the black * tape line leading to the ball dispenser. This is done by detecting * when a tape sensor finds black tape a second time. This function * assumes that the robot is on the right playing field; consequently, * the left tape sensor is used to detect the black line. * * Robot drives at full speed (100) until the first black tape is * detected. Then the robot drives at 50 until the second black tape * is detected. * * In order to drive in the correct direction, a control algorithm is * implemented as such: the robot attempts to drive on the edge between * the 90% duty cycle and the gap that shows no signal. If left IR * sensor shows nothing, and right shows something, the robot turns * left. If right shows nothing and left shows something, the robot * turns right. If both show nothing the robot turns left, and if both * show something, the robot turns right. The function assumes that * the robot is oriented such that it is facing or almost facing the * 90% duty cycle beacon on the opponent's playing field. */ void DriveUntilLeftDoubleBlack(void); /* * Function: DriveUntilRightDoubleBlack * ----------------------------------- * Function to cause the robot to drive until it reaches the black * tape line leading to the ball dispenser. This is done by detecting * when a tape sensor finds black tape a second time. This function * assumes that the robot is on the left playing field; consequently, * the right tape sensor is used to detect the black line. * * Robot drives at full speed (100) until the first black tape is * detected. Then the robot drives at 50 until the second black tape * is detected. * * In order to drive in the correct direction, a control algorithm is * implemented as such: the robot attempts to drive on the edge between * the 90% duty cycle and the gap that shows no signal. If left IR * sensor shows nothing, and right shows something, the robot turns * left. If right shows nothing and left shows something, the robot * turns right. If both show nothing the robot turns right, and if both * show something, the robot turns left. The function assumes that * the robot is oriented such that it is facing or almost facing the * 90% duty cycle beacon on the opponent's playing field. */ void DriveUntilRightDoubleBlack(void); /* * Function: MoveToBeaconControl * ----------------------------- * Control stage for moving towards a beacon. Assumes robot is * already facing the beacon. Speed is an input. */ void MoveToBeaconControl(char speed); /* * Function: FollowTapeToDispenser * ------------------------------- * Robot follows tape until it hits the dispenser button. Should pick up * a ball and back up to reset the dispenser. Hitting the dispenser is * detected by the activation of the front bumb sensor. */ void FollowTapeToDispenser(void); /* * Function: MoveTowardBallDispenser * --------------------------------- * Robot moves forward until it hits the ball dispenser, then backs * up to release the dispenser button after collecting a ball. */ void MoveTowardBallDispenser(void); /* * Function: PickUpNBalls * ---------------------- * Function to direct robot to collect a number of balls specified * by an input char. */ void PickUpNBalls(char c); /* * Function: ReacquireDispenser * ---------------------------- * Function to direct robot to reacquire the black tape line leading * to the dispenser. Robot backs up a little bit, then rotates right * for a certain amount of time, and then rotates left double that * amount of time. If the robot detects the black tape line, it stops * rotating. The robot then waits until a total of ten seconds have * passed in this function, in order to reset the ball dispenser timing. */ void ReacquireDispenser(void); /* * Function: BackUpToGotoGoalThree * ------------------------------- * Function to cause the robot to back away from the dispenser * in preparation to rotate and acquire goal three. Takes a char * determining the side the robot is on as an input. Depending * on which side, the robot backs up a different amount of time. */ void BackUpToGotoGoalThree(char side); /* * Function: RotateRightToGoalThree * -------------------------------- * Robot rotates right while looking for the 90% duty cycle beacon * that marks goal three. Once it is found, the robot stops. */ void RotateRightToGoalThree(void); /* * Function: RotateLeftToGoalThree * -------------------------------- * Robot rotates left while looking for the 90% duty cycle beacon * that marks goal three. Once it is found, the robot stops. */ void RotateLeftToGoalThree(void); /* * Function: DriveToGoalThree * -------------------------- * Function to direct robot to drive forward until it reaches goal * three. Robot drives forward at a speed of 80 until it reaches * green tape under a control scheme to make sure it is going in the * right direction. At this point, the speed is halved to 40. * * Reaaching goal three is detected either by activation of the goal * three bump sensor, or by detecting that the robot is pushing against * a wall, determined by the requested motor duty cycle by PID control. * * Once the robot reaches goal three, it opens the hopper to dump all * its balls. Robot then drives forward against goal three for a short * time to shake the hopper and cause stuck balls to fall out. Then * the robot closes its hopper. */ void DriveToGoalThree(void); /* * Function: SecondDispenserRunRight * --------------------------------- * Function to direct robot to prepare for a second dispenser * run after depositing its balls in goal three. Assumes robot * starts at goal three and on right playing field. * * Robot goes backwards at full speed for a certain amount of * time. Robot then rotates right until it detects the 30% * beacon marking goal one. The robot then opens its shooter * in order to shoot any balls that may have mistakenly fallen * into the shooter ramp (in order to avoid carrying more than * ten balls at any one time). Robot then moves forward until * it detects black tape. */ void SecondDispenserRunRight(void); /* * Function: SecondDispenserRunLeft * --------------------------------- * Function to direct robot to prepare for a second dispenser * run after depositing its balls in goal three. Assumes robot * starts at goal three and on left playing field. * * Robot goes backwards at full speed for a certain amount of * time. Robot then rotates left until it detects the 30% * beacon marking goal one. The robot then opens its shooter * in order to shoot any balls that may have mistakenly fallen * into the shooter ramp (in order to avoid carrying more than * ten balls at any one time). Robot then moves forward until * it detects black tape. */ void SecondDispenserRunLeft(void); /* * Function: FinalShotRight * ------------------------ * Function to direct robot to take its final shot with the shooter * at goal two. Assumes the robot starts at goal three on the right * side of the field. */ void FinalShotRight(void); /* * Function: FinalShotLeft * ------------------------ * Function to direct robot to take its final shot with the shooter * at goal two. Assumes the robot starts at goal three on the left * side of the field. */ void FinalShotLeft(void); /* * Function: RotateLeftUntilBeacon * -------------------------------- * Robot rotates left at input speed until it detects a beacon * with duty cycle d. */ void RotateLeftUntilBeacon(char d, int speed); /* * Function: RotateRightUntilBeacon * -------------------------------- * Robot rotates right at input speed until it detects a beacon * with duty cycle d. */ void RotateRightUntilBeacon(char d, int speed); /* * Function: RotateRightUntilBlackTape * ----------------------------------- * Robot rotates right until the middle sensors detect black tape. */ void RotateRightUntilBlackTape(void); /* * Function: RotateLeftUntilBlackTape * ----------------------------------- * Robot rotates left until the middle sensors detect black tape. */ void RotateLeftUntilBlackTape(void); #endif #include "ME218_E128.h" /* Their modules */ #include /* Our modules */ #include "tape.h" #include "IR.h" #include "motor.h" #include "Encoder.h" #include "actions.h" #include /* * Function: WaitForTime * --------------------- * Function that waits for an input amount of time. Uses * the Timers12 library. */ void WaitForTime(unsigned int time) { unsigned int initial = TMRS12_GetTime(); while(TMRS12_GetTime() - initial < time); } /* * Function: WaitAndSort * --------------------- * Function that waits for an input amount of time, and ensures * that the robot is sorting balls at the same time. Uses the * Timers12 library. */ void WaitAndSort(unsigned int time) { unsigned int initial = TMRS12_GetTime(); while(TMRS12_GetTime() - initial < time) { SorterRun(); } } /* * Function: GetOutOfStart * ----------------------- * Function to cause robot to prepare to leave the starting position * to head towards the ball dispenser, and also determine which side * of the field it is on. * * Robot rotates left. If it hits a wall, determined by the requested * motor duty cycle determined by PID control, then the robot changes * the direction of rotation. Robot continues rotating until the 50% * duty cycle beacon is detected. Once that is detected, the robot * continues rotating in its current direction. Depending on what the * IR sensors detect next, the robot determines which side of the field * it is on. * * If the right IR sensor detects a 90% duty cycle, and the robot is * rotating right, then the robot is on the right side of the field. * If the left IR sensor detects no signal, and the robot is rotating * left, then the robot is on the right side of the field. In this * case, the robot recorrects by rotating right until the right * sensor detects a 90% duty cycle. * The decision process is reversed exactly for the left side of the * field. * * Once the side is determined, the robot stops, and the side is returned * as a char. */ char GetOutOfStart(void) { int startTime = TMRS12_GetTime(); char dir = 0; //0 turning left, 1 turning right char win = 10; char done = 0; char side; char flag = 0; SetRampRate(3); LeftMotorEnc(40, _BACKWARD); RightMotorEnc(40, _FORWARD); while(1) { while(1) { if(TMRS12_GetTime() - startTime > 400 && (GetRightDuty() >= 90 || GetLeftDuty() >= 90)) break; if((IRRightDuty() < 50 + win && IRRightDuty() > 50 - win) && (IRLeftDuty() < 50 + win && IRLeftDuty() > 50 - win)) { done = 1; break; } } if(done) break; if(dir == 0) { dir = 1; LeftMotorEnc(40, _FORWARD); RightMotorEnc(40, _BACKWARD); startTime = TMRS12_GetTime(); } else if(dir == 1) { dir = 0; LeftMotorEnc(40, _BACKWARD); RightMotorEnc(40, _FORWARD); startTime = TMRS12_GetTime(); } } while(1) { //right field, going right if((IRRightDuty() < 90 + win && IRRightDuty() > 90 - win) && dir == 1) { side = 2; break; } //left field, going left if((IRLeftDuty() < 90 + win && IRLeftDuty() > 90 - win) && dir == 0) { side = 1; break; } //left field, going right if(IRRightDuty() == -1 && dir == 1) { side = 1; //correct to left SetRampRate(100); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _BACKWARD); SetRampRate(3); LeftMotorEnc(40, _BACKWARD); RightMotorEnc(40, _FORWARD); while(1) { if((IRRightDuty() < 87 + win && IRRightDuty() > 87 - win)) break; } dir = 0; break; } //right field, going left if((IRLeftDuty() == -1) && dir == 0) { side = 2; //correct to right SetRampRate(100); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _FORWARD); SetRampRate(3); LeftMotorEnc(40, _FORWARD); RightMotorEnc(40, _BACKWARD); while(1) { if((IRLeftDuty() < 87 + win && IRLeftDuty() > 87 - win)) break; } dir = 1; break; } } if(dir == 0) { SetRampRate(100); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _FORWARD); } else if(dir == 1) { SetRampRate(100); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _BACKWARD); } return side; } /* * Function: DriveUntilLeftDoubleBlack * ----------------------------------- * Function to cause the robot to drive until it reaches the black * tape line leading to the ball dispenser. This is done by detecting * when a tape sensor finds black tape a second time. This function * assumes that the robot is on the right playing field; consequently, * the left tape sensor is used to detect the black line. * * Robot drives at full speed (100) until the first black tape is * detected. Then the robot drives at 50 until the second black tape * is detected. * * In order to drive in the correct direction, a control algorithm is * implemented as such: the robot attempts to drive on the edge between * the 90% duty cycle and the gap that shows no signal. If left IR * sensor shows nothing, and right shows something, the robot turns * left. If right shows nothing and left shows something, the robot * turns right. If both show nothing the robot turns left, and if both * show something, the robot turns right. The function assumes that * the robot is oriented such that it is facing or almost facing the * 90% duty cycle beacon on the opponent's playing field. */ void DriveUntilLeftDoubleBlack(void) { char flag = 0; unsigned int triggerTime; unsigned int startTime = TMRS12_GetTime(); SetRampRate(3); LeftMotorEnc(100, _FORWARD); RightMotorEnc(100, _FORWARD); //Until First Black Tape while(1) { if(flag) { if(LeftBlack()) { if(TMRS12_GetTime() - triggerTime > 10) break; //make sure black for 10 ms } else { flag = 0; } } else { if(LeftBlack() && TMRS12_GetTime() - startTime > 300) { //wait until beyond green tape flag = 1; triggerTime = TMRS12_GetTime(); } } if(IRRightDuty() != -1 && IRLeftDuty() == -1) { LeftMotorEnc(90, _FORWARD); RightMotorEnc(100, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() != -1) { RightMotorEnc(90, _FORWARD); LeftMotorEnc(100, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() == -1) { LeftMotorEnc(90, _FORWARD); RightMotorEnc(100, _FORWARD); } else { RightMotorEnc(80, _FORWARD); LeftMotorEnc(100, _FORWARD); } } //Until White Floor LeftMotorEnc(50, _FORWARD); RightMotorEnc(50, _FORWARD); while(1) { if(!LeftBlack()) break; if(IRRightDuty() != -1 && IRLeftDuty() == -1) { LeftMotorEnc(40, _FORWARD); RightMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() != -1) { RightMotorEnc(40, _FORWARD); LeftMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() == -1) { LeftMotorEnc(45, _FORWARD); RightMotorEnc(50, _FORWARD); } else { RightMotorEnc(40, _FORWARD); LeftMotorEnc(50, _FORWARD); } } //Until Second Black Tape flag = 0; while(1) { if(flag) { if(LeftBlack()) { if(TMRS12_GetTime() - triggerTime > 5) break; } else { flag = 0; } } else { if(LeftBlack()) { flag = 1; triggerTime = TMRS12_GetTime(); } } if(IRRightDuty() != -1 && IRLeftDuty() == -1) { LeftMotorEnc(40, _FORWARD); RightMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() != -1) { RightMotorEnc(40, _FORWARD); LeftMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() == -1) { LeftMotorEnc(50, _FORWARD); RightMotorEnc(50, _FORWARD); } else { RightMotorEnc(40, _FORWARD); LeftMotorEnc(50, _FORWARD); } } LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); } /* * Function: DriveUntilRightDoubleBlack * ----------------------------------- * Function to cause the robot to drive until it reaches the black * tape line leading to the ball dispenser. This is done by detecting * when a tape sensor finds black tape a second time. This function * assumes that the robot is on the left playing field; consequently, * the right tape sensor is used to detect the black line. * * Robot drives at full speed (100) until the first black tape is * detected. Then the robot drives at 50 until the second black tape * is detected. * * In order to drive in the correct direction, a control algorithm is * implemented as such: the robot attempts to drive on the edge between * the 90% duty cycle and the gap that shows no signal. If left IR * sensor shows nothing, and right shows something, the robot turns * left. If right shows nothing and left shows something, the robot * turns right. If both show nothing the robot turns right, and if both * show something, the robot turns left. The function assumes that * the robot is oriented such that it is facing or almost facing the * 90% duty cycle beacon on the opponent's playing field. */ void DriveUntilRightDoubleBlack(void) { char flag = 0; unsigned int triggerTime; unsigned int startTime = TMRS12_GetTime(); SetRampRate(3); LeftMotorEnc(100, _FORWARD); RightMotorEnc(100, _FORWARD); //Until First Black Tape while(1) { if(flag) { if(RightBlack()) { if(TMRS12_GetTime() - triggerTime > 10) break; //make sure black for 10 ms } else { flag = 0; } } else { if(RightBlack() && TMRS12_GetTime() - startTime > 300) { //wait until beyond green tape flag = 1; triggerTime = TMRS12_GetTime(); } } if(IRRightDuty() != -1 && IRLeftDuty() == -1) { LeftMotorEnc(90, _FORWARD); RightMotorEnc(100, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() != -1) { RightMotorEnc(90, _FORWARD); LeftMotorEnc(100, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() == -1) { LeftMotorEnc(100, _FORWARD); RightMotorEnc(90, _FORWARD); } else { RightMotorEnc(100, _FORWARD); LeftMotorEnc(80, _FORWARD); } } //Until White Floor LeftMotorEnc(50, _FORWARD); RightMotorEnc(50, _FORWARD); while(1) { if(!RightBlack()) break; if(IRRightDuty() != -1 && IRLeftDuty() == -1) { LeftMotorEnc(40, _FORWARD); RightMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() != -1) { RightMotorEnc(40, _FORWARD); LeftMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() == -1) { LeftMotorEnc(50, _FORWARD); RightMotorEnc(45, _FORWARD); } else { RightMotorEnc(50, _FORWARD); LeftMotorEnc(40, _FORWARD); } } //Until Second Black Tape flag = 0; while(1) { if(flag) { if(RightBlack()) { if(TMRS12_GetTime() - triggerTime > 5) break; } else { flag = 0; } } else { if(RightBlack()) { flag = 1; triggerTime = TMRS12_GetTime(); } } if(IRRightDuty() != -1 && IRLeftDuty() == -1) { LeftMotorEnc(40, _FORWARD); RightMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() != -1) { RightMotorEnc(40, _FORWARD); LeftMotorEnc(50, _FORWARD); } else if(IRRightDuty() == -1 && IRLeftDuty() == -1) { LeftMotorEnc(50, _FORWARD); RightMotorEnc(45, _FORWARD); } else { RightMotorEnc(50, _FORWARD); LeftMotorEnc(40, _FORWARD); } } LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); } /* * Function: MoveToBeaconControl * ----------------------------- * Control stage for moving towards a beacon. Assumes robot is * already facing the beacon. Speed is an input. */ void MoveToBeaconControl(char speed) { static char lastState; if (CheckFlag10MS()){ //check IR beacons every 10 ms ClearFlag10MS(); if ((IRLeftDuty()==-1) && (IRRightDuty()==-1) && (lastState == 1)) { //we lost the beacon LeftMotorEnc(0, _FORWARD); RightMotorEnc(speed, _FORWARD); } else if ((IRLeftDuty()==-1) && (IRRightDuty()==-1) && (lastState == 2)) { //we lost the beacon LeftMotorEnc(speed, _FORWARD); RightMotorEnc(0, _FORWARD); } else if (IRLeftDuty()>(IRRightDuty()+3)){ LeftMotorEnc(speed - 10, _FORWARD); RightMotorEnc(speed, _FORWARD); lastState = 1; } else if (IRLeftDuty()<(IRRightDuty()-3)){ LeftMotorEnc(speed, _FORWARD); RightMotorEnc(speed - 10, _FORWARD); lastState = 2; } else{ LeftMotorEnc(speed, _FORWARD); RightMotorEnc(speed, _FORWARD); } } } /* * Function: FollowTapeToDispenser * ------------------------------- * Robot follows tape until it hits the dispenser button. Should pick up * a ball and back up to reset the dispenser. Hitting the dispenser is * detected by the activation of the front bumb sensor. */ void FollowTapeToDispenser(void) { char lastState; unsigned int startTime = TMRS12_GetTime(); SetRampRate(3); LeftMotorEnc(30, _FORWARD); RightMotorEnc(30, _FORWARD); while((!FrontHit())) { SorterRun(); if(MidRightBlack() && MidLeftBlack()) { LeftMotorEnc(30, _FORWARD); RightMotorEnc(30, _FORWARD); } else if(MidRightBlack() && !MidLeftBlack()) { LeftMotorEnc(30, _FORWARD); RightMotorEnc(25, _FORWARD); lastState = 1; } else if(MidLeftBlack() && !MidRightBlack()) { LeftMotorEnc(25, _FORWARD); RightMotorEnc(30, _FORWARD); lastState = 2; } else { if(lastState == 1) { LeftMotorEnc(30, _FORWARD); RightMotorEnc(0, _FORWARD); } else if(lastState == 2) { LeftMotorEnc(0, _FORWARD); RightMotorEnc(30, _FORWARD); } } } LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitAndSort(520); SetRampRate(3); LeftMotorEnc(30, _BACKWARD); RightMotorEnc(30, _BACKWARD); WaitAndSort(250); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); WaitAndSort(50); } /* * Function: MoveTowardBallDispenser * --------------------------------- * Robot moves forward until it hits the ball dispenser, then backs * up to release the dispenser button after collecting a ball. */ void MoveTowardBallDispenser(void){ int startTime = TMRS12_GetTime(),time1; SorterRun(); SetRampRate(3); LeftMotorEnc(30, _FORWARD); RightMotorEnc(30, _FORWARD); while((TMRS12_GetTime() - startTime < 500 || (GetRightDuty() < 70 && GetLeftDuty() < 70)) && (!FrontHit())) { SorterRun(); } LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitAndSort(520); //Wait 400 SetRampRate(3); LeftMotorEnc(30, _BACKWARD); RightMotorEnc(30, _BACKWARD); WaitAndSort(250); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); WaitAndSort(50); } /* * Function: PickUpNBalls * ---------------------- * Function to direct robot to collect a number of balls specified * by an input char. */ void PickUpNBalls(char c) { int i; for(i = 0; i < c; i++) { MoveTowardBallDispenser(); } } /* * Function: ReacquireDispenser * ---------------------------- * Function to direct robot to reacquire the black tape line leading * to the dispenser. Robot backs up a little bit, then rotates right * for a certain amount of time, and then rotates left double that * amount of time. If the robot detects the black tape line, it stops * rotating. The robot then waits until a total of ten seconds have * passed in this function, in order to reset the ball dispenser timing. */ void ReacquireDispenser(void) { unsigned int startTime = TMRS12_GetTime(); unsigned int tempTime; SetRampRate(3); //backup a bit LeftMotorEnc(30, _BACKWARD); RightMotorEnc(30, _BACKWARD); WaitAndSort(200); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); WaitAndSort(100); LeftMotorEnc(30, _FORWARD); RightMotorEnc(30, _BACKWARD); tempTime = TMRS12_GetTime(); while(TMRS12_GetTime() - tempTime < 300) { if(MidRightBlack() || MidLeftBlack()) { break; } SorterRun(); } LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _BACKWARD); WaitAndSort(100); LeftMotorEnc(30, _BACKWARD); RightMotorEnc(30, _FORWARD); tempTime = TMRS12_GetTime(); while(TMRS12_GetTime() - tempTime < 700) { if(MidRightBlack() || MidLeftBlack()) { break; } SorterRun(); } LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _FORWARD); WaitAndSort(100); while(TMRS12_GetTime() - startTime < 4800){ SorterRun(); } } /* * Function: BackUpToGotoGoalThree * ------------------------------- * Function to cause the robot to back away from the dispenser * in preparation to rotate and acquire goal three. Takes a char * determining the side the robot is on as an input. Depending * on which side, the robot backs up a different amount of time. */ void BackUpToGotoGoalThree(char side) { int time; if(side == 2) time = 300; else if(side == 1) time = 360; SetRampRate(10); LeftMotorEnc(100, _BACKWARD); RightMotorEnc(100, _BACKWARD); WaitAndSort(time); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); } /* * Function: RotateRightToGoalThree * -------------------------------- * Robot rotates right while looking for the 90% duty cycle beacon * that marks goal three. Once it is found, the robot stops. */ void RotateRightToGoalThree(void) { SetRampRate(3); LeftMotorEnc(40, _BACKWARD); RightMotorEnc(40, _FORWARD); while(IRRightDuty() < 87 - 7 || IRRightDuty() > 87 + 7 || IRLeftDuty() < 87 - 7 || IRLeftDuty() > 87 + 7 ); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _FORWARD); } /* * Function: RotateLeftToGoalThree * -------------------------------- * Robot rotates left while looking for the 90% duty cycle beacon * that marks goal three. Once it is found, the robot stops. */ void RotateLeftToGoalThree(void) { SetRampRate(3); LeftMotorEnc(40, _FORWARD); RightMotorEnc(40, _BACKWARD); while(IRRightDuty() < 87 - 7 || IRRightDuty() > 87 + 7 || IRLeftDuty() < 87 - 7 || IRLeftDuty() > 87 + 7 ); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _BACKWARD); } /* * Function: DriveToGoalThree * -------------------------- * Function to direct robot to drive forward until it reaches goal * three. Robot drives forward at a speed of 80 until it reaches * green tape under a control scheme to make sure it is going in the * right direction. At this point, the speed is halved to 40. * * Reaaching goal three is detected either by activation of the goal * three bump sensor, or by detecting that the robot is pushing against * a wall, determined by the requested motor duty cycle by PID control. * * Once the robot reaches goal three, it opens the hopper to dump all * its balls. Robot then drives forward against goal three for a short * time to shake the hopper and cause stuck balls to fall out. Then * the robot closes its hopper. */ void DriveToGoalThree(void) { SetRampRate(10); LeftMotorEnc(80, _FORWARD); RightMotorEnc(80, _FORWARD); while(1) { MoveToBeaconControl(80); if(AnyGreen()) break; } LeftMotorEnc(40, _FORWARD); RightMotorEnc(40, _FORWARD); while((GetRightDuty() < 90 && GetLeftDuty() < 90) || !Goal3Hit()); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); OpenHopper(); WaitForTime(100); //Jostle machine LeftMotorEnc(30, _FORWARD); RightMotorEnc(30, _FORWARD); WaitForTime(200); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); CloseHopper(); } /* * Function: SecondDispenserRunRight * --------------------------------- * Function to direct robot to prepare for a second dispenser * run after depositing its balls in goal three. Assumes robot * starts at goal three and on right playing field. * * Robot goes backwards at full speed for a certain amount of * time. Robot then rotates right until it detects the 30% * beacon marking goal one. The robot then opens its shooter * in order to shoot any balls that may have mistakenly fallen * into the shooter ramp (in order to avoid carrying more than * ten balls at any one time). Robot then moves forward until * it detects black tape. */ void SecondDispenserRunRight(void) { SetRampRate(3); LeftMotorEnc(100, _BACKWARD); RightMotorEnc(100, _BACKWARD); WaitForTime(700); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); SelectSideEyes(); RotateRightUntilBeacon(30, 40); OpenShooter(); SetRampRate(3); LeftMotorEnc(50, _FORWARD); RightMotorEnc(50, _FORWARD); while(!AnyBlack()); CloseShooter(); SelectFrontEyes(); WaitForTime(30); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); } /* * Function: SecondDispenserRunLeft * --------------------------------- * Function to direct robot to prepare for a second dispenser * run after depositing its balls in goal three. Assumes robot * starts at goal three and on left playing field. * * Robot goes backwards at full speed for a certain amount of * time. Robot then rotates left until it detects the 30% * beacon marking goal one. The robot then opens its shooter * in order to shoot any balls that may have mistakenly fallen * into the shooter ramp (in order to avoid carrying more than * ten balls at any one time). Robot then moves forward until * it detects black tape. */ void SecondDispenserRunLeft(void) { SetRampRate(3); LeftMotorEnc(100, _BACKWARD); RightMotorEnc(100, _BACKWARD); WaitForTime(700); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); SelectSideEyes(); RotateLeftUntilBeacon(30, 40); OpenShooter(); SetRampRate(3); LeftMotorEnc(50, _FORWARD); RightMotorEnc(50, _FORWARD); while(!AnyBlack()); SelectFrontEyes(); CloseShooter(); WaitForTime(30); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); } /* * Function: FinalShotRight * ------------------------ * Function to direct robot to take its final shot with the shooter * at goal two. Assumes the robot starts at goal three on the right * side of the field. */ void FinalShotRight(void) { unsigned long Mile1; static char lastState,linedup=0; char win=3; int speed=60; SetRampRate(3); LeftMotorEnc(100, _BACKWARD); RightMotorEnc(100, _BACKWARD); WaitForTime(450); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); WaitForTime(200); SelectSideEyes(); LeftMotorEnc(40, _FORWARD); RightMotorEnc(0, _FORWARD); Mile1=MileageLeft(); while((MileageLeft()-Mile1)<180); RotateRightUntilBeacon(68, 30); Mile1=MileageLeft(); while((MileageLeft()-Mile1)<140 || linedup==0){ if ((MileageLeft()-Mile1)>100){ speed=30; win=1; } if ((IRLeftDuty()==-1) && (IRRightDuty()==-1) && (lastState == 1)) { //we lost the beacon LeftMotorEnc(0, _FORWARD); RightMotorEnc(speed, _FORWARD); linedup=0; } else if ((IRLeftDuty()==-1) && (IRRightDuty()==-1) && (lastState == 2)) { //we lost the beacon LeftMotorEnc(speed, _FORWARD); RightMotorEnc(0, _FORWARD); linedup=0; } else if (IRLeftDuty()>(IRRightDuty()+win)){ LeftMotorEnc(speed - 10, _FORWARD); RightMotorEnc(speed, _FORWARD); lastState = 1; linedup=0; } else if (IRLeftDuty()<(IRRightDuty()-win)){ LeftMotorEnc(speed, _FORWARD); RightMotorEnc(speed - 10, _FORWARD); lastState = 2; linedup=0; } else{ LeftMotorEnc(speed, _FORWARD); RightMotorEnc(speed, _FORWARD); linedup=1; } } SetRampRate(3); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); LeftMotorEnc(30, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(250); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); OpenShooter(); DisableMotor(); } /* * Function: FinalShotLeft * ------------------------ * Function to direct robot to take its final shot with the shooter * at goal two. Assumes the robot starts at goal three on the left * side of the field. */ void FinalShotLeft(void) { unsigned long Mile1; static char lastState,linedup=0; char win=3;int speed=60; SetRampRate(3); LeftMotorEnc(100, _BACKWARD); RightMotorEnc(100, _BACKWARD); WaitForTime(450); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _BACKWARD); WaitForTime(200); SelectSideEyes(); LeftMotorEnc(0, _FORWARD); RightMotorEnc(40, _FORWARD); Mile1=MileageRight(); while((MileageRight()-Mile1)<180); Mile1=MileageRight(); RotateLeftUntilBeacon(68, 30); Mile1=MileageLeft(); while((MileageLeft()-Mile1)<140 || linedup==0){ if ((MileageLeft()-Mile1)>100){ speed=30; win=1; } if ((IRLeftDuty()==-1) && (IRRightDuty()==-1) && (lastState == 1)) { //we lost the beacon LeftMotorEnc(0, _FORWARD); RightMotorEnc(speed, _FORWARD); linedup=0; } else if ((IRLeftDuty()==-1) && (IRRightDuty()==-1) && (lastState == 2)) { //we lost the beacon LeftMotorEnc(speed, _FORWARD); RightMotorEnc(0, _FORWARD); linedup=0; } else if (IRLeftDuty()>(IRRightDuty()+win)){ LeftMotorEnc(speed - 10, _FORWARD); RightMotorEnc(speed, _FORWARD); lastState = 1; linedup=0; } else if (IRLeftDuty()<(IRRightDuty()-win)){ LeftMotorEnc(speed, _FORWARD); RightMotorEnc(speed - 10, _FORWARD); lastState = 2; linedup=0; } else{ LeftMotorEnc(speed, _FORWARD); RightMotorEnc(speed, _FORWARD); linedup=1; } } SetRampRate(3); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); LeftMotorEnc(0, _FORWARD); RightMotorEnc(25, _FORWARD); WaitForTime(150); LeftMotorEnc(0, _FORWARD); RightMotorEnc(0, _FORWARD); WaitForTime(100); OpenShooter(); } /* * Function: RotateLeftUntilBeacon * -------------------------------- * Robot rotates left at input speed until it detects a beacon * with duty cycle d. */ void RotateLeftUntilBeacon(char d, int speed){ char win = 7; //Start turning left SetRampRate(10); LeftMotorEnc(speed, _BACKWARD); RightMotorEnc(speed, _FORWARD); while((IRLeftDuty()<(d-win)) || (IRLeftDuty()>(d+win)) || (IRRightDuty()<(d-win)) || (IRRightDuty()>(d+win))){ if (((IRLeftDuty()d-10))||((IRLeftDuty()d-10))) speed=30; } //Stop turning left SetRampRate(100); LeftMotorEnc(0, _BACKWARD); RightMotorEnc(0, _FORWARD);; } /* * Function: RotateRightUntilBeacon * -------------------------------- * Robot rotates right at input speed until it detects a beacon * with duty cycle d. */ void RotateRightUntilBeacon(char d, int speed){ char win = 5; //Start turning left SetRampRate(10); LeftMotorEnc(speed, _FORWARD); RightMotorEnc(speed, _BACKWARD); while((IRLeftDuty()<(d-win)) || (IRLeftDuty()>(d+win)) || (IRRightDuty()<(d-win)) || (IRRightDuty()>(d+win))){ if (((IRLeftDuty()>d+10)&&(IRLeftDuty()d+10)&&(IRLeftDuty()