/*
 * File:   rpm_control.c
 * Author: corrado
 *
 * Created on January 10, 2016, 6:52 PM
 */

#include <xc.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <string.h>

void setup_uart(void)
{
    // configure UART
    // setup UART
    TRISCbits.TRISC6 = 0;   // TX as output
    TRISCbits.TRISC7 = 1;   // RX as input

    TXSTA1bits.SYNC = 0; // Async operation
    TXSTA1bits.TX9 = 0; // No tx of 9th bit
    TXSTA1bits.TXEN = 1; // Enable transmitter
    RCSTA1bits.RC9 = 0; // No rx of 9th bit
    // Setting for 19200 BPS
    BAUDCON1bits.BRG16 = 0; // Divisor at 8 bit
    TXSTA1bits.BRGH = 0; // No high-speed baudrate
    SPBRG1 = 51; // divisor value for 19200

    RCSTA1bits.CREN = 1; // Enable receiver
    RCSTA1bits.SPEN = 1; // Enable serial port

    PIE1bits.TX1IE = 0; // disable TX hardware interrupt
    PIE1bits.RC1IE = 0; // disable RX hardware interrupt

}

void putch(char c) {
    // wait the end of transmission
    while (TXSTA1bits.TRMT == 0) {};
    TXREG1 = c; // send the new byte
}

void setup_adc(void) {
    ANSELAbits.ANSA0 = 1; // RA0 = analog input --> AN0
    ADCON1bits.PVCFG = 0b00; // Positive reference = +VDD
    ADCON1bits.NVCFG = 0b00; // Negative reference = GND
    ADCON2bits.ADFM = 1; // format = right justified
    ADCON2bits.ACQT = 0b111; // acquisition time = 20*TAD
    ADCON2bits.ADCS = 0b110; // conversion clock = FOSC/64
    ADCON0bits.CHS = 0; // select channel 0
    ADCON0bits.ADON = 1; // turn on ADC
}

void setup_capture(void)
{
    T1CONbits.TMR1CS = 0b00; // FOSC/4
    T1CONbits.T1CKPS = 0b11; // prescaler 1:8
    T1GCONbits.TMR1GE = 0; // not gated
    TMR1L = 0; TMR1H = 0; // clear timer

    CCP1CONbits.CCP1M = 0b0101; // capture every rising
    CCPTMRS0bits.C1TSEL = 0b00; // CCP1 uses timer1
    TRISCbits.TRISC2 = 1; // RC2/CCP1 pin set as input

    PIR1bits.CCP1IF = 0;        // clear interrupt flag
    PIE1bits.CCP1IE = 1;        // enable interrupt flag

    T1CONbits.TMR1ON = 1; // turn on timer
}

void setup_pwm(void)
{
    // setup timer 2
    T2CONbits.T2CKPS = 0b01; // prescaler 1:4
    T2CONbits.T2OUTPS = 0b1001;   // postscaler division by 10
    PR2 = 255;
    TMR2 = 0;
    // With these setting, the PR2 value correspond to 64 microsecs.
    // Using a postscaler with 1:10, we will have an TMR2 interrupt
    // each 640 microsecs
    PIR1bits.TMR2IF = 0;
    PIE1bits.TMR2IE = 1; // enable TMR2 interrupt

    // setup CCP2
    CCP2CONbits.CCP2M = 0b1100; // PWM
    TRISCbits.TRISC1 = 0;       // set CCP2 as output
    CCPTMRS0bits.C2TSEL = 0b00; // select TIMER2
    
    // turn on timer
    T2CONbits.TMR2ON = 1;
}

void set_duty_cycle(int duty)
{
    CCP2CONbits.DC2B = duty & 3;
    CCPR2 = duty >> 2;
}

unsigned int last_capture, current_capture, current_int_period = 0;
unsigned char timer_2_ticks, capture_flag;

void interrupt isr(void)
{
    if (PIR1bits.CCP1IF == 1) {
        // capture event
        current_capture = CCPR1;
        current_int_period = current_capture - last_capture;
        last_capture = current_capture;
        capture_flag = 1;
        PIR1bits.CCP1IF = 0;        // clear interrupt flag
    }
    if (PIR1bits.TMR2IF == 1) {
        ++timer_2_ticks;
        PIR1bits.TMR2IF = 0;
    }
}

float evaluate_speed(void)
{
    float real_period, motor_rev_per_seconds, gear_rev_per_seconds;

    if (current_int_period != 0) {
        real_period = (float)current_int_period * 0.5e-6;

        // the encoder has 3 ticks per motor revolution
        motor_rev_per_seconds = 1.0 / (3.0 * real_period);
        
        // multiply by gearbox ration
        gear_rev_per_seconds = motor_rev_per_seconds / 76.84;

        return gear_rev_per_seconds * 60; // convert to revolution per minute
    }
    else
        return 0;
}


void main(void) {

    float current_speed, motor_drive, target_speed = 0;

    setup_uart();
    setup_capture();
    setup_adc();
    setup_pwm();

    INTCONbits.GIE = 1; // global enable interrupts
    INTCONbits.PEIE = 1; // peripheral enable interrupts
    RCONbits.IPEN = 0; // no priorities

    ADCON0bits.GODONE = 1;
    for (;;) {
        int duty;
        if (timer_2_ticks == 100) {
            // this loop will execute each 0.064 secs --> 64 millisecs
            timer_2_ticks = 0;

            if (ADCON0bits.GODONE == 0) {
                // conversion completed

                // evaluate current speed
                if (capture_flag == 0)
                    current_speed = 0;
                else
                    current_speed = evaluate_speed();
                capture_flag = 0;

                // get target speed
                target_speed = ADRES * 82.0 / 1023.0;
                // retrigger ADC conversion
                ADCON0bits.GODONE = 1;

#define K 1.5
                // control algorithm
                motor_drive += (target_speed - current_speed) * K;
                // saturate
                if (motor_drive < 0) motor_drive = 0;
                if (motor_drive > 1023) motor_drive = 1023;

                duty = (int)motor_drive;
                set_duty_cycle(duty);
            }
            printf("DUTY = %d, Target Speed = %f, Current Speed = %f\n\r",
                    duty, target_speed, current_speed);
            
        }
    }
}