mecanum2017.cpp

00001 #include "mbed.h"
00002 #include "T_motor.h"
00003 #include "mecanum2017.h"
00004 
00005 mecanum2017::mecanum2017(I2C* i2c_, float para[], char addr[], int phase) //paraはモーターの回転方向を±1で表すよ いつもは全部+1だよ
00006     : m1(i2c_, addr[0]), m2(i2c_, addr[1]), m3(i2c_, addr[2]), m4(i2c_, addr[3])
00007 {
00008     para1 = para[0];
00009     para2 = para[1];
00010     para3 = para[2];
00011     para4 = para[3];
00012     oldangle = 0.0;
00013     alpha = PI / 4;
00014     beta = phase * (PI / 2); //phaseは初期角度の指定 左に90°で-1, 右に90°で+1
00015     sokudo = 0.28;
00016 }
00017 
00018 /* うごけー */
00019 void mecanum2017::move(int xdate, int ydate) //xdate, ydateは -64 ~ +64
00020 {
00021     //if(xdate == 0 && ydate == 0) free();
00022     kasoku();
00023     double angle = atan2(double(ydate), double(xdate)) + beta; //入力の角度
00024     if(angle == oldangle); //前回と角度が同じだったらそのまま
00025     else
00026     {
00027         m1 = float(para1 * sokudo * sin(angle - alpha));
00028         m2 = float(para2 * sokudo * sin(angle + alpha));
00029         m3 = float(para3 * sokudo * sin(angle - alpha));
00030         m4 = float(para4 * sokudo * sin(angle + alpha));
00031     }
00032     oldangle = angle; // 今回の角度を記憶
00033 }
00034 
00035 /*かくどー*/
00036 void mecanum2017::anglemove(double angle)
00037 {
00038     if(angle != 10)//10 == NONE_angle
00039     {
00040         kasoku();
00041         angle += beta;
00042         if(angle == oldangle); //前回と角度が同じだったらそのまま
00043         else
00044         {
00045             m1 = float(para1 * sokudo * sin(angle - alpha));
00046             m2 = float(para2 * sokudo * sin(angle + alpha));
00047             m3 = float(para3 * sokudo * sin(angle - alpha));
00048             m4 = float(para4 * sokudo * sin(angle + alpha));
00049         }
00050         oldangle = angle; // 今回の角度を記憶
00051     }
00052     else free();
00053 }
00054 
00055 /* まわれー */
00056 void mecanum2017::rotation(int rxdate)
00057 {
00058     kasoku();
00059     m1 = para1 * sokudo * -rxdate * 0.015625; // 1 / 64 = 0.015625
00060     m2 = para2 * sokudo *  rxdate * 0.015625;
00061     m3 = para3 * sokudo *  rxdate * 0.015625;
00062     m4 = para4 * sokudo * -rxdate * 0.015625;
00063 }
00064 
00065 /* とまれー */
00066 void mecanum2017::stop()
00067 {
00068     m1.stop();
00069     m2.stop();
00070     m3.stop();
00071     m4.stop();
00072     oldangle = 10;
00073     timer.reset();
00074 }
00075 
00076 /*そのままー*/
00077 void mecanum2017::free()
00078 {
00079     m1.free();
00080     m2.free();
00081     m3.free();
00082     m4.free();
00083     oldangle = 10;
00084     timer.reset();
00085 }
00086 
00087 void mecanum2017::kasoku()
00088 {
00089     int time = timer.read_ms();
00090     if(time < 1000)
00091     {
00092         if(time == 0)
00093         {
00094             timer.start();
00095             time = 10;
00096         }
00097         if(time >= 1000) timer.stop();
00098         sokudo = time / 1000.0f; //ms * 1000 / 10;
00099         oldangle = 10;
00100     }
00101 }