seeker.c

// **dsPIC30F2012**

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

// ----- config -----
#pragma config FOSFPR = FRC_PLL16       // Oscillator (FRC w/PLL 16x)
#pragma config FCKSMEN = CSW_FSCM_OFF   // Clock Switching and Monitor (Sw Disabled, Mon Disabled)
#pragma config FWPSB = WDTPSB_16        // WDT Prescaler B (1:16)
#pragma config FWPSA = WDTPSA_512       // WDT Prescaler A (1:512)
#pragma config WDT = WDT_OFF            // Watchdog Timer (Disabled)
#pragma config FPWRT = PWRT_64          // POR Timer Value (64ms)
#pragma config BODENV = BORV20          // Brown Out Voltage (Reserved)
#pragma config BOREN = PBOR_OFF         // PBOR Enable (Disabled)
#pragma config MCLRE = MCLR_EN          // Master Clear Enable (Enabled)
#pragma config GWRP = GWRP_OFF          // General Code Segment Write Protect (Disabled)
#pragma config GCP = CODE_PROT_OFF      // General Segment Code Protection (Disabled)
#pragma config ICS = ICS_PGD            // Comm Channel Select (Use PGC/EMUC and PGD/EMUD)


// ----- device -----
#define LED_STATE  (LATFbits.LATF4)
#define GAIN_CTRL_0 (TRISBbits.TRISB4)
#define GAIN_CTRL_1 (TRISBbits.TRISB5)
#define GAIN_CTRL_2 (TRISCbits.TRISC13)
#define GAIN_CTRL_3 (TRISCbits.TRISC14)
#define GAIN_CTRL_0_LAT (LATBbits.LATB4)
#define GAIN_CTRL_1_LAT (LATBbits.LATB5)
#define GAIN_CTRL_2_LAT (LATCbits.LATC13)
#define GAIN_CTRL_3_LAT (LATCbits.LATC13)
#define PEAKHOLD_FLUSH (LATCbits.LATC15)
#define SYS_FREQ (29480000)


// ----- util -----

inline int usec_to_PR(int t) {
    return ((unsigned int) ((double) (t) * 29.48 - 1.0));
}

inline int compress(int x, int min, int max) {
    if (max < x) return max;
    if (x < min) return min;
    return x;
}


// ----- init -----

void GPIO_init(void) {
    LATB = LATC = LATD = LATF = 0;
    TRISB = 0b0000000000001111;
    TRISC = 0b0000000000000000;
    TRISD = 0b0000000000000000;
    TRISF = 0b0000000000000111;
}

void ADC_init(void) {
    ADCON1 = 0b0000000011100100;
    ADCON2 = 0b0000010000001100;
    ADCON3 = 0b000010100011111;
    ADCHS = 0;
    ADPCFG = 0b1111111111110000;
    ADCSSL = 0b0000000000001111;
    ADCON1bits.ADON = 1;
}

void Timer1_init(void) {
    T1CON = 0;
    TMR1 = 0;
    T1CONbits.TCKPS = 0b10;
    PR1 = usec_to_PR(10000 / 64);
    IFS0bits.T1IF = 0;
    IEC0bits.T1IE = 1;
    IPC0bits.T1IP = 3;
    T1CONbits.TON = 1;
}

void Timer2_init(void) {
    T2CON = 0;
    TMR2 = 0;
    T2CONbits.TCKPS = 0b10;
    PR2 = usec_to_PR(20000 / 64);
    IFS0bits.T2IF = 0;
    IEC0bits.T2IE = 1;
    IPC1bits.T2IP = 5;
    T2CONbits.TON = 1;
}

void UART1_init(void) {
    U1STA = 0b0000010000000000;
    U1BRG = 15; //(7370000 * 4 / (16 * 115200)) - 1;
    U1MODE = 0;
    U1MODEbits.UARTEN = 1;
}

void OC_init(void) {
    OC1R = OC2R = 0;
    OC1RS = OC2RS = 0;
    OC1CON = OC2CON = 0b110;
}

void init(void) {
    GPIO_init();
    ADC_init();
    UART1_init();
    Timer1_init();
    Timer2_init();
    OC_init();
}


// ----- servo control -----

const int deg_speed_limit = 2;
const int deg_min = 10;
const int deg_max = 170;
int verrical_deg = 90;
int horizontal_deg = 90;

const unsigned int deg_PR_table[] = {
    230, 234, 239, 244, 249, 254, 259, 263, 268, 274, 278, 283, 288, 293, 298, 303, 307, 312,
    317, 322, 327, 332, 337, 341, 346, 351, 356, 361, 366, 371, 375, 380, 385, 390, 395, 400,
    405, 409, 415, 419, 424, 429, 434, 438, 444, 449, 453, 458, 463, 468, 473, 478, 482, 487,
    492, 497, 502, 507, 512, 516, 521, 526, 531, 536, 541, 546, 550, 555, 560, 565, 570, 575,
    580, 584, 590, 594, 599, 604, 609, 613, 619, 624, 628, 633, 638, 643, 648, 653, 657, 662,
    667, 672, 677, 682, 687, 691, 696, 701, 706, 711, 716, 721, 725, 730, 735, 740, 745, 750,
    755, 760, 765, 769, 774, 779, 784, 789, 794, 799, 803, 808, 813, 818, 823, 828, 832, 837,
    842, 847, 852, 857, 862, 866, 871, 876, 881, 886, 891, 896, 900, 906, 910, 915, 920, 925,
    930, 935, 940, 944, 949, 954, 959, 964, 969, 974, 978, 983, 988, 993, 998, 1003, 1007, 1012,
    1017, 1022, 1027, 1032, 1037, 1041, 1046, 1051, 1056, 1061, 1066, 1071, 1076, 1081, 1085, 1090, 1095, 1100
};

inline void servo1_set(int deg) {
    OC1RS = deg_PR_table[deg];
}

inline void servo2_set(int deg) {
    OC2RS = deg_PR_table[deg];
}


// ----- sensor -----
// sensor 0: 右上(対面から見て)
// sensor 1: 左上
// sensor 2: 右下
// sensor 3: 左下
// error : 右と上が正

const int cut_off_border = 100;
int sensor[4];

int error_vertical = 0;
int error_horizontal = 0;
int is_detecting = 0;

inline int sensor_value_cut_off(int x) {
    if (x < cut_off_border) return 0;
    return x;
}

inline void sensor_scan_routine(void) {
    for (int i = 0; i < 4; i++) sensor[i] = sensor_value_cut_off(*(&ADCBUF0 + i));
}

void calc_error(void) {
    int s[4] = {sensor[0] != 0, sensor[1] != 0, sensor[2] != 0, sensor[3] != 0};
    int v = s[0] + s[1] - s[2] - s[3];
    int h = s[0] - s[1] + s[2] - s[3];
    error_vertical = v;
    error_horizontal = h;
    is_detecting = 0;
    for (int i = 0; i < 4; i++)if (sensor[i] != 0)is_detecting = 1;
}


// ----- interrupt -----

void __attribute__((interrupt, auto_psv)) _T1Interrupt(void) {
    static int sensor_phase = 0;
    IFS0bits.T1IF = 0;
    switch (sensor_phase) {
        case 0:
            sensor_scan_routine();
            PEAKHOLD_FLUSH = 1;
            sensor_phase = 1;
            break;
        default:
            PEAKHOLD_FLUSH = 0;
            sensor_phase = 0;
    }
}

volatile int flag = 0;

void __attribute__((interrupt, auto_psv)) _T2Interrupt(void) {
    IFS0bits.T2IF = 0;
    flag++;
}


// ----- main dish -----

inline int to_center_limit(int deg) {
    if (deg < 90) {
        if (90 < deg + deg_speed_limit)return 90;
        return deg + deg_speed_limit;
    } else {
        if (deg - deg_speed_limit < 90)return 90;
        return deg - deg_speed_limit;
    }
}

void seeking_routine(void) {
    const int servo_speed = 2;
    calc_error();
    LED_STATE = !is_detecting;
    if (is_detecting) {
        horizontal_deg = compress(horizontal_deg + error_horizontal * servo_speed, deg_min, deg_max);
        verrical_deg = compress(verrical_deg + error_vertical * servo_speed, deg_min, deg_max);
    } else {
        verrical_deg = to_center_limit(verrical_deg);
        horizontal_deg = to_center_limit(horizontal_deg);
    }
    servo1_set(horizontal_deg);
    servo2_set(verrical_deg);
}

void debug(void) {
    calc_error();
    //printf("h:%d , v:%d\n", error_horizontal, error_vertical);
    printf("%d , %d , %d, %d\n", sensor[0], sensor[1], sensor[2], sensor[3]);
    servo1_set(90);
    servo2_set(90);
}

int main(void) {
    init();
    while (1) {
        if (flag) {
            seeking_routine();
            flag = 0;
        }
        //if (10 < flag) {
        //  debug();
        //  flag = 0;
        //}
    }
    return 0;
}