#include "mbed.h"

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

DigitalOut motor_sw1(p5);
DigitalOut motor_dir1(p6);
DigitalOut motor_sw2(p7);
DigitalOut motor_dir2(p8);
DigitalOut power_sw(p30);

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

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;
//unsigned short psd_volt;
    float psd_volt, servo_newtral, servo_unit;


    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;

    servo_pwm1.period_ms(20);
    servo_newtral = 1.0 / 20.0;
    servo_unit = servo_newtral / 1024.0;
    servo_pwm1 = servo_newtral;

    //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++;
//            printf("W_R = %5d\r\n", count_R);
        }
        if (W_L !=  wheel_L) {
            W_L = wheel_L;
            count_L++;
//            printf("W_L = %5d\r\n", count_L);
        }

        if (command == '5' || count_R >= 24|| count_L >= 24) {
            motor_sw1 = 0;
            motor_sw2 = 0;

            count_R = 0;
            count_L = 0;

            servo_pwm1 = servo_newtral + servo_unit * 0.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;
            
            servo_pwm1 = servo_pwm1 + servo_unit;
        }

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

            servo_pwm1 = servo_pwm1 - servo_unit;
        }

        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");
        }
    }


}