#include "mbed.h"

//DigitalOut myled(LED2);
//PwmOut led(LED1);

//TA8440H 接続ピンの設定
DigitalOut motor_sw1(p5);
DigitalOut motor_dir1(p6);
DigitalOut motor_sw2(p7);
DigitalOut motor_dir2(p8);

PwmOut motor_pwm1(p21);
PwmOut motor_pwm2(p22);

//ＤＩＰリレー 接続ピンの設定
DigitalOut power_sw(p30);

//ＰＳＤセンサ接続ピンの設定
AnalogIn psd_F(p15);
AnalogIn psd_R(p16);
AnalogIn psd_L(p17);

//車輪エンコーダ（フォトインタラプタ）接続ピンの設定
DigitalIn wheel_R(p11);
DigitalIn wheel_L(p12);

//シリアル通信の設定
Serial pc(USBTX, USBRX); // tx, rx 9600bpsデフォルト

int main() {

    unsigned char command, W_R, W_L;
    unsigned int count_R, count_L;
    float psd_volt;


    pc.printf("Control program for w6v4a robot mobile unit.\r\n");

    motor_pwm1.period_ms(10);
    motor_pwm1 = 0.3;
    motor_dir1 = 0;
    motor_sw1 = 0;

    motor_pwm2.period_ms(10);
    motor_pwm2 = 0.3;
    motor_dir2 = 0;
    motor_sw2 = 0;

    //Switch the DIP relay for PSD & rotaly encoder.
    power_sw = 1;

    W_R = 0;
    W_L = 0;
    count_R = 0;
    count_L = 0;

    while (1) {

	//コマンド受信
        command = 0;
        if (pc.readable()) command = pc.getc();

	//車輪エンコーダ読取
        if (W_R !=  wheel_R) {
            W_R = wheel_R;
            count_R++;
        }
        if (W_L !=  wheel_L) {
            W_L = wheel_L;
            count_L++;
        }

	//「５」プレス若しくは車輪１回転分でモーター停止
        if (command == '5' || count_R >= 24|| count_L >= 24) {
            motor_sw1 = 0;
            motor_sw2 = 0;

            count_R = 0;
            count_L = 0;
        }

	//「８」前進、「２」後進、「４」「６」左右旋回
        if (command == '8') {
            motor_dir1 = 0;
            motor_dir2 = 0;
            motor_sw1 = 1;
            motor_sw2 = 1;
        }

        if (command == '2') {
            motor_dir1 = 1;
            motor_dir2 = 1;
            motor_sw1 = 1;
            motor_sw2 = 1;
        }

        if (command == '4') {
            motor_dir1 = 0;
            motor_dir2 = 1;
            motor_sw1 = 1;
            motor_sw2 = 1;
        }

        if (command == '6') {
            motor_dir1 = 1;
            motor_dir2 = 0;
            motor_sw1 = 1;
            motor_sw2 = 1;
        }

	//ＰＳＤセンサ電圧表示
        if (command == '7') {
            psd_volt = psd_F * 3.3;
            pc.printf("PSD F = %5.1f\r\n",psd_volt);

            psd_volt = psd_R * 3.3;
            pc.printf("PSD R = %5.1f\r\n",psd_volt);

            psd_volt = psd_L * 3.3;
            pc.printf("PSD L = %5.1f\r\n",psd_volt);

            pc.printf("\r\n");
        }
    }
}