/* --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 <ti/devices/msp432p4xx/driverlib/driverlib.h>
/* Standard Includes */
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
//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;
volatile uint8_t i;
if (sensList == 11111111 && init == 1)
{
}
//go Left
{
for(i = 0; i < 8; i++)
{
if(i < 4)
sensArr[i] = 1;
else
sensArr[i] = 0;
}
}
//go Right
{
for(i = 0; i < 8; i++)
{
if(i < 4)
sensArr[i] = 0;
else
sensArr[i] = 1;
}
}
//go Striaght
{
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);
}
