#include "picsys18.h"

void
setup()
{
    pinMode(LED8, OUTPUT) ;
    pinMode(LED9, OUTPUT) ;

    pinMode(CN7, OUT_SERVO1) ;
    pinMode(CN8, OUT_SERVO2) ;

    SEG7_initDefault() ;

    USB_begin() ;
}


int cIndex = 0 ;
char buff[50] ;
BOOL eol = FALSE ;

BYTE servoIndex[] = {NULL, CN7, CN8} ;

char
posParam(void)
{
    int i ;
    for (i = 1 ; buff[i]; i++) {
        if (buff[i] == ':') {
            return i+1 ;
        }
    }

    return 0 ;	
}	

void
loop()
{
    int c, i, j, v ;

    if (USB_attached()) {
        digitalWrite(LED9, HIGH) ;

        // １行分の文字の読み込み
        while (USB_available()) {
            c = USB_getc() ;
            USB_putc(c) ;
            if (c == '\r') {
                USB_putc('\n') ;
                buff[cIndex] = '\0' ;
                eol = TRUE ;
            } else {
                buff[cIndex++] = c ;
                eol = FALSE ;
            }					
        }		

        // CRが入力されたら処理
        if (eol) {
            switch (buff[0]) {
                case 'v':
                case 'V':
                    i = atoi(&buff[1]) ;
                    if ((j = posParam()) == 0) {
                        // 引数がない場合には、現在の回転値を返す
                        v = digitalRead(servoIndex[i]) ;
                        USB_puti(v) ;
                    } else {
                        // 引数がある場合には、それを設定
                        v = atoi(&buff[j]) ;
                        digitalWrite(servoIndex[i], v) ;
                    }
                    SEG7_write(v, SEG7_INT) ;
                    break ;
			}

            USB_putrs("\r\n") ;
            cIndex = 0 ;
            eol = FALSE ;
        }
    } else {
        digitalWrite(LED9, HIGH) ;
    }

    digitalWrite(LED8, ((int)millis()) & 0x200) ;
}
