/* --COPYRIGHT--,BSD * Copyright (c) 2017, Texas Instruments Incorporated * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * * Neither the name of Texas Instruments Incorporated nor the names of * its contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * --/COPYRIGHT--*/ /****************************************************************************** * MSP432 Empty Project * * Description: An empty project that uses DriverLib * * MSP432P401 * ------------------ * /|\| | * | | | * --|RST | * | | * | | * | | * | | * | | * Author: *******************************************************************************/ /* DriverLib Includes */ #include /* Standard Includes */ #include #include #include //8 7 6 5 4 3 2 1 0 #define sens8 GPIO_PORT_P7,GPIO_PIN7 #define sens7 GPIO_PORT_P7,GPIO_PIN6 #define sens6 GPIO_PORT_P7,GPIO_PIN5 #define sens5 GPIO_PORT_P7,GPIO_PIN4 #define sens4 GPIO_PORT_P7,GPIO_PIN3 #define sens3 GPIO_PORT_P7,GPIO_PIN2 #define sens2 GPIO_PORT_P7,GPIO_PIN1 #define sens1 GPIO_PORT_P7,GPIO_PIN0 #define sensODD GPIO_PORT_P9,GPIO_PIN2 #define sensEVEN GPIO_PORT_P5,GPIO_PIN3 #define RLED GPIO_PORT_P1,GPIO_PIN0 //5 4 3 2 1 0 #define bump5 GPIO_PORT_P4,GPIO_PIN7 #define bump4 GPIO_PORT_P4,GPIO_PIN6 #define bump3 GPIO_PORT_P4,GPIO_PIN5 #define bump2 GPIO_PORT_P4,GPIO_PIN3 #define bump1 GPIO_PORT_P4,GPIO_PIN2 #define bump0 GPIO_PORT_P4,GPIO_PIN0 #define lMotorDrive GPIO_PORT_P3,GPIO_PIN7 #define lMotorDir GPIO_PORT_P5,GPIO_PIN4 #define lMotorPWM GPIO_PORT_P2,GPIO_PIN7 #define RGBRED GPIO_PORT_P2,GPIO_PIN0 #define RGBGREEN GPIO_PORT_P2,GPIO_PIN1 #define RGBBLUE GPIO_PORT_P2,GPIO_PIN2 #define rMotorDrive GPIO_PORT_P3,GPIO_PIN6 #define rMotorPWM GPIO_PORT_P2,GPIO_PIN6 #define rMotorDir GPIO_PORT_P5,GPIO_PIN5 #define frontBlinker GPIO_PORT_P8,GPIO_PIN5 #define frontBlinkerLeft GPIO_PORT_P8,GPIO_PIN0 #define backBlinkerLeft GPIO_PORT_P8,GPIO_PIN7 #define backBlinkerRight GPIO_PORT_P8,GPIO_PIN6 #define DUTY_NOMINAL 12000 #define PERIOD 15000 #define MAX 13000 #define kp 27 #define ki .4 #define kd 30 //Timer Declarations Timer_A_PWMConfig myPWM; Timer_A_PWMConfig myPWMr; Timer_A_UpModeConfig myTimer; Timer_A_UpModeConfig myTimerInterrupt; //PI Controller Declarations int32_t P = 0, I = 0, U, D; int32_t last_prop = 0; int32_t change = 0; int32_t swing = 2500; uint8_t sensArr[8] = {0,0,0,0,0,0,0,0}; uint8_t sensList = 0x00; uint8_t countRand = 0; uint8_t init =0 ; //Timer Functions void (*TimerA1Task)(void); void configTimerA(uint16_t delayValue, uint16_t clockDivider); void myTimerADelay(uint16_t delayValue, uint16_t clockDivider); void TimerA1_Init(void(*task)(void), uint16_t period); void TA1_0_IRQHandler(void); void wait1us(uint16_t num); //Sensor Functions void initPin(void); void checkLine(uint8_t sensArr[8]); void sensorOutput(); void turnLeft(int32_t U); void turnRight(int32_t U); int main(void) { /* Stop Watchdog */ MAP_WDT_A_holdTimer(); volatile uint16_t i, ii; uint8_t bumpArr[6], pushFLG[6] = {0,0,0,0,0,0}, startFLG = 0; initPin(); while(1) { bumpArr[5] = GPIO_getInputPinValue(bump5); bumpArr[4] = GPIO_getInputPinValue(bump4); bumpArr[3] = GPIO_getInputPinValue(bump3); bumpArr[2] = GPIO_getInputPinValue(bump2); bumpArr[1] = GPIO_getInputPinValue(bump1); bumpArr[0] = GPIO_getInputPinValue(bump0); if(startFLG == 0) { for(i = 0; i < 6; i++) { if(bumpArr[i] == GPIO_INPUT_PIN_LOW) { for(ii = 0; ii < 10000; ii++){}; pushFLG[i] =1; } } for(i = 0; i < 6; i++) { if(bumpArr[i] == GPIO_INPUT_PIN_HIGH && pushFLG[i] == 1) { GPIO_setOutputHighOnPin(lMotorDrive); GPIO_setOutputHighOnPin(rMotorDrive); GPIO_setOutputHighOnPin(frontBlinker); GPIO_setOutputHighOnPin(frontBlinkerLeft); Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE); pushFLG[i] = 0; startFLG = 1; TimerA1_Init(&TA1_0_IRQHandler, 734); } } } else { //Stop the bot if it hits something for(i = 0; i < 6; i++) { if(bumpArr[i] == GPIO_INPUT_PIN_LOW) { __disable_irq(); GPIO_setOutputLowOnPin(frontBlinker); GPIO_setOutputLowOnPin(frontBlinkerLeft); GPIO_setOutputHighOnPin(backBlinkerLeft); GPIO_setOutputHighOnPin(backBlinkerRight); GPIO_setOutputHighOnPin(lMotorDir); GPIO_setOutputHighOnPin(rMotorDir); Timer_A_stop(TIMER_A0_BASE); myPWM.dutyCycle = 14000; myPWMr.dutyCycle = 14000; Timer_A_outputPWM(TIMER_A0_BASE, &myPWM); Timer_A_outputPWM(TIMER_A0_BASE, &myPWMr); Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE); pushFLG[i] = 1; } } for (i = 0; i < 6; i++) { if(bumpArr[i] == GPIO_INPUT_PIN_HIGH && pushFLG[i] == 1) { pushFLG[i] = 0; } } countRand++; countRand%= 3; } } } void TA1_0_IRQHandler(void) { TIMER_A1->CCTL[0] &= ~0x0001; checkLine(sensArr); sensorOutput(); GPIO_toggleOutputOnPin(backBlinkerLeft); } void turnRight(int32_t U) { //Slow down and Turn wheel back wards if(DUTY_NOMINAL + U > MAX) { GPIO_setOutputHighOnPin(lMotorDir); myPWM.dutyCycle = MAX-((DUTY_NOMINAL + U) -MAX); } else if (DUTY_NOMINAL + U < 0) //Fastest Speed { GPIO_setOutputLowOnPin(lMotorDir); myPWM.dutyCycle = 0; } else//Not backward or fastest { GPIO_setOutputLowOnPin(lMotorDir); myPWM.dutyCycle = (DUTY_NOMINAL + U); } //ONce past the slowest speed go backwards if(DUTY_NOMINAL - U > MAX){ GPIO_setOutputHighOnPin(rMotorDir); myPWMr.dutyCycle = MAX-((DUTY_NOMINAL - U) -MAX); } else if(DUTY_NOMINAL - U < 0)//Fastst speed { GPIO_setOutputLowOnPin(rMotorDir); myPWMr.dutyCycle = 0; } else { GPIO_setOutputLowOnPin(rMotorDir); myPWMr.dutyCycle = DUTY_NOMINAL - U; } Timer_A_outputPWM(TIMER_A0_BASE, &myPWM); Timer_A_outputPWM(TIMER_A0_BASE, &myPWMr); Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE); } void sensorOutput() { int32_t v = 0; int16_t count = -300; int8_t rand = 4; rand = 4; volatile uint8_t i; if (sensList == 11111111 && init == 1) { rand = countRand; } //go Left if(rand == 0) { for(i = 0; i < 8; i++) { if(i < 4) sensArr[i] = 1; else sensArr[i] = 0; } } //go Right else if(rand == 1) { for(i = 0; i < 8; i++) { if(i < 4) sensArr[i] = 0; else sensArr[i] = 1; } } //go Striaght else if (rand == 2) { for(i = 0; i < 8; i++) { if(i == 3 || i == 4) sensArr[i] = 1; else sensArr[i] = 0; } } init = 1; for( i =0 ; i < 8 ; i++) { switch(i) { case 0: count = -300; break; case 1: count = -200; break; case 2: count = -50; break; case 3: count = -25; break; case 4: count = 25; break; case 5: count = 50; break; case 6: count = 200;break; case 7: count = 300; break; } if(sensArr[i] == 1) { v += count; } } P = v; I = I + P; D = P - last_prop; last_prop = P; U = P*kp + I*ki + D*kd; if (U > MAX) U = MAX; if (U < (-1*MAX)) U = -1*MAX; if (sensList == 00011000) { myPWM.dutyCycle = DUTY_NOMINAL; myPWMr.dutyCycle = DUTY_NOMINAL; Timer_A_outputPWM(TIMER_A0_BASE, &myPWM); Timer_A_outputPWM(TIMER_A0_BASE, &myPWMr); Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE); } else if (sensList == 00000000) { GPIO_setOutputHighOnPin(lMotorDir); GPIO_setOutputHighOnPin(rMotorDir); } else turnRight(U); // myPWM.dutyCycle = (DUTY_NOMINAL + U); // myPWMr.dutyCycle = (DUTY_NOMINAL - U); // Timer_A_outputPWM(TIMER_A0_BASE, &myPWM); // Timer_A_outputPWM(TIMER_A0_BASE, &myPWMr); // Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE); } void TimerA1_Init(void(*task)(void), uint16_t period){ TimerA1Task = task; // user function TIMER_A1->CTL = 0x02C0; TIMER_A1->CCTL[0] = 0x0010; TIMER_A1->CCR[0] = (period - 1); // compare match value TIMER_A1->EX0 = 0x0007; // configure for input clock divider /6 // interrupts enabled in the main program after all devices initialized NVIC->IP[2] = (NVIC->IP[2]&0xFF00FFFF)|0x00400000; // priority 2 NVIC->ISER[0] = 0x00000400; // enable interrupt 10 in NVIC TIMER_A1->CTL |= 0x0014; // reset and start Timer A1 in up mode __enable_irq(); } void initPin() { GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P2, GPIO_PIN6, GPIO_PRIMARY_MODULE_FUNCTION); GPIO_setAsPeripheralModuleFunctionOutputPin(GPIO_PORT_P2, GPIO_PIN7, GPIO_PRIMARY_MODULE_FUNCTION); myPWM.clockSource = TIMER_A_CLOCKSOURCE_SMCLK; myPWM.clockSourceDivider = TIMER_A_CLOCKSOURCE_DIVIDER_1; myPWM.compareOutputMode = TIMER_A_OUTPUTMODE_SET_RESET; myPWM.compareRegister = TIMER_A_CAPTURECOMPARE_REGISTER_4; myPWM.timerPeriod = PERIOD; myPWM.dutyCycle = DUTY_NOMINAL; Timer_A_outputPWM(TIMER_A0_BASE, &myPWM); myPWMr.clockSource = TIMER_A_CLOCKSOURCE_SMCLK; myPWMr.clockSourceDivider = TIMER_A_CLOCKSOURCE_DIVIDER_1; myPWMr.compareOutputMode = TIMER_A_OUTPUTMODE_SET_RESET; myPWMr.compareRegister = TIMER_A_CAPTURECOMPARE_REGISTER_3; myPWMr.timerPeriod = PERIOD; myPWMr.dutyCycle = DUTY_NOMINAL; Timer_A_outputPWM(TIMER_A0_BASE, &myPWMr); GPIO_setAsInputPinWithPullUpResistor(bump5); GPIO_setAsInputPinWithPullUpResistor(bump4); GPIO_setAsInputPinWithPullUpResistor(bump3); GPIO_setAsInputPinWithPullUpResistor(bump2); GPIO_setAsInputPinWithPullUpResistor(bump1); GPIO_setAsInputPinWithPullUpResistor(bump0); GPIO_setAsOutputPin(sensODD); GPIO_setAsOutputPin(sensEVEN); GPIO_setAsOutputPin(RGBRED); GPIO_setAsOutputPin(RGBBLUE); GPIO_setAsOutputPin(RGBGREEN); GPIO_setAsOutputPin(frontBlinker); GPIO_setAsOutputPin(frontBlinkerLeft); GPIO_setAsOutputPin(backBlinkerLeft); GPIO_setAsOutputPin(backBlinkerRight); GPIO_setOutputLowOnPin(RGBRED); GPIO_setOutputLowOnPin(RGBBLUE); GPIO_setOutputLowOnPin(RGBGREEN); GPIO_setOutputLowOnPin(frontBlinker); GPIO_setOutputLowOnPin(frontBlinkerLeft); GPIO_setOutputLowOnPin(backBlinkerLeft); GPIO_setOutputLowOnPin(backBlinkerRight); GPIO_setAsOutputPin(lMotorDrive); GPIO_setAsOutputPin(rMotorDrive); GPIO_setAsOutputPin(lMotorDir); GPIO_setAsOutputPin(rMotorDir); GPIO_setOutputLowOnPin(lMotorDir); GPIO_setOutputHighOnPin(lMotorDrive); GPIO_setOutputHighOnPin(rMotorDrive); GPIO_setOutputLowOnPin(rMotorDir); Timer_A_stop(TIMER_A0_BASE); } void configTimerA(uint16_t delayValue, uint16_t clockDivider) { myTimer.clockSource = TIMER_A_CLOCKSOURCE_SMCLK; myTimer.clockSourceDivider = clockDivider; myTimer.timerPeriod = delayValue; myTimer.timerClear = TIMER_A_DO_CLEAR; } void myTimerADelay(uint16_t delayValue, uint16_t clockDivider) { configTimerA(delayValue, clockDivider); Timer_A_initUpMode(TIMER_A2_BASE, &myTimer); Timer_A_startCounter(TIMER_A2_BASE, TIMER_A_UP_MODE); while(TA2CTL&TAIFG == 0); Timer_A_stop(TIMER_A2_BASE); Timer_A_clearInterruptFlag(TIMER_A2_BASE); } void wait1us(uint16_t num) { volatile uint16_t i; for(i = 0; i < num; i++) myTimerADelay(1, TIMER_A_CLOCKSOURCE_DIVIDER_1); } void checkLine(uint8_t sensArr[8]) { volatile uint16_t ii; //Turn on IR LED GPIO_setOutputHighOnPin(sensEVEN); GPIO_setOutputHighOnPin(sensODD); //Set Reflectence to Output GPIO_setAsOutputPin(sens8); GPIO_setAsOutputPin(sens7); GPIO_setAsOutputPin(sens6); GPIO_setAsOutputPin(sens5); GPIO_setAsOutputPin(sens4); GPIO_setAsOutputPin(sens3); GPIO_setAsOutputPin(sens2); GPIO_setAsOutputPin(sens1); //Flash them GPIO_setOutputHighOnPin(sens8); GPIO_setOutputHighOnPin(sens7); GPIO_setOutputHighOnPin(sens6); GPIO_setOutputHighOnPin(sens5); GPIO_setOutputHighOnPin(sens4); GPIO_setOutputHighOnPin(sens3); GPIO_setOutputHighOnPin(sens2); GPIO_setOutputHighOnPin(sens1); wait1us(10); GPIO_setAsInputPin(sens8); GPIO_setAsInputPin(sens7); GPIO_setAsInputPin(sens6); GPIO_setAsInputPin(sens5); GPIO_setAsInputPin(sens4); GPIO_setAsInputPin(sens3); GPIO_setAsInputPin(sens2); GPIO_setAsInputPin(sens1); //1.5 ms wait1us(12); sensArr[7] = GPIO_getInputPinValue(sens8); sensArr[6] = GPIO_getInputPinValue(sens7); sensArr[5] = GPIO_getInputPinValue(sens6); sensArr[4] = GPIO_getInputPinValue(sens5); sensArr[3] = GPIO_getInputPinValue(sens4); sensArr[2] = GPIO_getInputPinValue(sens3); sensArr[1] = GPIO_getInputPinValue(sens2); sensArr[0] = GPIO_getInputPinValue(sens1); sensList = 0x00; for (ii =0 ; ii < 8; ii++) { if(sensArr[ii] == 1) { switch(ii) { case 0: sensList += BIT7; break; case 1: sensList += BIT6; break; case 2: sensList += BIT5; break; case 3: sensList += BIT4; break; case 4: sensList += BIT3; break; case 5: sensList += BIT2; break; case 6: sensList += BIT1; break; case 7: sensList += BIT0; break; } } } GPIO_setOutputLowOnPin(sensEVEN); GPIO_setOutputLowOnPin(sensODD); }