void gpio_config()
{
//Switches
MAP_GPIO_setAsInputPinWithPullUpResistor(GPIO_PORT_P1, GPIO_PIN1); // S1
MAP_GPIO_setAsInputPinWithPullUpResistor(GPIO_PORT_P1, GPIO_PIN4); // S2
//Set Output Pins for Motor Driver
MAP_GPIO_setAsOutputPin(GPIO_PORT_P6,GPIO_PIN6); // Output to H-Bridge - P6.6 / IN1
MAP_GPIO_setAsOutputPin(GPIO_PORT_P2,GPIO_PIN5); // Output to H-Bridge - P2.5 / IN2
MAP_GPIO_setAsOutputPin(GPIO_PORT_P2,GPIO_PIN6); // Output to H-Bridge - P2.6 / IN3
MAP_GPIO_setAsOutputPin(GPIO_PORT_P2,GPIO_PIN7); // Output to H-Bridge - P2.7 / IN4
MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P2, GPIO_PIN5); // LEFT MOTOR
MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P6, GPIO_PIN6);
MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P2, GPIO_PIN7); // RIGHT MOTOR
MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P2, GPIO_PIN6);
//Sensor Input PIns
MAP_GPIO_setAsInputPin(GPIO_PORT_P5, GPIO_PIN0); //P5.0 - L
MAP_GPIO_setAsInputPin(GPIO_PORT_P5, GPIO_PIN2); //P5.2
MAP_GPIO_setAsInputPin(GPIO_PORT_P3, GPIO_PIN6); //P3.6
MAP_GPIO_setAsInputPin(GPIO_PORT_P6, GPIO_PIN7); //P6.7 - C
MAP_GPIO_setAsInputPin(GPIO_PORT_P2, GPIO_PIN3); //P2.3 - C
MAP_GPIO_setAsInputPin(GPIO_PORT_P5, GPIO_PIN1); //P5.1
MAP_GPIO_setAsInputPin(GPIO_PORT_P3, GPIO_PIN5); //P3.5
MAP_GPIO_setAsInputPin(GPIO_PORT_P3, GPIO_PIN7); //P3.7 - R / Blue
//Encoder Pins
MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P4, GPIO_PIN1, GPIO_PRIMARY_MODULE_FUNCTION); //Output A1
MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P4, GPIO_PIN6, GPIO_PRIMARY_MODULE_FUNCTION); //Output B1
MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P3, GPIO_PIN0, GPIO_PRIMARY_MODULE_FUNCTION); //Output A2
MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P5, GPIO_PIN7, GPIO_PRIMARY_MODULE_FUNCTION); //Output B2
}
void clock_config()
{
//Set Clock
unsigned int dcoFrequency = 3E+6;
MAP_CS_setDCOFrequency(dcoFrequency); // Set DCO clock source frequency
MAP_CS_initClockSignal(CS_SMCLK, CS_DCOCLK_SELECT, CS_CLOCK_DIVIDER_1); // Tie SMCLK to DCO
MAP_Timer_A_configureUpMode(TIMER_A0_BASE,&upConfig_0); // Configure Timer A using above struct
MAP_Interrupt_enableInterrupt(INT_TA0_0); // Enable Timer A interrupt
}
/*
void UART_config()
{
// UART configuration
MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P1, GPIO_PIN2 | GPIO_PIN3, GPIO_PRIMARY_MODULE_FUNCTION);
MAP_UART_initModule(EUSCI_A0_BASE, &uartConfig);
MAP_UART_enableModule(EUSCI_A0_BASE);
MAP_UART_clearInterruptFlag (EUSCI_A0_BASE, EUSCI_A_UART_RECEIVE_INTERRUPT);
MAP_UART_enableInterrupt(EUSCI_A0_BASE, EUSCI_A_UART_RECEIVE_INTERRUPT);
MAP_Interrupt_enableInterrupt(INT_EUSCIA0);
MAP_Interrupt_enableSleepOnIsrExit();
MAP_Interrupt_enableMaster();
}
*/
void main(void)
{
// Stop watchdog timer
WDTCTL = WDTPW | WDTHOLD;
//UART_config();
clock_config();
led_config();
pwm_config();
gpio_config();
// Set Timer A period (PWM signal period)
TA2CCR0 = 16000 ;
TA0CCR0 = 16000 ;
// TA0CCR1 = 8000; //Left Motor full speed - 50% Duty Cycle
// TA2CCR1 = 8000; //Right Motor full speed
/*
// Set Duty cycle to 0 for calibration
TA2CCR1 = 0 ;
TA0CCR1 = 0 ;
// Set output mode to Reset/Set
TA2CCTL1 = OUTMOD_7 ;
TA0CCTL1 = OUTMOD_7 ;
// Initialize Timer A
TA2CTL = TASSEL__SMCLK | MC__UP | TACLR ;
TA0CTL = TASSEL__SMCLK | MC__UP | TACLR ;
// Initializing TimerA
MAP_Timer_A_configureUpMode(TIMER_A0_BASE, &upConfig_0);
MAP_Interrupt_enableInterrupt(INT_TA0_0);
*/
//start timer
MAP_Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_UP_MODE);
while (1)
{
for(i=0; i<=5000; i++){}; //Calibrate Sensors
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P6, GPIO_PIN7) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P2, GPIO_PIN3)){
//00011000
goStraight = 1;
s6 = 1;
w6 = 0;
}
else {s6 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN0)) {
//10000000
goStraight = 0;
s2 = 1;
w2 = -80;
pastLine1 = 1;
pastLine2 = 0;
}
else {s2 = 0; w2 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN0) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN2)){
//11000000
goStraight = 0;
s3 = 1;
w3 = -60;
}
else {s3 = 0; w3 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN2) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN6)){
//01100000
goStraight = 0;
s4 = 1;
w4 = -40;
}
else {s4 = 0; w4 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN6) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P6, GPIO_PIN7)){
//00110000
goStraight = 0;
s5 = 1;
w5 = -20;
}
else {s5 = 0; w5 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P2, GPIO_PIN3) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN1)){
//00001100
goStraight = 0;
s7 = 1;
w7 = 20;
}
else {s7 = 0; w7 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN1) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN5)){
//00000110
goStraight = 0;
s8 = 1;
w8 = 40;
}
else {s8 = 0; w8 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN5) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN7)){
//00000011
goStraight = 0;
s9 = 1;
w9 = 60;
}
else {s9 = 0; w9 = 0;}
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN7)){
//00000001
goStraight = 0;
s10 = 1;
w10 = 80;
pastLine1 = 0;
pastLine2 = 1;
}
else {s10 = 0; w10 = 0;}
if(GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN7) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN5) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN1) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P2, GPIO_PIN3) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P6, GPIO_PIN7) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN6) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN2) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN0) && pastLine1 == 1){
//00000000 -->1 Line on far right
goStraight = 0;
s11 = 1;
w11 = 100;
}
else {s11 = 0; w11 = 0;}
if(GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN7) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN5) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN1) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P2, GPIO_PIN3) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P6, GPIO_PIN7) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P3, GPIO_PIN6) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN2) && GPIO_INPUT_PIN_LOW == MAP_GPIO_getInputPinValue(GPIO_PORT_P5, GPIO_PIN0) && pastLine2 == 1){
//1 <-- 00000000 Line on far left
goStraight = 0;
s1 = 1;
w1 = -100;
}
else {s1 = 0; w1 = 0;}
//PID
if(GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P6, GPIO_PIN7) && GPIO_INPUT_PIN_HIGH == MAP_GPIO_getInputPinValue(GPIO_PORT_P2, GPIO_PIN3)){
iErr = iErr + err;
}
else{iErr = 0;}
//CONTROL COMPUTATION
totalSensors = s1 + s2 + s3 + s4 + s5 + s6 + s7 + s8 + s9 + s10 + s11;
sumRead = w1 + w2 + w3 + w4 + w5 + w6 + w7 + w8 + w9 + w10 + w11;
//AVERAGE OF READINGS
err = center - (sumRead/totalSensors);
if(err < 0){
goRight =1;
}else if(err>0){
goRight =0;
}
dErr = err - lastErr;
lastErr = err;
control = kp * err + kd * lastErr + ki * iErr;
if(goStraight == 0)
{
if(goRight == 1)
{
TA0CCR1 = control*-1; //Left Motor speed up relative to the error amount
TA2CCR1 = 0;
}
else if(goRight == 0){
TA0CCR1 = 0;
TA2CCR1 = control; //Right Motor speed up relative to the error amount
}
}
else if(goStraight == 1){
TA0CCR1 = 1500; //Left Motor full speed - 50% Duty Cycle
TA2CCR1 = 1500; //Right Motor full speed
}
}
}