File content as of revision 6:cd2671c141cc:

#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];
Serial pc(USBTX,USBRX);
/* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */

#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;
    //if (left_value < 0) {
    //  left_value = left_value * 0.95;
    // } else {
    left_value = left_value * 0.85;
    right_value = right_value * 0.85;
    //  }
    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;
    }
    
    double R=fabs(right_value);
    double L=fabs(left_value);
    double om=fabs(omni_value);
    R=map(R,0,255,0,100);
    L=map(L,0,255,0,100);
    om=map(om,0,255,0,100);
    motor[mekanamuR].pwm=R;
    motor[mekanamuL].pwm=L;
    motor[omni].pwm=om;

//横移動時に曲がってしまう場合は調整してください。

}

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

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;
        double shita=atan(double(B)/double(A));
        double power=sqrt(double(A)*double(A)+double(B)*double(B));
        power=map(power,0,7.0,0,255);
        pc.printf("\r\n");

        if(A<0) {
            if(B>0) {
                shita=3.141592-atan(fabs(double(B/A)));
            } else if(B<0) {
                shita=-3.141592+atan(fabs(double(B/A)));
            } else if(B==0) {
                shita=3.141592;
            }
        }


        driveMecani(power,shita);
        
        if(A==0&&B==0) {
            motor[omni].dir=BRAKE;
            motor[omni].pwm=100;
            motor[mekanamuR].dir=BRAKE;
            motor[mekanamuR].pwm=100;
            motor[mekanamuL].dir=BRAKE;
            motor[mekanamuL].pwm=100;
            int turn=controller->AnalogR.X;
            turn=map(turn,0,14,-255,255);
            turnMecani(turn,1);
        } else if(A==0&&B>0) {
            motor[omni].dir=BRAKE;
            motor[omni].pwm=100;
            motor[mekanamuR].dir=FOR;
            motor[mekanamuR].pwm=power;
            motor[mekanamuL].dir=BACK;
            motor[mekanamuL].pwm=power;
        } else if(A==0&&B<0) {
            motor[omni].dir=BRAKE;
            motor[omni].pwm=100;
            motor[mekanamuR].dir=BACK;
            motor[mekanamuR].pwm=power;
            motor[mekanamuL].dir=FOR;
            motor[mekanamuL].pwm=power;
        }
        pc.printf("right=%lf,left=%lf,omni=%lf",motor[mekanamuR].pwm,motor[mekanamuL].pwm,motor[omni].pwm);



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

        MOTOR::Motor::Update(motor);
    }
}