File content as of revision 5:8a0663298e2c:

#include "mbed.h"
#include "Motor.h"
#include "MCP.h"
#include "XBee.h"
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#define SDA PB_7 
#define SCL PB_6
#define MCP_ADDRESS 0x40

MCP MCP(SDA, SCL, MCP_ADDRESS);
XBEE::ControllerData *controller;
MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];

/* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
    
#define omni 0
#define mekanamuR 1
#define mekanamuL 2
    
long map(long x, long in_min, long in_max, long out_min, long out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void outputMecani(double right_value, double left_value, double omni_value);
// メカニ平行移動数
void driveMecani(double power, double degree) {
  //powerには、 出力の大きさを0～255で入れてください。255で最大です。
  //degreeには、進みたい角度を0～360で入れてください。0で右進です。

  double right_value;//右メカナムの値
  double left_value;//左メカナムの値
  double omni_value;//オムニの値
  double pa;//前後の速度
  double pb;//左右の速度

  pa = -(sin((degree)) * fabs(sin((degree))) * power);
  pb = cos((degree)) * fabs(cos((degree))) * power;
  right_value = -(pa + pb);
  left_value = -(pa - pb);
  omni_value = -pb * 0.75;
  //if (left_value < 0) {
  //  left_value = left_value * 0.95;
  // } else {
  right_value = right_value * 0.95;
  //  }
  outputMecani(right_value, left_value, omni_value);

}

// メカニ旋回関数
void turnMecani(double power, double center) {
  //powerには、 出力の大きさを-255～255で入れてください。255で最大です。
  //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。

  double right_value; //右メカナムの値
  double left_value;  //左メカナムの値
  double omni_value;  //オムニの値

  if (center <= 0) {
    omni_value = - power * fabs(center);
    right_value = power;
    left_value = -power;
  } else {
    omni_value = -power;
    right_value = power * (center);
    left_value = -power * (center);
  }

  outputMecani(right_value, left_value, omni_value);
}

// メカニピン出力関数Arduino用
void outputMecani(double right_value, double left_value, double omni_value) {

  if (right_value < 0) {
    motor[mekanamuR].dir=BACK;
  } else {
    motor[mekanamuR].dir=FOR;
  }

  if (left_value < 0) {
    motor[mekanamuL].dir=FOR;
  } else {
    motor[mekanamuL].dir=BACK;
  }

  if (omni_value > 0) {
    motor[omni].dir=BACK;
  } else {
    motor[omni].dir=FOR;
  }
    motor[mekanamuR].pwm=fabs(right_value);
    motor[mekanamuL].pwm=fabs(left_value);
    motor[omni].pwm=fabs(omni_value);
 //横移動時に曲がってしまう場合は調整してください。

}

/* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */

int main() {
    
    XBEE::Controller::Initialize();
    MOTOR::Motor::Initialize();
    
     __enable_irq();
    
    /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
    
    
    
    /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
    
    while(1) {
        
        controller = XBEE::Controller::GetData();
        MCP.Update();
        
        /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
        
        int A=controller->AnalogL.X-7;
        int B=controller->AnalogL.Y-7;
        float shita=atan(double(B)/double(A));
        double power=sqrt(double(A)*double(A)+double(B)*double(B));
        power=map(power,0,9.90,0,255);
        
        if(A<0&&B<0){
            shita=shita+3.141592f;
            }else if(A<0&&B>0){
            shita=shita+3.141592/2;
            }
        if(A==0){
            if(B>0){
                shita=3.141592;
                }else if(B<0){shita=-3.141592;
                }else if(B==0){shita=0;
                }
                }
            
        
        /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
        
        MOTOR::Motor::Update(motor);
    }
}