// #define TESTING_SENSOR // #define LED_TEST // #define TEST_BOOST // PS2=RECIEVE PS3=TRANSMIT /***************************************************/ // Import Library #include <hidef.h> /* common defines and macros */ #include <mc9s12e128.h> /* derivative information */ #include "ME218_E128.h" /* bit definitions */ #include "S12eVec.h" /* interrupts */ #include <timers12.h> #include "ads12.h" #include <stdio.h> #include "E128_IO.h" #include "Communication.h" //Includes Color Defines /*****************Module Defines**********************/ // Define Numerical Constants #define TRUE 1 #define FALSE 0 #define ON 1 #define OFF 0 #define BOOST_VALUE 100 #define VOLTAGE_SPAN 330 #define FORWARD 0 #define REVERSE 1 #define MAX_SPEED 240 // rpm for biking at 20 mph #define HI_SIGNAL 0 #define LO_SIGNAL 1 #define _MS_ *3000 #define PERIOD (20_MS_) #define START_POSITION (2 _MS_) #define END_POSITION (1 _MS_) /******************Module Types*********************/ typedef enum{ FRONT_SENSOR, MIDDLE_SENSOR, BACK_SENSOR } FlagHistory_t; /*******************MODULE VARIABLES********************/ static unsigned int Speed = 0; static unsigned char Boost = OFF; static unsigned char Heading = 90; // 0 is West and 180 is East static unsigned char HeadingOld = 90; static unsigned char DriveDirection = FORWARD; static unsigned char BoostRechargeCounter; static unsigned char BoostState = OFF; static unsigned char ISRFlag = FALSE; static unsigned int LastTopWheelTime; static unsigned int CurrentTopWheelTime; static unsigned char LastMagnetFlag; static unsigned char CurrentMagnetFlag; static unsigned int HiTime = START_POSITION; static unsigned char LastInterrupt = LO_SIGNAL; /****************PRIVATE FUNCTION PROTOTYPES***************/ void InitPins(void); void InitIC(void); /*******************INTERRUPT RESPONSES***********************/ // Boost Button: Timer 1 IC7 interrupt response void interrupt _Vec_tim1ch7 ButtonPress(void){ (void)printf("Button Pressed\r\n"); TIM1_TFLG1 = _S12_C7F; /* clear IC7 flag */ // If you have a boost available, then use it if(BoostRechargeCounter == 100){ // Go to boost mode Boost = ON; //(void)printf("Boost ON\r\n"); // Restart boost timer (void) TMRS12_ClearTimerExpired(RECHARGE_TIMEOUT); //Clear timer flag (void) TMRS12_InitTimer(RECHARGE_TIMEOUT,RECHARGE_TIMEOUT_PERIOD); //Start a timeout for next recharge interval // Start boost timer (void) TMRS12_InitTimer(BOOST_TIMER,BOOST_TIMER_LENGTH); //Reset BoostRechargeCounter and start waiting for next boost BoostRechargeCounter = 0; } } // FRONT wheel sensor: IC6 interrupt response void interrupt _Vec_tim1ch6 TopSensor(void) { TIM1_TFLG1 = _S12_C6F; /* clear IC6 flag */ // Save time history for comparison before getting new time LastTopWheelTime = CurrentTopWheelTime; CurrentTopWheelTime = TMRS12_GetTime(); // Calculate the rpm of the wheel scaled to a number 0-100 // [60sec/min * 1000ms/sec * (1/Period)] * 100/Max Speed Speed = (60000/((CurrentTopWheelTime - LastTopWheelTime)*3))*100/MAX_SPEED; // Update flag history for direction determination LastMagnetFlag = CurrentMagnetFlag; CurrentMagnetFlag = FRONT_SENSOR; ISRFlag = TRUE; // To determine the direction, test the current and last interrupts // If both were from the same sensor, then there was a direction change // If the current and last interrupts were from different sensors, then the direction didn't change if(CurrentMagnetFlag == LastMagnetFlag){ //Toggle direction if(DriveDirection == FORWARD) DriveDirection = REVERSE; else DriveDirection = FORWARD; } else if(LastMagnetFlag == MIDDLE_SENSOR) DriveDirection = FORWARD; else DriveDirection = REVERSE; // Start timeout timer (void) TMRS12_InitTimer(WHEEL_TIMEOUT,WHEEL_TIMEOUT_PERIOD); //Start a timeout in case wheel has stopped } // MIDDLE wheel sensor: IC5 interrupt response void interrupt _Vec_tim1ch5 BottomSensor(void) { TIM1_TFLG1 = _S12_C5F; /* clear IC5 flag */ // Update flag history for direction determination LastMagnetFlag = CurrentMagnetFlag; CurrentMagnetFlag = MIDDLE_SENSOR; ISRFlag = TRUE; // To determine the direction, test the current and last interrupts // If both were from the same sensor, then there was a direction change // If the current and last interrupts were from different sensors, then the direction didn't change if(CurrentMagnetFlag == LastMagnetFlag){ //Toggle direction if(DriveDirection == FORWARD) DriveDirection = REVERSE; else DriveDirection = FORWARD; } else if(LastMagnetFlag == BACK_SENSOR) DriveDirection = FORWARD; else DriveDirection = REVERSE; } // BACK wheel sensor: IC4 interrupt response void interrupt _Vec_tim1ch4 LeftSensor(void) { TIM1_TFLG1 = _S12_C4F; /* clear IC7 flag */ // Update flag history for direction determination LastMagnetFlag = CurrentMagnetFlag; CurrentMagnetFlag = BACK_SENSOR; ISRFlag = TRUE; // To determine the direction, test the current and last interrupts // If both were from the same sensor, then there was a direction change // If the current and last interrupts were from different sensors, then the direction didn't change if(CurrentMagnetFlag == LastMagnetFlag){ //Toggle direction if(DriveDirection == FORWARD) DriveDirection = REVERSE; else DriveDirection = FORWARD; } else if(LastMagnetFlag == FRONT_SENSOR) DriveDirection = FORWARD; else DriveDirection = REVERSE; } // Servo Update: Timer 0 IC7 interrupt response void interrupt _Vec_tim0ch7 ServoCompare(void){ static unsigned int ThisHiTime; TIM0_TFLG1 = _S12_C7F; /* clear IC7 flag */ // Set next output compare based on if last time was low or high if (LastInterrupt == HI_SIGNAL){ TIM0_TC7 = TIM0_TC7 + (PERIOD - ThisHiTime); LastInterrupt = LO_SIGNAL; } else if (LastInterrupt == LO_SIGNAL){ ThisHiTime = HiTime; TIM0_TC7 = TIM0_TC7 + ThisHiTime; LastInterrupt = HI_SIGNAL; } // Clear OC7 Flag TIM0_TFLG1 = _S12_C7F; } /*************************FUNCTIONS***************************/ // Initialize I/O Pins void InitPins(void){ (void)ADS12_Init("OOOOOOAA"); // Set port T3 to output for Boost Servo T2 for debug DDRT |= BIT3HI | BIT2HI; //Make sure T4,T5,T6,T7 are inputs DDRT &= BIT4LO & BIT5LO & BIT6LO & BIT7LO; // Set all U ports and T0,T1 to outputs for Atoll LED display DDRU |= BIT0HI | BIT1HI | BIT2HI | BIT3HI | BIT4HI | BIT5HI | BIT6HI | BIT7HI; DDRT |= BIT0HI | BIT1HI; DDRP |= (BIT1HI | BIT0HI); } //Initialize input capture for speed sensors and boost button void InitIC(void){ // Initialize timer 1 TIM1_TSCR1 = _S12_TEN; TIM1_TSCR2 = (_S12_PR0 | _S12_PR2); // Enable button interrupt (Port T7) // Set up Timer 1 IC7 TIM1_TIOS &= ~(_S12_IOS7); // Set upto capture falling edges only TIM1_TCTL3 &= ~(_S12_EDG7A); TIM1_TCTL3 |= (_S12_EDG7B); // Clear Flag TIM1_TFLG1 = _S12_C7F; // Enable Interrupt TIM1_TIE |= _S12_C7I; //Set boost to uncharged initially BoostRechargeCounter = 0; // Enable Hall 1 (Port T6) // Set up Timer 1 IC6 TIM1_TIOS &= ~(_S12_IOS6); // Set upto capture falling edges only TIM1_TCTL3 &= ~(_S12_EDG6A); TIM1_TCTL3 |= (_S12_EDG6B); // Clear Flag TIM1_TFLG1 = _S12_C6F; // Enable Interrupt TIM1_TIE |= _S12_C6I; // Enable Hall 2 (Port T5) // Set up Timer 1 IC5 TIM1_TIOS &= ~(_S12_IOS5); // Set upto capture falling edges only TIM1_TCTL3 &= ~(_S12_EDG5A); TIM1_TCTL3 |= (_S12_EDG5B); // Clear Flag TIM1_TFLG1 = _S12_C5F; // Enable Interrupt TIM1_TIE |= _S12_C5I; // Enable Hall 3 (Port T4) // Set up Timer 1 IC4 TIM1_TIOS &= ~(_S12_IOS4); // Set upto capture falling edges only TIM1_TCTL3 &= ~(_S12_EDG4A); TIM1_TCTL3 |= (_S12_EDG4B); // Clear Flag TIM1_TFLG1 = _S12_C4F; // Enable Interrupt TIM1_TIE |= _S12_C4I; } // This function sets up OC7 void InitBoostServo(void){ TIM0_TSCR1 = _S12_TEN; TIM0_TIE = _S12_C7F; /* enable output capture interrupts*/ TIM0_TSCR2 = _S12_PR1|_S12_PR0; /* set pre-scale to /8 =3MHz timer clk*/ TIM0_TIOS = (_S12_IOS4 | _S12_IOS7); /* set cap/comp 7 to output compare */ TIM0_TCTL1 |= _S12_OL7; /* Toggle PT3 on compare*/ TIM0_TCTL1 &= ~_S12_OM7; PTT &= BIT3LO; // Start with low time and program the high TIM0_TC7 = TIM0_TCNT + START_POSITION; /* schedule first rise */ TIM0_TFLG1 = _S12_C7F; /* clear OC2 flag */ // Setup first boost charging step (void) TMRS12_InitTimer(RECHARGE_TIMEOUT,RECHARGE_TIMEOUT_PERIOD); //Start a timeout for next recharge interval EnableInterrupts; // Enable global interrupts } void InitDebug(void) { // Start running timer (void) TMRS12_InitTimer(RUNNING_TIMER,RUNNING_TIMER_PERIOD); // Turn off 90 degree detect LED to start\ PTP &= BIT1LO; } void UpdateDebug(void) { if(TMRS12_IsTimerExpired(RUNNING_TIMER) == TMRS12_EXPIRED){ (void) TMRS12_ClearTimerExpired(RUNNING_TIMER); // Clear timer flag PTP ^= BIT0HI; // Toggle LED PTP ^= BIT1HI; (void) TMRS12_InitTimer(RUNNING_TIMER,RUNNING_TIMER_PERIOD); // Start again } /* // Turn on LED if we're close to 90 degree heading if((Heading - 90) < 20 || (90 - Heading) < 20) PTP |= BIT1HI; */ } //Takes no inputs and returns the wheel speed unsigned char ReadWheelSpeed(void){ static unsigned char returnSpeed; returnSpeed = (unsigned char)Speed; if(Boost == ON){ returnSpeed = BOOST_VALUE; } if (TMRS12_IsTimerExpired(BOOST_TIMER) == TMRS12_EXPIRED) { (void) TMRS12_ClearTimerExpired(BOOST_TIMER); Boost = OFF; } return (unsigned char)returnSpeed; } //Takes no inputs and returns the wheel rotation direction unsigned char ReadWheelDirection(void){ return DriveDirection; } //Takes no inputs and returns nothing, but modifies the handlebar heading with West=0 and East=180 unsigned char ReadHandlebar(void){ Heading = (unsigned char)(((unsigned long)(ADS12_ReadADPin(0)) - 363)*30/165); return (unsigned char)Heading; } //Takes no inputs and returns nothing, but modifies the handlebar heading with West=0 and East=180 unsigned char ReadHandlebarOld(void){ HeadingOld = (unsigned char)((unsigned long)ADS12_ReadADPin(0)*180/(VOLTAGE_SPAN)); return (unsigned char)HeadingOld; } //Check if you should update the boost level void CheckBoostRecharge(void){ if(BoostRechargeCounter < 100){ if(TMRS12_IsTimerExpired(RECHARGE_TIMEOUT) == TMRS12_EXPIRED){ // Increment the boost by 5 so that it will be 100% charged after 20 seconds BoostRechargeCounter++; // (void)printf("Recharge %d\r\n",BoostRechargeCounter); // Clear timer flag and restart timeout for next recharge interval (void) TMRS12_ClearTimerExpired(RECHARGE_TIMEOUT); (void) TMRS12_InitTimer(RECHARGE_TIMEOUT,RECHARGE_TIMEOUT_PERIOD); UpdateBoostLevel(100-BoostRechargeCounter); } } } // Check for a wheel sensor timeout void CheckSensorTimeout(void){ if(TMRS12_IsTimerExpired(WHEEL_TIMEOUT) == TMRS12_EXPIRED){ (void) TMRS12_ClearTimerExpired(WHEEL_TIMEOUT); // Clear timer flag Speed = 0; // Set Speed to 0 since we are idle (void) printf("Timeout\r\n"); } } void UpdateBoostLevel(unsigned char Update){ unsigned int DutyCycle; DutyCycle = Update; HiTime = ((DutyCycle * ((START_POSITION - END_POSITION)/100)) + END_POSITION); //(void)printf("Hi Time %d Duty Cycle %d\r\n",HiTime,DutyCycle); } // This funciton takes the atoll number and team that captured it to control the status LEDs void AtollLED(unsigned char AtollNumber,unsigned char TeamColor){ switch(AtollNumber){ case ATOLL1: if(TeamColor == RED){ // Red On PTU |= BIT0HI; // Green Off PTU &= BIT1LO; } if(TeamColor == GREEN){ // Red Off PTU &= BIT0LO; // Green On PTU |= BIT1HI; } if(TeamColor == NO_COLOR){ // Red Off PTU |= BIT0HI; // Green Off PTU |= BIT1HI; } break; case ATOLL2: if(TeamColor == RED){ // Red On PTU |= BIT2HI; // Green Off PTU &= BIT3LO; } if(TeamColor == GREEN){ // Red Off PTU &= BIT2LO; // Green On PTU |= BIT3HI; } if(TeamColor == NO_COLOR){ // Red Off PTU |= BIT2HI; // Green Off PTU |= BIT3HI; } break; case ATOLL3: if(TeamColor == RED){ // Red On PTU |= BIT4HI; // Green Off PTU &= BIT5LO; } if(TeamColor == GREEN){ // Red Off PTU &= BIT4LO; // Green On PTU |= BIT5HI; } if(TeamColor == NO_COLOR){ // Red Off PTU |= BIT4HI; // Green Off PTU |= BIT5HI; } break; case ATOLL4: if(TeamColor == RED){ // Red On PTU |= BIT6HI; // Green Off PTU &= BIT7LO; } if(TeamColor == GREEN){ // Red Off PTU &= BIT6LO; // Green On PTU |= BIT7HI; } if(TeamColor == NO_COLOR){ // Red Off PTU |= BIT6HI; // Green Off PTU |= BIT7HI; } break; case ATOLL5: if(TeamColor == RED){ // Red On PTT |= BIT0HI; // Green Off PTT &= BIT1LO; } if(TeamColor == GREEN){ // Red Off PTT &= BIT0LO; // Green On PTT |= BIT1HI; } if(TeamColor == NO_COLOR){ // Red Off PTT |= BIT0HI; // Green Off PTT |= BIT1HI; } break; } } void AtollLEDOff(void){ AtollLED(ATOLL1, NO_COLOR); AtollLED(ATOLL2, NO_COLOR); AtollLED(ATOLL3, NO_COLOR); AtollLED(ATOLL4, NO_COLOR); AtollLED(ATOLL5, NO_COLOR); } #ifdef TESTING_SENSOR // Main: Takes nothing, Returns nothing void main(void) { static unsigned char DataToSend[7]; // Initialize: TMRS12_Init(TMRS12_RATE_1MS); // Initialize 1ms timer InitIC(); EnableInterrupts; // Enable global interrupts // Get first values for speed and direction CurrentTopWheelTime = TMRS12_GetTime(); CurrentMagnetFlag = BOTTOM_SENSOR; DriveDirection = FORWARD; // Setup first boost chraging (void) TMRS12_InitTimer(RECHARGE_TIMEOUT,RECHARGE_TIMEOUT_PERIOD); //Start a timeout for next recharge interval // Clear BoostRechargeCounter and start waiting for next boost BoostRechargeCounter = 0; (void) printf("Initialized\r\n"); // Loop Forver while(1) { CheckSensorTimeout(); // Everytime there is an interrupt, update the data if(ISRFlag == TRUE){ (void) printf("Speed %d Direction %d\r\n",Speed,DriveDirection); //(void) printf("Last Flag %d Current Flag %d Last Time %d Current Time %d\r\n",LastMagnetFlag,CurrentMagnetFlag,LastTopWheelTime,CurrentTopWheelTime); //(void) printf("Last Time %d Current Time %d Delta %d\r\n",LastTopWheelTime,CurrentTopWheelTime,Speed); ISRFlag = FALSE; } } } // Main Bracket #endif #ifdef LED_TEST void main(void){ unsigned char state = 0; InitPins(); AtollLEDOff(); TMRS12_Init(TMRS12_RATE_1MS); (void) TMRS12_InitTimer(0,1000); while(1){ if (TMRS12_IsTimerExpired(0) == 1){ if (state == 0){ // Turn all LEDs Green AtollLED(ATOLL1, GREEN); AtollLED(ATOLL2, GREEN); AtollLED(ATOLL3, GREEN); AtollLED(ATOLL4, GREEN); AtollLED(ATOLL5, GREEN); state = 1; } else if (state == 1){ // Turn all LEDs Red AtollLED(ATOLL1, RED); AtollLED(ATOLL2, RED); AtollLED(ATOLL3, RED); AtollLED(ATOLL4, RED); AtollLED(ATOLL5, RED); state = 2; } else if (state == 2) { // Turn all LEDs Off AtollLEDOff(); state = 0; } (void)TMRS12_ClearTimerExpired(0); (void)TMRS12_InitTimer(0, 1000); } } } #endif #ifdef TEST_BOOST //Main: Takes nothing, Returns nothing void main(void) { char press; // Initialize: InitBoostServo(); //TMRS12_Init(TMRS12_RATE_1MS); // Initialize 1ms timer //(void) TMRS12_InitTimer(0,100); // Set a 5ms timer to refresh Duty (void) printf("Initialized Boost Servo\r\n"); // Loop Forver while(1){ CheckServoTimers(); if(kbhit() == 1) { press = (char)getchar(); switch(press) { case '1': (void)printf("Duty %d\r\n",DutyCycle); DutyCycle = 0; break; case '2': (void)printf("Duty %d\r\n",DutyCycle); DutyCycle = 25; break; case '3': (void)printf("Duty %d\r\n",DutyCycle); DutyCycle = 50; break; case '4': (void)printf("Duty %d\r\n",DutyCycle); DutyCycle = 75; break; case '5': (void)printf("Duty %d\r\n",DutyCycle); DutyCycle = 100; break; } } }// While Bracket } // Main Bracket #endif