File content as of revision 2:e1fedc61e5e3:

#include "mbed.h"
#include "Motor.h"

//constant
#define PI  3.1415926

//Skps
Serial ps2(A0,A1,9600);//Tx,Rx(D8,D2)

//SKPS_Button_number
enum {
    //return value：digital
    p_select,
    p_joyl,
    p_joyr,
    p_start,
    p_up,
    p_right,
    p_down,
    p_left,
    p_l2,
    p_r2,
    p_l1,
    p_r1,
    p_triangle,
    p_circle,
    p_cross,
    p_square,
    //return_value：analog
    p_joy_lx,
    p_joy_ly,
    p_joy_rx,
    p_joy_ry,
    p_joy_lu,
    p_joy_ld,
    p_joy_ll,
    p_joy_lr,
    p_joy_ru,
    p_joy_rd,
    p_joy_rl,
    p_joy_rr,
    p_con_status,

    //return_value：none
    p_motor1,
    p_motor2
};

//TerminalDisplay(TeraTerm)
Serial pc(SERIAL_TX, SERIAL_RX);

//voltage
AnalogIn voltage(PC_4);

//UltraSonicSensor
DigitalIn change(NC);//pa6
Timer timer_volt;

//Motor
Motor motor(PA_12,PA_11,PA_5, PB_2,PB_1,PA_9, PB_15,PB_14,PB_13, PA_8,PB_4,PB_10);

//kicker
DigitalOut kick(NC);//pa_8

//ToggleSwitch
DigitalIn sw_start(PD_2); //program start switch
DigitalIn sw_reset(PC_12); //gyro sensor reset switch
DigitalIn sw_kick(USER_BUTTON);

//declear prototype (function list)
bool check_voltage(); //電圧をチェックする
void uart_send(unsigned char data);
unsigned char uart_rec(void);
unsigned char skps(unsigned char data);
unsigned char skps_vibrate(unsigned char motor, unsigned char value);
float convert_degree(float x,float y);
float convert_length(float x,float y);
/*****************************************************************/
/**********************main function******************************/
/*****************************************************************/

int main()
{

//**************************************************************//
////////////////////////initialize setting////////////////////////
//**************************************************************//
    wait_ms(50);
    kick = 0; //キックを解除する

    /*motor pwm frequency set*/
    motor.setPwmPeriod(0.00052);

    while (1) {
//***************************************************************//
////////////////Play mode////////////
//***************************************************************//
        timer_volt.reset();
        timer_volt.start();
        while (sw_start == 0 && check_voltage() == 1) {
            float deg,power,yaw_angle,lx,ly,rx;
            
            //convert int to float
            lx = (float)skps(p_joy_lx);
            ly = (float)skps(p_joy_ly);
            rx = (float)skps(p_joy_rx);
            deg = convert_degree(lx,ly);
            
            yaw_angle = (rx - 128)/128;
            
            power = convert_length(lx,ly)*0.7;
            if(power <= 10) { //遊びをカット
                power = 0;
            }
            
            if(skps(p_l1) == 0 || skps(p_square) == 0){
                motor.omniWheels(deg,power,yaw_angle*30);
            }
            else{
                motor.omniWheels(deg,power*0.5,yaw_angle*30);
            }
            /*kick*/
            if(skps(p_circle) == 0 || skps(p_r1)== 0){
                kick = 1;
                wait_ms(100);
                kick = 0;
            }
        }
        motor.omniWheels(0, 0, 0);
        /**********************end main function**************************/
    }
}


//////////////////////////////////////
/*Battery check voltage*/
//////////////////////////////////////
bool check_voltage()
{
    static float sum[5] = { 8, 8, 8, 8, 8 };
    static float Ave = 8;
    static bool S = 1;
    if (timer_volt.read() > 0.2) {
        for (int i = 4; i > 0; i--) {
            sum[i] = sum[i - 1];
        }
        sum[0] = voltage.read() * 9.9;
        Ave = (sum[0] + sum[1] + sum[2] + sum[3] + sum[4]) / 5;
        if (Ave < 7.0) { //7.0V以下で自動遮断
            S = 0;
        } else {
            S = 1;

        }
        timer_volt.reset();
    }
    return S;
}

void uart_send(unsigned char data)
{
    ps2.putc(data);
}

unsigned char uart_rec(void)
{
    unsigned char temp;
    while(ps2.readable() == 0);
    temp = ps2.getc();
    return temp;
}

unsigned char skps(unsigned char data)
{
    uart_send(data);
    return uart_rec();
}

unsigned char skps_vibrate(unsigned char motor, unsigned char value)
{
    uart_send(motor);
    uart_send(value);
}

/**************************************************************/
/*関数名：convert_degree(float x,float y)*/
/*引数：ジョイスティックのX,Y Axis*/
/*戻り値：float型　極座標の角度(前方が0度,時計回りに360度)*/
/*概要:ロボットの進行方向を決めるときに使う*/
/*************************************************************/
float convert_degree(float x,float y){
  float degree;
  degree = (180/PI)*atan2(y - 128,x - 128) + 360 + 90;//オイラー角度に変換
  
  //角度を0~360度に変換
  if(degree >= 360.0){
    degree = degree - 360;
  }
  return degree;
}

/***************************************************************/
/*関数名:convert_length(float x,flaot y)*/
/*引数:ジョイスティックのX,Y*/
/*戻り値：float型 極座標の原点から(x,y)点までの長さを0~100で返す*/
/*概要:モーターのパワーを決めるときに,極座標の長さを使う*/
/***************************************************************/
float convert_length(float x,float y){
  float xy_length;
  xy_length = pow(x - 128,2) + pow(y - 128,2);
  xy_length = (100 * xy_length)/(128*128);
  if(xy_length >= 100.0){
    xy_length = 100.0;
  }
  return xy_length;
}