main.cpp

00001 #include "mbed.h"
00002 #include "Motor.h"
00003 #include "MCP.h"
00004 #include "XBee.h"
00005 #include <stdint.h>
00006 #include <stdio.h>
00007 #include <math.h>
00008 #define SDA PB_7
00009 #define SCL PB_6
00010 #define MCP_ADDRESS 0x40
00011 
00012 MCP MCP(SDA, SCL, MCP_ADDRESS);
00013 XBEE::ControllerData *controller;
00014 MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00015 Serial pc(USBTX,USBRX);
00016 /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
00017 
00018 #define omni 0
00019 #define mekanamuR 1
00020 #define mekanamuL 2
00021 
00022 long map(long x, long in_min, long in_max, long out_min, long out_max)
00023 {
00024     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00025 }
00026 void outputMecani(double right_value, double left_value, double omni_value);
00027 // メカニ平行移動数
00028 void driveMecani(double power, double degree)
00029 {
00030     //powerには、 出力の大きさを0～255で入れてください。255で最大です。
00031     //degreeには、進みたい角度を0～360で入れてください。0で右進です。
00032 
00033     double right_value;//右メカナムの値
00034     double left_value;//左メカナムの値
00035     double omni_value;//オムニの値
00036     double pa;//前後の速度
00037     double pb;//左右の速度
00038 
00039     pa = -(sin((degree)) * fabs(sin((degree))) * power);
00040     pb = cos((degree)) * fabs(cos((degree))) * power;
00041     right_value = -(pa + pb);
00042     left_value = -(pa - pb);
00043     omni_value = -pb;
00044     //if (left_value < 0) {
00045     //  left_value = left_value * 0.95;
00046     // } else {
00047     left_value = left_value * 0.85;
00048     right_value = right_value * 0.85;
00049     //  }
00050     outputMecani(right_value, left_value, omni_value);
00051 
00052 }
00053 
00054 // メカニ旋回関数
00055 void turnMecani(double power, double center)
00056 {
00057     //powerには、 出力の大きさを-255～255で入れてください。255で最大です。
00058     //centerには、回転の中心の位置を-255~255で入れてください。中心の位置は、値が大きいとメカナム寄り、小さいとオムニ寄りになります。
00059 
00060     double right_value; //右メカナムの値
00061     double left_value;  //左メカナムの値
00062     double omni_value;  //オムニの値
00063 
00064     if (center <= 0) {
00065         omni_value = - power * fabs(center);
00066         right_value = power;
00067         left_value = -power;
00068     } else {
00069         omni_value = -power;
00070         right_value = power * (center);
00071         left_value = -power * (center);
00072     }
00073 
00074     outputMecani(right_value, left_value, omni_value);
00075 }
00076 
00077 // メカニピン出力関数Arduino用
00078 void outputMecani(double right_value, double left_value, double omni_value)
00079 {
00080 
00081     if (right_value < 0) {
00082         motor[mekanamuR].dir=BACK;
00083     } else {
00084         motor[mekanamuR].dir=FOR;
00085     }
00086 
00087     if (left_value < 0) {
00088         motor[mekanamuL].dir=FOR;
00089     } else {
00090         motor[mekanamuL].dir=BACK;
00091     }
00092 
00093     if (omni_value > 0) {
00094         motor[omni].dir=BACK;
00095     } else {
00096         motor[omni].dir=FOR;
00097     }
00098     
00099     double R=fabs(right_value);
00100     double L=fabs(left_value);
00101     double om=fabs(omni_value);
00102     R=map(R,0,255,0,100);
00103     L=map(L,0,255,0,100);
00104     om=map(om,0,255,0,100);
00105     motor[mekanamuR].pwm=R;
00106     motor[mekanamuL].pwm=L;
00107     motor[omni].pwm=om;
00108 
00109 //横移動時に曲がってしまう場合は調整してください。
00110 
00111 }
00112 
00113 /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
00114 
00115 int main()
00116 {
00117 
00118     XBEE::Controller::Initialize();
00119     MOTOR::Motor::Initialize();
00120 
00121     __enable_irq();
00122 
00123     /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
00124 
00125 
00126 
00127     /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
00128 
00129     while(1) {
00130 
00131         controller = XBEE::Controller::GetData();
00132         MCP.Update();
00133 
00134         /* ↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓ */
00135 
00136         int A=controller->AnalogL.X-7;
00137         int B=controller->AnalogL.Y-7;
00138         double shita=atan(double(B)/double(A));
00139         double power=sqrt(double(A)*double(A)+double(B)*double(B));
00140         power=map(power,0,7.0,0,255);
00141         pc.printf("\r\n");
00142 
00143         if(A<0) {
00144             if(B>0) {
00145                 shita=3.141592-atan(fabs(double(B/A)));
00146             } else if(B<0) {
00147                 shita=-3.141592+atan(fabs(double(B/A)));
00148             } else if(B==0) {
00149                 shita=3.141592;
00150             }
00151         }
00152 
00153 
00154         driveMecani(power,shita);
00155         
00156         if(A==0&&B==0) {
00157             motor[omni].dir=BRAKE;
00158             motor[omni].pwm=100;
00159             motor[mekanamuR].dir=BRAKE;
00160             motor[mekanamuR].pwm=100;
00161             motor[mekanamuL].dir=BRAKE;
00162             motor[mekanamuL].pwm=100;
00163             int turn=controller->AnalogR.X;
00164             turn=map(turn,0,14,-255,255);
00165             turnMecani(turn,1);
00166         } else if(A==0&&B>0) {
00167             motor[omni].dir=BRAKE;
00168             motor[omni].pwm=100;
00169             motor[mekanamuR].dir=FOR;
00170             motor[mekanamuR].pwm=power;
00171             motor[mekanamuL].dir=BACK;
00172             motor[mekanamuL].pwm=power;
00173         } else if(A==0&&B<0) {
00174             motor[omni].dir=BRAKE;
00175             motor[omni].pwm=100;
00176             motor[mekanamuR].dir=BACK;
00177             motor[mekanamuR].pwm=power;
00178             motor[mekanamuL].dir=FOR;
00179             motor[mekanamuL].pwm=power;
00180         }
00181         pc.printf("right=%lf,left=%lf,omni=%lf",motor[mekanamuR].pwm,motor[mekanamuL].pwm,motor[omni].pwm);
00182 
00183 
00184 
00185         /* ↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑ */
00186 
00187         MOTOR::Motor::Update(motor);
00188     }
00189 }