File content as of revision 7:0bc2bc07f2fe:

#include "mbed.h"
#include <vector>
//  Buffer Serial
#include "BufferSerial.h"

//  Left Motor
#define PWM_L A6
#define DIR_L A3
//  Right Motor
#define PWM_R A2
#define DIR_R D12
//  default setting
#define DIR_DEFAULT_L 1
#define DIR_DEFAULT_R 0

//  Math Function
#define PI 3.14159265359
//  power clock
#define POWER_CHANGE_PER_CLOCK 5

//  connect timeout LED
#define CONNECT_TIMEOUT_LED D2

//  PWM
PwmOut pwmL(PWM_L);
//  DIR
DigitalOut dirL(DIR_L);
//  PWM
PwmOut pwmR(PWM_R);
//  DIR
DigitalOut dirR(DIR_R);

//  serial
//Serial serial(D5,D4);
//  bufferSerial
BufferSerial _buffer_serial(D5,D4,115200,2000);

//  time out led
DigitalOut connectTimeoutLED(CONNECT_TIMEOUT_LED);

//  joystick analogin
//AnalogIn joyX(A0);
//AnalogIn joyY(A1);
//  stick value
float x = 0.5F;
float y = 0.5F;


//  current power
double currentPowerL = 0;
double currentPowerR = 0;

//  attach function
void control_rx();
//  time out
Timer connectTimer;
int connectLimit = 2000;

void load_data();

int main()
{
    //  period
    pwmL.period_us(100);
    pwmR.period_us(100);
    //  Dir
    dirL = DIR_DEFAULT_L;
    dirR = DIR_DEFAULT_R;
    //  power setting
    float maxPower = 0.95F;

    /*
    //  serial attach
    serial.baud(115200);
    serial.attach(control_rx,Serial::RxIrq);
    */
    //  connect timer
    connectTimer.reset();
    connectTimer.start();

    while(1) {
        //  prepare power value
        float powerL,powerR;

        //  loat data
        load_data();

        //  last connect checker
        if(connectTimer.read_ms() > connectLimit) {
            x = 0.5;
            y = 0.5;
            //  LED
            connectTimeoutLED = !connectTimeoutLED;
            wait_ms(50);
        }
        //  value format
        if(x > 1) x = 1.0;
        if(x < 0) x = 0.0;
        if(y > 1) y = 1.0;
        if(y < 0) y = 0.0;

        //  format x and y
        double formatX = (2*x) - 1;
        double formatY = (2*y) - 1;
        //  tan
        double tan = formatY / formatX;
        //  arc tan
        double arctan = (double) atan(tan);
        //  angle
        double angle = arctan * (180 / PI);
        //  range
        double range = sqrt((formatX * formatX) + (formatY * formatY));
        if(range > 1.0F) range = 1.0F;
        //  all power
        double allPower = maxPower * (range / 1);

        //  right left power persent
        double rightPowerPercent,leftPowerPercent;
        int area = 0;
        //1
        if((0 < formatX && formatX < 1) && (0 < formatY && formatY < 1)) {
            rightPowerPercent = ((angle / 45) - 1) / 1;
            leftPowerPercent = 1;

            area = 1;
        }
        //2
        if((formatX < 0 && -1 < formatX) && (0 < formatY && formatY < 1)) {
            rightPowerPercent = 1;
            leftPowerPercent = ((-angle / 45) - 1) / 1;

            area = 2;
        }
        //3
        if((formatX < 0 && -1 < formatX) && (formatY < 0 && -1 < formatY)) {
            rightPowerPercent = ((-angle / 45) + 1) / 1;
            leftPowerPercent = -1;

            area = 3;
        }
        //4
        if((formatX > 0 && formatX < 1) && (formatY < 0 && formatY > -1)) {
            rightPowerPercent = -1;
            leftPowerPercent = ((angle / 45) + 1) / 1;

            area = 4;
        }
        if(area == 0) {
            rightPowerPercent = 0;
            leftPowerPercent = 0;
        }

        //  convert power persent to real power
        powerL = allPower * leftPowerPercent;
        powerR = allPower * rightPowerPercent;

        //  need to change power(power distance)
        double needChangePowerL = powerL - currentPowerL;
        double needChangePowerR = powerR - currentPowerR;
        //  change power
        double changePowerL = needChangePowerL / POWER_CHANGE_PER_CLOCK;
        double changePowerR = needChangePowerR / POWER_CHANGE_PER_CLOCK;
        //  set power
        powerL = currentPowerL + changePowerL;
        powerR = currentPowerR + changePowerR;
        //  save current power
        currentPowerL = powerL;
        currentPowerR = powerR;

        //printf("before:%lf %lf distance:%lf %lf change:%lf %lf after:%lf %lf \n\r",powerL,powerR,needChangePowerL,needChangePowerR,changePowerL,changePowerR,powerL,powerR);

        if(powerL >= 0) {
            dirL = DIR_DEFAULT_L;
        } else {
            powerL = -powerL;
            dirL = !DIR_DEFAULT_L;
        }
        if(powerR >= 0) {
            dirR = DIR_DEFAULT_R;
        } else {
            powerR = -powerR;
            dirR = !DIR_DEFAULT_R;
        }

        pwmL.write((float)powerL);
        pwmR.write((float)powerR);

        wait_ms(50);
        //printf("powerL:%f powerR:%f formatX:%0lf formatY:%0lf angle:%d range:%0lf area: %d Lper:%0lf Rper:%0lf \n\r",powerL,powerR,formatX,formatY,(int) angle,range,area,leftPowerPercent,rightPowerPercent);
    }
}

void load_data()
{
    int size = 32;
    char bytes[size];
    size_t index = _buffer_serial.readBytesUntil('\n',(char*)bytes,size);
    int startIndex = index - 5;
    
    if(size - 5 < startIndex){
        printf("index error\n\r");
    }
    if(bytes[startIndex] == 0x3A) {
        //  cast float to double
        uint16_t uintX = ((bytes[startIndex + 1] << 8 | bytes[startIndex + 2]) & 0xffff);
        uint16_t uintY = ((bytes[startIndex + 3] << 8 | bytes[startIndex + 4]) & 0xffff);
        double formatX = (double)uintX / 65536;
        double formatY = (double)uintY / 65536;
        //  save
        x = formatX;
        y = formatY;
        
        printf("%lf %lf\n\r",formatX,formatY);
        //  reset connect timer
        connectTimer.reset();

    } else {
        printf("error\n\r");
    }
}