Subversion Repositories Code-Repo

// <editor-fold defaultstate="collapsed" desc="Configuration Bits">
// PIC16F1825 Configuration Bit Settings

// CONFIG1
#pragma config FOSC = INTOSC    // Oscillator Selection (INTOSC oscillator: I/O function on CLKIN pin)
#pragma config WDTE = OFF       // Watchdog Timer Enable (WDT enabled)
#pragma config PWRTE = OFF      // Power-up Timer Enable (PWRT disabled)
#pragma config MCLRE = OFF      // MCLR Pin Function Select (MCLR/VPP pin function is digital input)
#pragma config CP = OFF         // Flash Program Memory Code Protection (Program memory code protection is disabled)
#pragma config CPD = OFF        // Data Memory Code Protection (Data memory code protection is disabled)
#pragma config BOREN = ON       // Brown-out Reset Enable (Brown-out Reset enabled)
#pragma config CLKOUTEN = OFF   // Clock Out Enable (CLKOUT function is disabled. I/O or oscillator function on the CLKOUT pin)
#pragma config IESO = ON        // Internal/External Switchover (Internal/External Switchover mode is enabled)
#pragma config FCMEN = ON       // Fail-Safe Clock Monitor Enable (Fail-Safe Clock Monitor is enabled)

// CONFIG2
#pragma config WRT = OFF        // Flash Memory Self-Write Protection (Write protection off)
#pragma config PLLEN = ON       // PLL Enable (4x PLL enabled)
#pragma config STVREN = ON      // Stack Overflow/Underflow Reset Enable (Stack Overflow or Underflow will cause a Reset)
#pragma config BORV = LO        // Brown-out Reset Voltage Selection (Brown-out Reset Voltage (Vbor), low trip point selected.)
#pragma config LVP = OFF        // Low-Voltage Programming Enable (High-voltage on MCLR/VPP must be used for programming)
// </editor-fold>

#include "defines.h"
#include "INTERRUPTS.h"
#include "PWM.h"
#include "I2C1.h"
#include "UART.h"
#include "CONTROLLER.h"

void Pins_Init(void) {
    // Set all pins to digital I/O
    ANSELA = 0x0;
    ANSELC = 0x0;

//    // Enable weak pull-up if WPU bit is set
//    OPTION_REGbits.nWPUEN = 0;

    // CCP2 on RC3
    APFCON1bits.CCP2SEL = 0;
    
    // TX/CK function on RA0
    APFCON0bits.TXCKSEL = 1;
    // RX/DT function on RA1
    APFCON0bits.RXDTSEL = 1;

    LED_1_LAT  = 1;
    LED_1_TRIS = 0;
    LED_2_LAT  = 0;
    LED_2_TRIS = 0;

    CCP_1_LAT  = 0;
    CCP_1_TRIS = 0;
    CCP_2_LAT  = 0;
    CCP_2_TRIS = 0;
    
}

void LED_1_On(void) {
    LED_1_LAT = 0;
}

void LED_1_Off(void) {
    LED_1_LAT = 1;
}

void LED_2_On(void) {
    LED_2_LAT = 1;
}

void LED_2_Off(void) {
    LED_2_LAT = 0;
}

int main(void) {
    // Set internal oscillator speed to 32MHz
    OSCCONbits.SPLLEN = 1;  // 4x PLL enable (overwritten by config bits)
    OSCCONbits.IRCF = 0xE;  // Base frequency @ 8MHz
    OSCCONbits.SCS = 0b00;  // System clock determined by config bits

    // Initialize I/O
    Pins_Init();

    // Initialize PWM
    PWM_1_Init();
    PWM_2_Init();

    // Initialize I2C
    I2C1_DATA i2c_data;
    I2C1_Init(&i2c_data);
    I2C1_Configure_Master(I2C_100KHZ);

    UART_DATA uart_data;
    UART_Init(&uart_data);

    Controller_Init();

    // Initialize interrupts
    Interrupt_Init();
    Interrupt_Enable();

    PWM_1_Set(PWM_NOMINAL);
    PWM_2_Set(PWM_NOMINAL);

    LED_1_On();
    LED_2_On();

    __delay_ms(1000);

    LED_1_Off();
    LED_2_Off();

    CTRL_BTN_STATUS ctrl;
    Controller_Read(&ctrl);

    uint16_t pwm_left = PWM_NOMINAL;
    uint16_t pwm_right = PWM_NOMINAL;

    uint8_t median;
    int16_t L_X, L_Y, R_X, R_Y;
    
    uint8_t buffer[32];
    uint8_t length;
    uint8_t uart_state = UART_STATE_READ_CMD;
    uint8_t uart_cmd;

    while (1) {
        if (!Controller_Read(&ctrl)) {
            median = ctrl.REF / 2;
            L_X = (int16_t)median - (int16_t)ctrl.L_X_CH;
            L_Y = (int16_t)median - (int16_t)ctrl.L_Y_CH;
            R_X = (int16_t)median - (int16_t)ctrl.R_X_CH;
            R_Y = (int16_t)median - (int16_t)ctrl.R_Y_CH;

            // Left stick = throttle, Right stick = steering
    //        if (L_X > -5 && L_X < 5 &&
    //                R_Y > -5 && R_Y < 5) {
    //            pwm_left = PWM_NOMINAL;
    //            pwm_right = PWM_NOMINAL;
    //        } else {
    //            pwm_left = PWM_NOMINAL + (L_X * 4);
    //            pwm_right = PWM_NOMINAL + (L_X * 4);
    //
    //            pwm_left += R_Y * 4;
    //            pwm_right -= R_Y * 4;
    //        }

            // Left stick = left control, Right stick = right control
            if (L_X > -5 && L_X < 5 &&
                    R_X > -5 && R_X < 5) {
                pwm_left = PWM_NOMINAL;
                pwm_right = PWM_NOMINAL;
            } else {
                pwm_left = PWM_NOMINAL + (R_X * 4);
                pwm_right = PWM_NOMINAL + (L_X * 4);
            }

            if (L_X < -5 || L_X > 5) LED_2_On();
            else LED_2_Off();

            if (R_X < -5 || R_X > 5) LED_1_On();
            else LED_1_Off();


            if (pwm_left > PWM_MAX)
                pwm_left = PWM_MAX;
            if (pwm_left < PWM_MIN)
                pwm_left = PWM_MIN;

            if (pwm_right > PWM_MAX)
                pwm_right = PWM_MAX;
            if (pwm_right < PWM_MIN)
                pwm_right = PWM_MIN;

            PWM_1_Set(pwm_left);
            PWM_2_Set(pwm_right);
        } else {
            PWM_1_Set(PWM_NOMINAL);
            PWM_2_Set(PWM_NOMINAL);

            LED_1_Off();
            LED_2_Off();

            // If no controller is attached, switch control to UART
            while(1) {
                length = UART_Read(buffer);
                for (uint8_t i = 0; i < length; i++) {
                    if (uart_state == UART_STATE_READ_CMD) {
                        if (buffer[i] == UART_CMD_RESET) {
                            RESET();
                        } else {
                            // Read and save first byte (command)
                            uart_cmd = buffer[i];
                            uart_state = UART_STATE_READ_DATA;
                        }
                    } else if (uart_state == UART_STATE_READ_DATA) {
                        uart_state = UART_STATE_READ_CMD;

                        // Process received data
                        if (uart_cmd == UART_CMD_LEFT_FORWARD) {
                            pwm_left = PWM_NOMINAL + (uint16_t) (buffer[i] * 2);
                            if (buffer[i] != 0) LED_2_On();
                            else LED_2_Off();
                        } else if (uart_cmd == UART_CMD_LEFT_BACKWARD) {
                            pwm_left = PWM_NOMINAL - (uint16_t) (buffer[i] * 2);
                            if (buffer[i] != 0) LED_2_On();
                            else LED_2_Off();
                        } else if (uart_cmd == UART_CMD_RIGHT_FORWARD) {
                            pwm_right = PWM_NOMINAL + (uint16_t) (buffer[i] * 2);
                            if (buffer[i] != 0) LED_1_On();
                            else LED_1_Off();
                        } else if (uart_cmd == UART_CMD_RIGHT_BACKWARD) {
                            pwm_right = PWM_NOMINAL - (uint16_t) (buffer[i] * 2);
                            if (buffer[i] != 0) LED_1_On();
                            else LED_1_Off();
                        }

                        if (pwm_left > PWM_MAX)
                            pwm_left = PWM_MAX;
                        if (pwm_left < PWM_MIN)
                            pwm_left = PWM_MIN;

                        if (pwm_right > PWM_MAX)
                            pwm_right = PWM_MAX;
                        if (pwm_right < PWM_MIN)
                            pwm_right = PWM_MIN;

                        PWM_1_Set(pwm_right);
                        PWM_2_Set(pwm_left);
                    }
                }
            }
        }
    }
}